State Transition Function of IMU
The IMU (Inertial Measurement Unit) sensor can measure the acceleration and angular velocity by using an accelerometer and a gyroscope. The vehicle localization algorithm based on IMU is to calculate the state information of the vehicle relative to the initial moment by integrating the measurement values of the IMU. To facilitate description, a simple definition of the coordinate system is required. We have chosen a right-angle coordinate system whose relative position to the Earth remains constant and whose orientation remains unchanged as the world frame, with the gravitational acceleration pointing in the negative z-axis direction. Usually, a coordinate system fixed to the IMU inertial sensor needs to be defined, which is generally referred to as the sensor coordinate system. Additionally, in order to combine the kinematic constraint information of the vehicle with the Kalman filtering algorithm, a carrier coordinate system needs to be defined. The acceleration signals and angular velocity signals read by the IMU inertial sensor are all obtained in the sensor coordinate system.
In the schematic \( \boldsymbol{ (R_n^{IMU}, p_n^{IMU}) } \) represents the mapping from the IMU frame to the world frame, \( \boldsymbol{ (R_n^C, p_n^C) } \) represents the mapping from the IMU frame to the vehicle frame, and \( \boldsymbol{ v_n^{IMU}, v_n^C } \) respectively represents the velocity of the vehicle in the world frame and the velocity in the vehicle frame.
The standard EKF algorithm often encounters problems of positive feedback and inconsistency when estimating the state of the vehicle. To address these issues, we use a variant of EKF – the Invariant Extended Kalman Filter (IEKF) to estimate the state of the vehicle. In most practical applications, the tracking and localization effect of the IEKF algorithm is superior to that of EKF, and the IEKF algorithm has been widely applied in commercial and industrial fields 1 2.
In the IMU vehicle localization algorithm based on IEKF, if the bias information of acceleration and angular velocity is not considered 3, it will have a significant impact on the localization effect of the vehicle, resulting in large errors. Therefore, in order to reduce the influence of the bias of the measured acceleration and angular velocity on the localization effect of the vehicle, we incorporate it into the state variables for estimation. Therefore, the system state variables of the IEKF localization algorithm at time \(n\) are defined as: \[ \boldsymbol{ X_n = \left(R_n^{IMU}, v_n^{IMU}, p_n^{IMU}, b_n^w, b_n^a, R_n^C, p_n^C\right) } \]
In the formula, \( \boldsymbol{R_n^{IMU} \in SO(3)} \) is a \(3×3\) rotation matrix, representing the direction of the IMU at the current moment, and also representing the mapping from the IMU frame to the world frame. \( \boldsymbol{ v_n^{IMU} \in \mathbb{R}^3 } \) represents the velocity of the IMU at the current moment, \( \boldsymbol{p_n^{IMU} \in \mathbb{R}^3} \) represents the position of the IMU at the current moment, \( \boldsymbol{b_n^w \in \mathbb{R}^3} \) represents the bias of the angular velocity, \( \boldsymbol{b_n^a \in \mathbb{R}^3} \) represents the bias of the acceleration, \( \boldsymbol{R_n^C \in SO(3)} \) represents the direction of the vehicle frame, and \( \boldsymbol{p_n^C \in \mathbb{R}^3} \) represents the origin position of the vehicle frame. We choose the Euler angle method to represent the attitude information, which is more convenient to express, easier to understand.
The state update of the IMU (Inertial Measurement Unit) sensor relies on its measurement value \( \boldsymbol{ u_n = \begin{bmatrix} a_n^\top & w_n^\top \end{bmatrix}^\top \in \mathbb{R}^6 } \), where \( \boldsymbol{ a_n = \begin{bmatrix} a_{xn}, & a_{yn}, & a_{zn} \end{bmatrix}^\top \in \mathbb{R}^3 } \) and \( \boldsymbol{ w_n = \begin{bmatrix} w_{xn}, & w_{yn}, & w_{zn} \end{bmatrix}^\top \in \mathbb{R}^3 } \) represent the acceleration and angular velocity of the three axes at time \( n \), and \(dn\) represents the time interval from time \(n−1\) to time \(n\). The process noise of the system follows a normal distribution with a mean of \(0\) and a covariance of \(Qn\). Therefore, we define the state transition equation of the system as: \[ \boldsymbol{ x_n = f(x_{n-1}, u_n) } \]
In fact, the system updates based on the following formula: \[ \boldsymbol{ R_{n+1}^{IMU} = R_n^{IMU} \exp \left( \left( w_n d_n \right)_\times \right) } \]
The ()× in the formula represents an antisymmetric matrix, and the operation is as follows: \[ \boldsymbol{ \begin{bmatrix} w_1 , w_2 , w_3 \end{bmatrix}^\top = \begin{bmatrix} 0 & -w_3 & w_2 \\ w_3 & 0 & -w_1 \\ -w_2 & w_1 & 0 \end{bmatrix} } \]
Here, “exp()” represents the mapping of the corresponding Lie algebra 𝑠𝑜(3) → 𝑆𝑂(3). We can calculate it in the following way: \[ \boldsymbol{\theta = \lvert w_n d_n \rvert} \] \[ \boldsymbol{a = \frac{w_n d_n}{\lvert w_n d_n \rvert}} \] \[ \boldsymbol{ \exp(\theta a_\times) = \cos\theta \, I + (1 – \cos\theta)aa^\top + \sin\theta \, a_\times } \]
After the update, we obtained the rotation information from the sensor frame to the world frame. Next, we also need to update the velocity and position information: \[ \boldsymbol{ v_{n+1}^{IMU} = v_n^{IMU} + \left( R_n^{IMU} a_n + g \right) d_n } \] \[ \boldsymbol{ p_{n+1}^{IMU} = p_n^{IMU} + v_n^{IMU} d_n } \]
In the formula, \(g\) represents the gravitational acceleration.
The measurement information equation formula of the IMU (Inertial Measurement Unit) sensor is: \[ \boldsymbol{ w_n^{IMU} = w_n + b_n^w + w_n^w } \] \[ \boldsymbol{ a_n^{IMU} = a_n + b_n^a + w_n^a } \]
In the formula, \(\boldsymbol{w_n^{IMU}, a_n^{IMU}} \) represent the angular velocity information and acceleration information measured by the gyroscope and accelerometer respectively, \(\boldsymbol{w_n, a_n} \) are the true values, \(\boldsymbol{b_n^w, b_n^a}\) are the bias of angular velocity and acceleration respectively, \(\boldsymbol{w_n^w, w_n^a} \) are the noise information of angular velocity and acceleration respectively.
The bias update equation for angular velocity and acceleration is: \[ \boldsymbol{ b_{n+1}^w = b_n^w + w_n^{b_w} } \] \[ \boldsymbol{ b_{n+1}^a = b_n^a + w_n^{b_a} } \]
In the formula, \( \boldsymbol{ w_n^{b_w} } \) and \( \boldsymbol{ w_n^{b_a} } \) represent the white noise of the Gaussian distribution.
Obviously, when using the Kalman filtering algorithm to estimate the state of a vehicle, the state prediction equation is not a linear equation. To update the covariance matrix of the system state, it needs to be linearized. Therefore, the covariance matrix of the system state can be updated in the following: \[ \boldsymbol{ P_{n+1} = F_n P_n F_n^\top + G_n Q_n G_n^\top } \]
Where,
\[ \boldsymbol{ F_n = I_{21 \times 21} + \begin{bmatrix} \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & -(R_n^{IMU}) & \mathbf{0}_3 & \mathbf{0}_{3 \times 6} \\ (\mathbf{g})_{\times} & \mathbf{0}_3 & \mathbf{0}_3 & -(v_n^{IMU})_{\times} R_n^{IMU} & -R_n^{IMU} & \mathbf{0}_{3 \times 6} \\ \mathbf{0}_3 & I_3 & \mathbf{0}_3 & -(p_n^{IMU})_{\times} R_n^{IMU} & \mathbf{0}_3 & \mathbf{0}_{3 \times 6} \\ \mathbf{0}_{12 \times 21} \end{bmatrix} d_n } \]
\[ \boldsymbol{ G_n = \begin{bmatrix} R_n^{IMU} & \mathbf{0}_3 & \mathbf{0}_{3 \times 12} \\ (v_n^{IMU})_{\times} R_n^{IMU} & R_n^{IMU} & \mathbf{0}_{3 \times 12} \\ (p_n^{IMU})_{\times} R_n^{IMU} & \mathbf{0}_3 & \mathbf{0}_{3 \times 12} \\ \mathbf{0}_{12 \times 3} & \mathbf{0}_{12 \times 3} & I_{12 \times 12} \end{bmatrix} d_n } \]
Zero-velocity Update Equation of IMU
As mentioned earlier, if only the acceleration and angular velocity signals measured by the IMU inertial sensor are used for state estimation of the vehicle, the IMU inertial sensor will exhibit the phenomenon of error accumulation, resulting in a significant bias between the localization effect and the estimated effect. In particular, the growth of speed error will significantly reduce the localization accuracy. Therefore, it is necessary to incorporate the kinematic constraints of the vehicle to limit the growth of errors, thereby achieving a relatively high localization accuracy.
For systems like vehicles that move forward, they do not move laterally or in the direction perpendicular to the ground. It can be known that the lateral and vertical velocities of the vehicle are approximately \(0\). By taking advantage of this characteristic, it can be incorporated into the Kalman filtering algorithm to constrain the vehicle’s velocity. The main principle is that in the vehicle frame, the vehicle’s lateral and vertical velocities are approximately \(0\). Thus, the velocity integral error of the IMU will not increase over time, thereby ensuring high localization accuracy over a long period of time.
To apply the IEKF, considering the relationship of velocities in different frames, the velocity information of the vehicle in the vehicle frame can be obtained through the following formula: \[ \boldsymbol{ v_n^c = \begin{bmatrix} v_n^{for} \\ v_n^{lat} \\ v_n^{up} \end{bmatrix} = R_n^{c^\top} R_n^{IMU^\top} v_n^{IMU} + (w_n)_{\times} p_n^c } \]
In the formula, the lateral and vertical velocities of the vehicle \(\boldsymbol{v_n^{lat}, v_n^{up}}\) is approximately \(0\).
The observational equation of the system is: \[ \boldsymbol{ z_n = \begin{bmatrix} z_n^{lat} \\ z_n^{up} \end{bmatrix} = \begin{bmatrix} h^{lat}(x_n) + n_n^{lat} \\ h^{up}(x_n) + n_n^{up} \end{bmatrix} = \begin{bmatrix} v_n^{lat} \\ v_n^{up} \end{bmatrix} + n_n } \]
In the formula, \( \boldsymbol{ n_n = \begin{bmatrix} n_n^{lat}, & n_n^{up} \end{bmatrix}^T } \) represents the noise of the observed model, which follows a Gaussian distribution with a mean of \(0\) and a covariance matrix of \(R_n \in \mathbb{R}^{2 \times 2}\). The observation matrix \( \boldsymbol{H_n}\) is: \[ \boldsymbol{ H_n = \begin{bmatrix} \mathbf{0}_{3 \times 3} & R_n^{IMU^\top} & \mathbf{0}_{3 \times 3} & -(p_n^c)_{\times} & \mathbf{0}_{3 \times 3} & M & N \end{bmatrix} } \]
Where, \[ \boldsymbol{ M = R_n^{c^\top} R_n^{IMU^\top} \left( v_n^{IMU} \right)_{\times}, \quad N = – \left( w_n^{IMU} – b_n^w \right)_{\times} } \]
Then, the vehicle’s status will be updated according to the following formula: \[ \boldsymbol{ K_n = P_n^- H_n \left( R + H_n P_n^- H_n^\top \right)^{-1} } \] \[ \boldsymbol{ dX = K_n \left( 0 – v_n^c \right) } \] \[ \boldsymbol{ x_n^+ = Update \left( x_n^-, dX \right) } \] \[ \boldsymbol{ P_n^+ = \left( I_{21} – K_n H_n \right) P_n^- } \]
In the formula, \( \boldsymbol{ P_n^+, P_n^- } \) represent the posterior covariance matrix and the prior covariance matrix respectively, while \( \boldsymbol{ x_n^+ , x_n^- } \) represent the posterior state and the prior state of the vehicle. Where, \( \boldsymbol{ dX = \begin{bmatrix} \xi_n^{{IMU}^\top} & db_n^\top & \xi_n^{{R^c}^\top} & dp_n^{{c}^\top} \end{bmatrix}^\top \in \mathbb{R}^{21} } \), Specifically, the operations of \( \boldsymbol{ \xi_n^{IMU} = \begin{bmatrix} \xi_n^{R^{IMU^\top}} & \xi_n^{v^{IMU^\top}} & \xi_n^{p^{IMU^\top}} \end{bmatrix}^\top \in \mathbb{R}^9 } \), \( \boldsymbol{ b_n = \begin{bmatrix} b_n^{w^\top} & b_n^{a^\top} \end{bmatrix}^\top \in \mathbb{R}^6 } \), \(update ()\) are as follows: \[ \boldsymbol{ \begin{cases} x_n^{IMU^+} = \exp_{SE_2(3)}\left( \xi_n^{IMU} \right) x_n^{IMU^-} \\ b_n^+ = b_n^- + db_n \\ R_n^{c^+} = \exp_{SO(3)}\left( \xi_n^{R^c} \right) R_n^{c^-} \\ p_n^{c^+} = p_n^{c^-} + dp_n^c \end{cases} } \]
The \(\exp_{SE_2(3)} \) in the formula represents the mapping of the corresponding Lie algebra from \( se_2(3) to SE_2(3) \), and it can be obtained through the following formula: \[ \boldsymbol{ \xi_n^\wedge = \begin{bmatrix} \left( \xi_n^R \right)_\times & \xi_n^v & \xi_n^p \\ 0_{2 \times 5} \end{bmatrix} } \] \[ \boldsymbol{ \exp_{SE_2(3)}(\xi_n) = I + \xi_n^\wedge + \frac{1 – \cos(\|\xi_n^R\|)}{\|\xi_n^R\|} (\xi_n^\wedge)^2 + \frac{\|\xi_n^R\| – \sin(\|\xi_n^R\|)}{\|\xi_n^R\|^3} (\xi_n^\wedge)^3 } \]
The subscripts and superscripts of each component in the \(𝑈𝑝𝑑𝑎𝑡𝑒()\) operation are consistent with the previous meanings, respectively representing the prior and posterior. By imposing zero-velocity constraints in the lateral and vertical directions of the vehicle, the cumulative error of the IMU inertial sensor is well constrained, and other variables of the system, such as the bias information of angular velocity and acceleration, have also been corrected. This improves the accuracy of the entire vehicle localization algorithm.
References
- Martin Brossard, Silvère Bonnabel, Axel Barrau. Invariant Kalman Filtering for Visual Inertial SLAM[C]. 2018 21st International Conference on Information Fusion (FUSION), 2018: 2021-2028.
- Barrau A, Bonnabel S. The Invariant Extended Kalman Filter as a Stable Observer[J]. IEEE Transactions on Automatic Control, 2017, 62(4): 1797-1812.
- Kok M, Hol J D, Sch N T B. Using Inertial Sensors for Position and Orientation Estimation[J]. Foundations & Trends in Signal Processing, 2018, 11(1-2): 1-153.