Research on Extended Kalman Filter and Particle Filter Combinational Algorithm in UWB and Foot-Mounted IMU Fusion Positioning

Algorithm 7

pf_imu_uwb_result = PfImuUwb(UwbData, ImuPath).

Input: UwbData: UWB range data; ImuPath: path calculated based on the ZuptImu algorithm with the fragmentation stored based on the timestamp of the UWB data.
Output: pf_imu_uwb_result: Uwb and Imu fusion positioning result
(1)	particle_num = 300; init_position = [0, 0]; sigma1 = 0.5; sigma2 = 0.5; P_state = [particle_num, 2]; score = [particle_num]
(2)	weight = [particle_num]; FusingResult = [UwbData.rows(),2]; beaconset = [the coordinates of n beacons]
(3)	init_position = trianglepose(BeaconSet, UwbData[1:5,])
(4)	P_state = add_noise(init_position, sigma1)
(5)	For j in UwbData.rows()
(6)	delta_pose = ImuPath[j,:] − ImuPath[j − 1,:]
(7)	P_state = add_odo_noise(P_state, delta_pose, sigma1)
(8)	score = Evaluation(UwbData[j], P_state, sigma2)
(9)	weight∗ = score
(10)	P_state, weight = Resample(P_state, weight)
(11)	FusingResult[j] = get_result(P_state, weight)
(12)	End
(13)	Return FusingResult