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

Algorithm 4

Value = CostFunction(Pose, range_list, beacon_set).

Input: Pose: current pose; range_list: list of ranges from the four beacons to the tag; beacon_set: coordinates of four beacons
Output: Value: the calculation result based on the cost function
(1)	val = 0.0
(2)	For i in beaconset.rows()
(3)	val += abs(norm(beaconset[i,:] − pose) − range_list[i])
(4)	End
(5)	Return val