里程计航迹推演与IMU预积分

参考：VINS代码注释版

预积分，难以理解的地方主要是，雅可比的计算、怎么通过迭代的方式得到协方差的、使用deltaBias更新预积分是什么鬼?
其实，这与计算轮式里程计在两帧图像之间的协方差过程类似，只不过里程计比较简单，使用航迹推演，再结合协方差传递律，就能得到，所以就很容易明白了，里程计中的航迹推演就对应IMU预积分中的使用中值法计算得到的两帧之间 PVQ 增量（注意，对速度和位移的预积分并没有去掉加速度测量值中的重力g），即预积分要算的东西，预积分说白了就是测量值，测量值都有noise，航迹推演的noise我们知道怎么算得，IMU预积分的noise也一样，只是稍微麻烦一点，用的一套误差运动学方程，最后可得到如下状态方程，
$\dot x = Ax+Bw, w \sim N(0, \Sigma)$

因为$A$ 和 $B$ 不是常矩阵，所以这是一个线性时变连续系统，学过控制理论的对这个公式应该很熟悉吧，因为采样周期很小，所以可以进行近似离散化，进而得到，
$x (k+1)= (I + \delta tA)x(k)+\delta t Bw(k), w(k) \sim N(0, \Sigma_k)$

注意，这里的 $w(k)$ 已经变成了离散的噪声，关于IMU的连续噪声如何换成离散噪声用过Kalibr的都比较熟悉吧。为了得到从第i张图片到第i+1张图片之间对IMU预积分的协方差，协方差的传播律大家已经很熟悉了，那预积分的协方差就是对上式进行迭代计算。除此之外，对IMU预积分的时候为什么要计算一个雅可比矩阵，因为后面要在初值处进行泰勒展开（实际上只用到了对bias的雅克比）。每来一帧IMU数据，就递推一次雅可比矩阵和协方差矩阵，最后处理完最后一帧IMU数据后，预积分就完成了，协方差有了，然后就顺理成章开始优化了.
上面这块的实现在 IntegrationBase::midPointIntegration() 函数内

预积分的结果其实就是测量值，类似我们相机观测到的特征点的座标、里程计的测量值等等，唯一的区别就是，预积分作为测量值本来是不会改变的，但是因为这个测量值里面还含有优化变量bias，这是因为，我们在预积分公式的推到时，将IMU相对世界座标系（w）的位姿、速度都转换成相对上一帧图像时IMU的座标系上去了（即，b_k），但是确没有bias做任何处理，所以bias就留下了（其实bias本身变化就很小，两帧图像之间只有微小的变化，不像IMU的位姿，每一时刻都有很大程度的不同），既然这个预积分得到含有优化变量bias，那么当bias改变了，我们自然就要重新计算预积分了，怎么就算了？一个直接的方式就是使用新的bias重新计算一次预积分，这种方法固然可行，但是这和我们当初为什么分离优化变量的初衷相违背（我们正是为了在优化变量更新后不重新计算预积分啊），那该怎么办呢，还记得这个bias的变化很小这个常识吗，很自然就想到使用泰勒展开（进行近似），泰勒展开需要雅可比啊（即，预积分对 k 时刻bias的梯度的转置），这个雅可比我们刚刚在预积分时就算过了啊，所以啊，预积分最后一层神秘的面纱已经揭开了^~^
上面这块的实现在 IntegrationBase::evaluate() 函数内

刚说到要开始优化了，IMU预积分的结果就是测量值，我们如何使用使用测量值进行优化呢？还记得我们如何使用相机对地图点的观测构造残差的的吗，这里也一样，其实我们在推导预积分的时候就已经得到残差了（公式5），只是我们没有写成残差的形式而已，需要进行移项，然后结果一目了然（公式24），然后就是优化的套路，如高斯牛顿法，给一个增量，通过泰勒展开分离，得到这个增量的线性方程，然后就构成了一个线性最小二乘，通过求解这个线性最小二乘，使目标函数最小，也就得到这个增量应该是多少，将这个增量加到上一次优化变量的值上，就得到了本次的优化变量的值，一直循环下去，就得到最终的优化变量的结果，因为进行泰勒展开时，需要使用雅可比，计算雅可比使用的是待优化变量的初值（为了收敛，必须使用初值），只有第一次优化时，计算雅可比使用的是初值，之后使用的上一次优化结果作为线性化点计算雅可比，因为需要反复更新雅可比矩阵，所以耗时是难免，那么有没有什么好的方法避免呢？SVO为了加速，采用inverse compositional formulation方式，只需要计算一次雅可比矩阵
上面这块的实现在 IMUFactor::Evaluate() 函数内

---

<完>
@leatherwang

---