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

Algorithm 3

uwb_opt_result = OptUwb(UwbData).

Input: UwbData: the original UWB value;
Output: uwb_opt_result: the positioning result obtained through the UWB-based optimization algorithm
(1)	OptResult = [], initial_pose = [0.0, 0.0]
(2)	For i in UwbData.rows()
(3)	range_list = UwbData[i,:]
(4)	res = minimize(CostFunction, initial_pose, range_list, bounds = ((−40, 40), (−40, 40)))
(5)	initial_pose = res
(6)	OptResult[i,:] = res
(7)	End
(8)	Return OptResult