理论四、 MSCKF IMU Propagation

感谢：紫薯萝卜 https://www.zhihu.com/column/c_1121353757664964608

IMU Propagation过程：利用两帧图像之间的所有IMU观测数据（加速度 和角速度 ），对MSCKF的状态向量和协方差进行迭代预测。它相当于EKF中的预测过程。本节主要探讨的是如何根据状态量之间的物理关系推导出状态的线性化运动模型，以及如何根据运动模型对状态向量及其协方差进行迭代预测。

 

1. 运动模型推导

1.1 状态真实值的系统模型

各状态量真实值之间的 物理关系 如下：

其中， 是Global系下的加速度， 是IMU系的角速度

IMU观测量的定义如下：

其中， 表示四元数转旋转矩阵， 为IMU测量误差（高斯白噪声）。注意：原始MSCKF论文中考虑了地球自旋速度 ，S-MSCKF中将这部分忽略，观测量可以化简为

1.2 状态估计量的系统模型

状态估计量的系统模型：

其中，加速度和角速度估计值定义如下（仅考虑了零偏和噪声）：

1.3 运动模型推导

根据状态估计量的系统模型推导IMU误差状态的运动模型（需要直到IMU的误差方程，再去写下面方程就很简单）：

误差方程一般的求解思路：

f(p) = p2 + .....

f(p+dp) = (p+dp)2  + ....

得到：f(p+dp) - f(p) = 

 

先上结论：

其中 向量中依次是陀螺仪误差、陀螺仪bias误差、加速计误差、加速计bias误差。具体推导细节放在最后。

 

2. 状态向量预测

这里直接对状态估计量进行迭代预测，而不是误差状态向量，但协方差是根据误差状态的运动模型进行迭代。每来一帧IMU观测数据，需要对 中的姿态 、速度 和位置 进行预测， 保持不变。

姿态预测

姿态四元数采用的0阶积分(假设角速度在单位时间内恒定不变)，推导过程见[1]中的1.6.1小节。

其中

速度和位置预测

速度和位置可以采用的4阶Runge-Kutta积分（精度更高计算方式，了解即可）, 其计算过程如下：

常用的三种数值积分：Euler积分、Mid-Point积分、Runge-Kutta积分(4阶)。它们的精度依次从低到高、计算量依次从小到大。令 的权重为1，其他为0，RK4就退化为Mid-Point法；令 的权重为1，其他为0，RK4就退化为Euler积分。

 

3. 状态协方差预测

状态协方差预测公式如下：

单帧IMU数据的迭代公式如下：

其中， 为IMU迭代的帧数， 为两帧IMU数据的时间间隔，上式中假设了 在 时间段内恒定。

参考文献

1. Indirect Kalman Filter for 3D Attitude Estimation
2. Mourikis A I, Roumeliotis S I. A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation[C]// IEEE International Conference on Robotics and Automation. IEEE, 2007:3565-3572