VINS-Mono-IMU预积分 （三：为什么要预积分+预积分推导）

第二节的积分公式

滤波问题中的IMU积分

从上面第二节的公式可以看出来，积分的公式实际是从第k时刻的状态到第k+1时刻的状态转移，其中的$p_{bk},v_{bk},q_{bk}$都是第k时刻的最优估计值
在滤波中，来了图像帧的位姿其实是相当于观测值，两帧图像间大概会有10个IMU数据，图像的频率一般在20Hz~30Hz，IMU的一般为100Hz，当来到第k+1时刻的位姿时，会把相机的位姿和IMU积分得到的位姿同时扔给滤波器中获得置信度最高的位姿，然后把该位姿赋值给第k时刻的变量，循环往复。
IMU的位姿在滤波里面的作用就是预测值，滤波中一般有预测和更新，通过IMU预测新一帧的位姿，通过特征点重投影的值来进行更新。
在滤波问题中IMU实际上是不需要预积分的，只需要正常积分即可。

在优化问题中的区别 + 预积分推导

在优化问题中会优化过去k时刻$k-1,k-2,k-i$的位姿，而滤波是只会维持当前时刻的最优位姿估计，以前的位姿估计和当前时刻的位姿没有任何关系，优化会对过去的状态量进行统一的调整，此时IMU扮演的角色是第k到第k+1时刻的位姿的约束，在滤波的积分中，所有量的基坐标都是世界坐标，即全部都是从开始时刻一直积分到最新时刻，而在IMU中的为了形成帧间约束，积分的基坐标是 $b_k$ 而不是 $w$ ，此时只需要将上面的公式在左右两边乘上$(R_{bk}^w{)}^{-1}$
此时公式变成如下(公式来源于VINS-Mono论文)：


此时的$\alpha ,\beta ,\gamma$就是预积分量，此时可以看出这3个量没有任何相邻两帧之间的关系，即没有直接引入相邻两帧的姿态或者速度来作为它的观测值（相机的重投影位姿），在滤波的积分中的第k时刻的PVQ就是融合了相机观测值的结果，这里的预积分是要对位姿进行约束的，所以本质上是不允许和观测值有直接联系。唯一有一点关系的是$R_t^{b_k}$。这个 $t$ 时刻就是 k 到 k+1 时刻的 $\Delta t$ 时刻，$R_t^{b_k}$ 表示的是第 k 时刻到 $\Delta t$ 时刻的位姿变化量，也是和$b_k$时刻的位姿和速度没有任何关系，$b_k$ 是由最下面的角速度积分实时算出来的 $\Delta t$ 时间的变化量。IMU预积分约束相邻两帧之间的约束。
这里面的 $\alpha ,\beta ,\gamma$ 在优化问题中就是一个观测，和重投影一样都是属于观测。

离散时间下的预积分

离散下的积分形式如下：

实际IMU信号必然是离散的，连续积分是理想的状态，需要通过10个IMU数据得到预积分量，VINS中采取的是中值积分的过程。
上面的公式可以用中学的公式$\Delta x=v_0\Delta t+\frac{1}{2}a\Delta t^2$ 进行理解，这里面的 $a$ 是固定值，但是实际上两个时刻的 a 是不会相同的，中值积分就是当来到第 i 和 第 i+1 时刻的 a 的时候 $a=\frac{a_i+a_{i+1}}{2}$ 然后将这个 a 带入进去获得离散时间下位移的变化量，角速度就是$\hat{w}=\frac{w_i+w_{i+1}}{2}$，位姿R也是$R=\frac{R({\gamma }_i^{b_k})+R({\gamma }_{i+1}^{b_k})}{2}$ 这里在代码中就是对四元数进行线性插值，这样的近似就可以不使用积分的形式了。$b_k$ 时刻（即起始时刻）的位姿实际是一个单位阵，位置和速度为 0。来到 IMU 数据的时候永远都是先更新姿态量再更新速度和位置。

说明1： 实际上，上面的 $\hat{a}$ 是包含重力加速度的，由于重力加速度是一个常量，所以可以把它提到外面去，但是这也涉及到后面的重力对齐，只要与重力方向平行才能减去.

说明2： 以上来看 IMU 预积分也不难，代码中实际又很复杂，实际上是为了计算这个IMU预积分量的协方差矩阵（置信度）复杂，因为走得越远置信度会降低，不同的置信度在优化中的优化权重都不同，上面只写了更新 $b_{k+1}$ 时刻的位姿，实际上还需要更新 $b_{k+1}$ 时刻的协方差矩阵，计算协方差矩阵会利用误差卡尔曼的理论来进行计算，将在下一节中进行讲解