imu预积分处理

VINS-Mono是HKUST的Shen Shaojie团队开源的一套非常优秀的Visual-Inertial融合定位算法。关于算法的介绍以及论文可以通过此链接查看。

IMU、ˆω和ˆa的原始陀螺仪和加速度计测量值由下式给出：

 

 在车身坐标系测量的IMU测量结合了抗重力和平台动力学的力，受加速度偏置ba、陀螺仪偏置bw和加性噪声的影响。

我们假设这个额外的噪声为加速度噪声和陀螺仪噪声。这些噪声服从高斯噪声分布。

给定对应于图像帧bk和bk+1的两个时间瞬间，位置、速度和方向状态可以通过世界坐标系中时间间隔[tk, tk+1]内的惯性测量传播: 

 可以看出，IMU 状态传播需要帧 bk 的旋转、位置和速度。当这些起始状态发生变化时，我们需要反向传播 IMU 测量值。特别是在基于优化的算法中，每次我们调整姿势时，我们需要在它们之间反向传播 IMU 测量值。这种传播策略在计算上要求很高。为了避免反向传播，我们采用了预积分算法。在将参考帧从世界帧更改为局部帧bk后，我们只能预先集成与线性加速度^a和角度速度^ω相关的部分，如下所示：

其实IMU积分和IMU预积分都可以当作相邻图像帧之间的约束，区别在于，IMU积分公式中包含了IMU在世界坐标系下的位姿，这就导致了当每次图像帧对应的IMU位姿调整时，预测下一图像帧对应的 IMU位姿只能再次进行积分。而IMU预积分公式中的位姿变化只是相对于上一图像帧对应的IMU位姿的变化量，实际计算时用不到IMU在世界坐标系下的位姿，这样每当IMU位姿调整后，只需在此基础上加上一个预积分量就能得到下一帧位姿。
 

预积分的计算也很巧妙，只需要在IMU积分的等式两边左乘一个由世界系到IMU系的旋转矩阵即可。

 然而，其中，α \alphaα、β \betaβ、γ \gammaγ就是IMU的预积分量。具体形式如下：
 

 IMU预积分只与IMU测量值有关，其实IMU预积分就是将相邻两图像帧之间的IMU数据积分起来，以此作为约束来限制相邻图像帧的位姿调整。

 离散时间的预积分：

从上面的分析我们知道预积分就是把一段时间的IMU数据积分起来。而我们的SLAM系统是以离散时间的形式向后端发送IMU数据，所以我们要用到IMU运动模型的离散时间积分。离散时间δt 的状态变化量如下式计算。

 在实际应用中有两种方法计算。

①用第k时刻的测量值来计算两个相邻时刻 k 到 k+1 的位姿，即：

②用两个时刻的测量值 a, ω 的平均值来计算两个相邻时刻 k 到 k+1 的位姿，即：

 第①种方法叫做欧拉法，第②种方法叫做中值法。

VINS-mono中采用的中值积分的方法来计算IMU的预积分：