NOTE / 8/2/2020
IMU Attitude Estimation: Quaternion Kinematics for ESKF, Part 4 SLAM Technical Notes SLAM VIO sensor fusion
Using the approach in Quaternion Kinematics for ESKF , I implemented an IMU attitude estimator: the gyroscope performs propagation and the accelerometer provides updates.
State
The state contains the IMU attitude from the IMU frame to ENU and the gyroscope bias:
χ = [ I G R b g ] \mathbf \chi = [\begin{matrix} {}^G_I \mathbf R & \mathbf b_g \end{matrix}] \\ χ = [ I G R b g ] Propagation
The IMU kinematics from the book are:
I G R ˙ = I G R [ ω m − b g − n w ] × b g ˙ = 0 + n g \begin{align*} {}^G_I \dot{\mathbf R} &= {}^G_I \mathbf R[\mathbf \omega_m - \mathbf b_g - \mathbf n_w]_\times \\ \dot{\mathbf b_g} &= \mathbf 0+ \mathbf n_g \end{align*} \\ I G R ˙ b g ˙ = I G R [ ω m − b g − n w ] × = 0 + n g Define rotational error as:
I G R = I G R ^ Exp ( δ θ ) {}^G_I \mathbf R = {}^G_I \hat{\mathbf R} \text{Exp}(\delta \mathbf \theta) \\ I G R = I G R ^ Exp ( δ θ ) The error-state kinematics are:
δ θ ˙ = − [ ω m − b g ] × δ θ − δ b g − n w δ b g ˙ = n g \begin{align} \dot{\delta \mathbf \theta} &= -[\mathbf \omega_m - \mathbf b_g]_\times \delta \mathbf \theta - \delta \mathbf b_g - \mathbf n_w \\ \dot{\delta \mathbf b_g} &= \mathbf n_g \end{align} \\ δ θ ˙ δ b g ˙ = − [ ω m − b g ] × δ θ − δ b g − n w = n g With Euler integration:
δ θ ← Exp [ ( ω m − b g ) Δ t ] T δ θ − δ b g Δ t + θ i δ b g ← δ b g + ω i \begin{align*} \delta\mathbf \theta &\leftarrow \text{Exp}[(\mathbf \omega_m - \mathbf b_g)\Delta t]^T \delta \mathbf \theta - \delta b_g \Delta t +\mathbf \theta_i \\ \delta \mathbf b_g &\leftarrow \delta \mathbf b_g +\bm\omega_i \end{align*} \\ δ θ δ b g ← Exp [( ω m − b g ) Δ t ] T δ θ − δ b g Δ t + θ i ← δ b g + ω i In matrix form:
[ δ θ δ b g ] = [ Exp [ ( ω m − b g ) Δ t ] T − I Δ t 0 I ] ⏟ F x [ δ θ δ b g ] + [ I 0 0 I ] ⏟ F i [ θ i ω i ] \left[\begin{matrix} \delta \mathbf \theta \\ \delta \mathbf b_g \end{matrix}\right] = \underbrace{\left[\begin{matrix} \text{Exp}[(\mathbf \omega_m - \mathbf b_g)\Delta t]^T & - \mathbf I \Delta t \\ \mathbf 0 & \mathbf I \end{matrix} \right]}_{\mathbf F_x} \left[\begin{matrix} \delta \mathbf \theta \\ \delta \mathbf b_g \end{matrix}\right] + \underbrace{ \left[ \begin{matrix} \mathbf I & \mathbf 0 \\ \mathbf 0 & \mathbf I \end{matrix} \right] }_{\mathbf F_i }\left[\begin{matrix} \mathbf \theta_i \\ \mathbf \omega_i \end{matrix}\right] \\ [ δ θ δ b g ] = F x [ Exp [( ω m − b g ) Δ t ] T 0 − I Δ t I ] [ δ θ δ b g ] + F i [ I 0 0 I ] [ θ i ω i ] The covariance propagation is:
Σ k + 1 = F x Σ k F x T + F i Q F i T \mathbf \Sigma_{k+1} = \mathbf F_x \mathbf \Sigma_k \mathbf F_x^T + \mathbf F_i \mathbf Q \mathbf F_i^T \\ Σ k + 1 = F x Σ k F x T + F i Q F i T Update
When the IMU is moving at constant velocity, the accelerometer measures gravity direction. In practice, departures from constant velocity are treated as noise, yielding the measurement equation:
a = − I G R T g + n a \mathbf a =- {}^G_I \mathbf R^T \mathbf g + \mathbf n_a \\ a = − I G R T g + n a where
g = [ 0 0 − 1 ] T \mathbf g = \left[\begin{matrix} 0 &0 & -1 \end{matrix} \right]^T g = [ 0 0 − 1 ] T is the gravity direction in ENU.
The Jacobian is:
H = [ [ I G R T g ] × 0 ] \mathbf H = \left[ \begin{matrix} [{}^G_I \mathbf R^T \mathbf g]_\times & \mathbf 0 \end{matrix} \right] \\ H = [ [ I G R T g ] × 0 ] Initialization
As in the previous article , use the accelerometer to initialize roll and pitch, and set yaw to zero.
Implementation
ydsf16/IMUOrientationEstimator
References
[1] Sola, Joan. Quaternion Kinematics for the Error-State Kalman Filter . 2017.
