Research Article
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 |
|