| Lines 10–15 indicate that if IMU data is read first, then calculate every particle based on the foot-mounted IMU through the EKF. In Line 12, the add_Noise function represents the pose estimation after the Gaussian noise is added to the acceleration and gyrodata each time. |
| Lines 16–27 indicate that when UWB data is received, pose data will be updated based on the new observations. Lines 16–19 show the calculation on the weight of the current particle according to the newly acquired range value. Lines 20–23 indicate the updating of the pose array based on the new weight, and Line 24 records the UWB and foot-mounted IMU fused positioning results into the FusingResult array. Line 25 indicates the resampling of particle weight to update the particle state according to the value of resampling. |
| Input: UwbData: UWB range data; ImuData: IMU data; ZuptData: the zero velocity state of IMU data at a specific moment |
| Output: pf_ekf_result: the positioning result with the integration of Uwb and Imu |
| (1) | imu_index = 0; uwb_index = 0; particle_num = 300; init_postion = [0, 0]; init_heading = 0; sigma1 = 0.5; sigma2 = 0.5; pose = [0, 0]; |
| (2) | weight = [particle_num]; score = [particle_num]; Ekf_List = [particle_num]; beacon_set = [the coordinates of n beacons]; |
| (3) | For i in particle_num |
| (4) | Ekf_List[i] = init_Nav_Eq(ImuData[1:20], init_postion, init_heading) |
| (5) | End |
| (6) | while true |
| (7) | If imu_index == ImuData.rows() || uwb_index == UwbData.rows() |
| (8) | break; |
| (9) | End |
| (10) | If time(UwbData(uwb_index)) > time(ImuData(imu_index)) |
| (11) | For i in particle_num |
| (12) | pose[i] = Ekf_List[i].get_Position(add_Noise(ImuData, ZuptData, sigma1)) |
| (13) | End |
| (14) | imu_index++ |
| (15) | Else |
| (16) | For i in particle_num |
| (17) | score[i] = Ekf_List[i].Evaluation(beaconset, UwbData(uwb_index), sigma2) |
| (18) | weight[i]∗ = score[i] |
| (19) | End |
| (20) | pose_fusing = [0,0,0]; weight[i] = weight[i]/Sum(weight[i]) |
| (21) | For i in particle_num |
| (22) | pose_fusing += pose[i] ∗ weight[i] |
| (23) | End |
| (24) | FusingResult[uwb_index] = pose_fusing |
| (25) | Ekf_List, weight = Resample(Ekf_List, weight) |
| (26) | uwb_index++; |
| (27) | End |
| (28) | End |
| (29) | Return FusingResult |
