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

Algorithm 6

Score = Evaluation(UwbData[j], P_state, sigma2, beacon_set).

Input: UwbData: UWB range data; P_state: particle state array; sigma2: the deviation value for particle evaluation; beacon_set: the coordinates of four beacons.
Output: score: the score of every particle after the evaluation
(1)	For i in particle_num
(2)	score[i] = 1.0
(3)	For k in beacon_set.rows()
(4)	score[i]∗ = normPdf(range_list[i], norm(beacon_set[i,:] − pose), sigma2)
(5)	End
(6)	End
(7)	Return score