IMU 姿态估计 Quaternion kinematics for ESKF[part 4]

使用《Quaterniond Kinematic for ESKF》的方法，实现一个IMU姿态估计。用陀螺仪的做Propagation，加速度计做Update。

State

我们的状态里面是IMU坐标系到ENU坐标系的姿态和陀螺仪的bias。

$$\mathbf \chi = [\begin{matrix} {}^G_I \mathbf R & \mathbf b_g \end{matrix}] \\$$

Propagation

把书中的IMU kinematic搬出来：

$$\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*} \\$$

定义旋转误差为：

$${}^G_I \mathbf R = {}^G_I \hat{\mathbf R} \text{Exp}(\delta \mathbf \theta) \\$$

我们的error-state kinematic就是

$$\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} \\$$

用欧拉积分

$$\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*} \\$$

写成矩阵形式：

$$\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] \\$$

协方差Propagation就是

$$\mathbf \Sigma_{k+1} = \mathbf F_x \mathbf \Sigma_k \mathbf F_x^T + \mathbf F_i \mathbf Q \mathbf F_i^T \\$$

Update

如果IMU做匀速运动，加速度计测量的就是重力（方向）。当然实际上，并不可能做匀速运动，可以当做噪声处理。我们就可以建立一个量测方程。

$$\mathbf a =- {}^G_I \mathbf R^T \mathbf g + \mathbf n_a \\$$

其中，

$$\mathbf g = \left[\begin{matrix} 0 &0 & -1 \end{matrix} \right]^T$$

为重力在ENU系下的方向。

雅克比为：

$$\mathbf H = \left[ \begin{matrix} [{}^G_I \mathbf R^T \mathbf g]_\times & \mathbf 0 \end{matrix} \right] \\$$

初始化

与上一篇文章相似https://zhuanlan.zhihu.com/p/152662055，采用加速度初始化roll pitch，yaw设置为0.

实现

https://github.com/ydsf16/IMUOrientationEstimator

参考资料

[1] Sola, Joan. Quaternion kinematics for the error-state Kalman lter[J]. 2017.