imu预积分_IMU预积分知识整理<二>

前言

上个周末在准备答辩的相关事宜，所以没有整理知识点更文。现在顺利完成了答辩，继续总结上次没弄完的知识点。

上次说到预积分的离散形式，给出了预积分的中值法离散表达公式：

之前我一直在说公式里没有给出噪声和偏置的符号表示，这里终于添加了噪声的形式。

预积分的再思考

后面的知识点是预积分残差的协方差和雅可比的推导。在学习手写VIO课件的时候，我对这部分的内容一度非常迷惑，不知这两部分有什么用。后来结合CSDN博客上的解析，算是理解了一些。

首先回到出发点，我们为什么要预积分？

答：为了利用IMU传感器的加速度计和陀螺仪的数据，在减少计算量的情况下，对物体的位姿(Location和Pose)做一个估计。

预积分如何求解？

答：利用上述得到的离散预积分的公式，迭代的进行计算。

这个时候问题就出现了，由于存在噪声误差和偏置误差，如果不考虑这两个变量，随着时间的推移，这部分的误差会逐渐累计，影响到后面的位姿估计。课件里有个很好的比喻，就是综艺里面经常玩的一个游戏，给出一个词汇，第一个人看之后用肢体语言向第二个人表演，然后第二个人再向第三个人表演...经过几次传递后，最后一个人很难正确说出这个词汇。

因此，我们会用优化的方式对每次IMU计算得到的PVQ进行处理，把误差分摊到每一次测量上。

既然用到了误差，那么就要构建残差函数，预积分的误差函数为：

误差函数为一个15维的矩阵。其中，旋转部分四元数只取代表旋转的3个变量。

在计算机视觉中，对于非线性优化问题，通常使用迭代的方式进行求解，没错就是求导，那么我们需要计算预积分残差函数的雅可比矩阵。

为什么要确定IMU随机噪声和高斯噪声的协方差呢？

答：简单的说，协方差矩阵里的每个元素，就是优化问题中对应残差项的权重系数。

非线性优化中，IMU误差，相机重投影误差，他们的权重是受噪声协方差的影响。比如IMU噪声协方差小，权重大，更相信IMU。噪声协方差影响置信度，表示信谁多一点。VINS论文中公式没有直接说。

这里需要了解误差传递函数，来计算预积分量的协方差。

此外，还有一个预积分(不是预积分残差)的雅克比矩阵可以更新由于陀螺仪偏置造成的误差。

知道了上述知识点，才是完整的预积分过程和计算过程。

预积分量的协方差
首先来看看预积分量的协方差是如何计算的。

给出协方差的传递公式(数学上的知识，拿来直接用而不问为什么)：

已知 

先给出相邻时刻误差的线性传递方程，后面会有证明：

接着给出协方差矩阵的传递形式

  为测量噪声的协方差矩阵，我们可以人为标定出来。

  为状态量误差的协方差矩阵，是可以通过递推求出每一个时刻的协方差矩阵形式，最开始  。

接下来就是讨论F和G是如何求的

第一种方式

上面给出了相邻时刻的线性传递方程，它的推导如下：

令状态量为

其中  为真实值，  为误差，输入量u的噪声为n。

对于非线性系统

它的状态误差的线性递推关系如下：

我们根据状态变量之间的关系就可以求F和G的雅可比矩阵。

第二种方式

如果我们知道误差随时间变化的导数关系，比如：

(课件里面为什么是这个关系，没想明白。跟下面速度和加速度的公式对不上。)

那么误差状态的传递方程为：

相比之前的线性推导关系，可以得到:

刚好在VINS中，我们是知道误差随时间变化的导数关系的。比如我们已经知道速度和状态量的关系：

那么我们就可以推导速度的误差和状态a误差的关系

这样就可得到A和B的值，进而得到F和G。

回顾预积分的误差递推公式，把噪声和偏置都考虑进来，有：

根据第一种方式，我们希望得到如下的形式：

后面就是推导F和G了。

先给出最终的结果。

其中的系数为：

推导过程比较繁琐，我还没细看，后面再慢慢理解。

总结：公式那么多，最后要得到的就是预积分形式下误差递推的F和G的形式，然后再根据协方差的递推公式，得到当前状态下的协方差矩阵，后面优化的时候根据这个协方差矩阵去设置权重值。

这部分内容有些不好理解，照着讲义捋了一遍后思路清晰了很多，还是得多总结多思考。

结语

扯点其他的。

几天前完成了自己硕士毕业答辩，因为疫情影响，毕业答辩放在了网上。预答辩的时候没怎么准备，回答的一团糟，对自己的状态不是很满意，这次终答虽然准备的也比较仓促，但是表现的还行。

回想自己这三年，自己不算懒惰，几乎每次很晚才回寝室，大部分周末也待在实验室(单身狗的生活就是这么枯燥)，可毕业的时候没有一篇满意的论文发布，毕设做的东西也烂的和shit一样。心里还是有不少遗憾的，和自己最开始读研时候的“雄心壮志”差的有点远。

原因也有想过，可能还是自己太菜了吧，然后选择的课题不太对，研究的人和成果太少。。。最后老师给的研究的题目属于戴着枷锁跳舞。

现在总算是毕业了，大学时候参加的飞思卡尔比赛，到读研时候选择三维方向，以及现在参加工作时选择的行业，也算是不忘初心。缺憾就是在SLAM领域，我才刚刚起步，和认识的同届专门做无人驾驶的朋友差了不少。

种下一棵树最好的时间是十年前，其次就是现在。与自己做个一年的约定，看一年后自己比现在进步如何。

加油吧。

参考资料

[1] 贺一家，高翔，崔华坤，手写VIO讲义