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

Algorithm 2

zupt_result = ZuptImu(ImuData).

Input: ImuData: the original imu value
Output: zupt_result: foot-mounted IMU-based positioning result
(1)	Acquire the acceleration acc_s and the gyroscope data from ImuData to initialize yaw, pitch, and roll
(2)	Construct a coordinate transformation matrix C _pre and indicate the zero bias of with
(3)	Initialize the acceleration, the velocity, and the position vector that are separately indicated by acc_n, vel_n, and pos_n in the navigation coordinate
(4)	Initialize the acceleration noise = 0.01 m/s, the gyro noise = 0.01 rad/s, and the observation noise = 0.01 m/s
(5)	Construct an observation matrix H and an observation noise matrix R, and initialize the zero velocity detection threshold _threshold = 0.6 rad/s
(6)	For t in ImuData.cols()
(7)	dt = timestamp(t) − timestamp(t − 1)
(8)	= (:,t) −
(9)	Construct an angular velocity skew-symmetric matrix and update the coordinate transformation matrix C = C _pre ∗ (2I3×3 +  dt) (2I3×3 −  dt)^−1
(10)	acc_n(:,t) = 0.5 ∗ (C + C _prev) ∗ acc_s(:,t)
(11)	vel_n(:,t) = vel_n(:,t − 1) + ((acc_n(:,t) − [0; 0; ]) + (acc_n(:,t − 1) − [0; 0; ])) ∗ dt/2
(12)	pos_n(:,t) = pos_n(:,t − 1) + (vel_n(:,t) + vel_n(:,t − 1)) ∗ dt/2
(13)	Calculate the state transfer matrix F and the system error covariance matrix Q
(14)	P = F ∗ P ∗ F′ + Q;
(15)	If norm((:,t)) < _threshold
(16)	K = (P ∗ (H)′)/((H) ∗ P ∗ (H)′ + R)
(17)	delta_x = K ∗ vel_n(:,t)
(18)	P = (I9×9 − K ∗ H) ∗ P
(19)	attitude_error = delta_x(1:3); pos_error = delta_x(4:6); vel_error = delta_x(7:9)
(20)	Construct an angular error skew-symmetric matrix Se
(21)	C = (2 ∗ I3×3 + Se)/(2 ∗ I3×3 − Se ∗ C)
(22)	vel_n(:,t) = vel_n(:,t) − vel_error; pos_n(:,t) = pos_n(:,t) − pos_error
(23)	End
(24)	heading(t) = a tan 2(C(2,1), C(1,1)); C _prev = C
(25)	End
(26)	Return pos_n