VSLAM：IMU预积分公式推导
一、IMU预积分
传统的递推算法是根据上一时刻的IMU状态量，利用当前时刻测量得到的加速度与角速度，进行积分得到当前时刻的状态量。但是在VIO紧耦合非线性优化当中，各个状态量都是估计值，并且会不断调整，每次调整都会重新进行积分，传递IMU测量值。预积分的目的是将相对测量量与据对位姿解耦合，避免优化时重复进行积分。四元数的表示方法有两种：一种是Hamilton(右手系)表示，另一种是JPL(左手系)表示。读者对公式推导时一定注意。
常用性质：
1. 四元数的连续积分：
2. 四元数的左乘与右乘：
我们将四元数表示为：
，则左右乘可以表示为：
3. 四元数导数如下：
，则导数可表示为：
1.1 当前时刻的位置，速度，旋转变量的连续表达式
我们将图像帧记作及，body坐标系下记作和，我们将位置，速度和旋转在时间到内进行积分，其世界坐标系下的公式可以写为：
1.2 当前时刻的位置，速度，旋转变量的离散表达式
我们以中值积分给出离散表示:
1.3 两帧之间的位置，速度，旋转增量的连续表达式
转到第帧的body坐标系下，相当于两边同时乘，我们直接用论文中的公式来表示：
其中：
对的相对运动量，其中状态的改变并不会对其产生影响，可以将其作为非线性优化变量，避免重复计算。实际到这里，只要求解出积分，我们就完成了预积分的计算，我们的目标也就是在此。实际当中随机游走也是发生改变的，所以我们将上述变量再次进行一阶近似，我们再次使用论文中的公式进行表示：
至此，IMU的预积分表达式我们就已经得到了。
1.4 两帧之间的位置，速度，旋转增量的离散表达式
1.5 连续表达式下的位置、速度、旋转增量误差、协方差、Jacobian
IMU在每一个时刻积分出来的变量都是有误差的，我们针对误差进行分析，分析误差的传递方程，下面为连续时间下的误差导数方程：
进行推导，假设true表示真实测量值，含误差，nominal表示不含噪声的理论值，则有：
其中：
则：
进行推导：
根据四元数的导数性质：
我们将上面的等式进行汇总：
最终：
我们将导数的定义拿来：
则：
对比扩展卡尔曼公式，我们发现其正好是非线性系统进行线性的表达式，据此，我们给出相似的协方差预测公式：
其中，协方差初始值为0，噪声协方差矩阵可以表示为：
类似，我们也可以获得误差的Jacobian迭代公式：
Jacobian的初始值为单位矩阵。
1.6 离散形式的增量分析
实际只需根据中值积分，将连续形式表达式进行离散化即可，推导过程省略，比较简单
其中：
离散误差传递方程可以简写为：
则Jacobian的迭代公式为：
。注意，我们在此计算Jacobian，仅仅是为了后端非线性优化过程对bias的计算提供帮助。
协方差的迭代公式：
协方差矩阵初始值为0，噪声的协方差矩阵为:
本文仅做学术分享，如有侵权，请联系删文。
