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

Algorithm 5

pf_result = PfUwb(UwbData).

Input: UwbData: UWB range data;
Output: pf_result: the positioning result obtained based on the particle filter algorithm
(1)	particle_num = 300; init_position = [0.0,0.0]; sigma1 = 0.5; sigma2 = 0.5; P_state = [particle_num,2]; score = [particle_num]; weight = [particle_num]; pf_result = [UwbData.rows(), 2]; beacon_set = [Coordinates of n beacons]
(2)	P_state = add_noise(init_position, sigma1)
(3)	For j in UwbData.rows()
(4)	P_state += add_noise(P_state, sigma1)
(5)	score = Evaluation(UwbData[j], P_state, sigma2, beacon_set)
(6)	weight∗ = score
(7)	P_state, weight = Resample(P_state, weight)
(8)	pf_result[j] = get_result(P_state, weight)
(9)	End
(10)	Return pf_result