VINS 细节系列 - 预积分_vins 预积分-CSDN博客

理论部分

IMU的模型

以t时刻，IMU作为参考系，我们能够得到以下的IMU模型：

左边是IMU的测量值，包括了3轴的加速度，以及3轴的角速度信息；右边第一项是陀螺仪和加速度计的真值，第二项是偏移偏移，最后一项是测量噪声项。注意一点的是，由于IMU测量的时候，是除了重力加速度外，所有外力对IMU参考系力的和对系统产生的加速度，会把反向重力加速度测量在内。意思是，当IMU自由落体的时候，加速度的读数是0；IMU平放在桌子上的时候，读数是g。

这里假设噪声 na,nb 是服从高斯正态分布，而 ba,bw 偏置服从随机游走模型(倒数服从高斯正态分布)。在已知 初始的P1,Q1,V1,ba1,bg1 的情况下，并且知道每个时刻ba,bg的时候，通过下面的这个式子的离散化表示，我们就能够通过积分得到后面这每一个时刻的P, Q, V：

即可以通过这个式子：

正常这个公式是可以比较直观上理解的；但是有一个问题，因为这个公式中，有一个 R旋转矩阵是从imu坐标系旋转到世界坐标系的绝对位姿。后端优化的过程中，会不断的修正 p,q,v 的值，使得一个代价函数最小，现在我们一旦修改了一个p, 因为积分项里面有一项与IMU的绝对位姿有关，因此我们又要进行重复的一个积分运算。即：

理解：首先要关注问题的关键，即使我们之前没有接触过预积分，但是我们看了上面的一段话，也要明白其实这个问题的本质是坐标系的转换问题；想一下，假如我们从 1 到 2时刻进行积分，然后后端优化，这时候我们可以求出来 p,q,v 优化后的值，那不能求出来就不管了吧，我们要对之前的位姿态修正呀，可是你这一修正，q变了，对应的R变了，那就从新从1 积分到 2，得到一个准确的2对应的位置；OK，咱在往前走，2 到 3， R右变了，在回来重新积分一下，得到准确的3，就这样往复着，很麻烦！

为了避免这一个重复积分运算的过程，vins提出了预积分的概念，通过了坐标系的转换，将绝对的坐标姿态转化为了相对的坐标转换，把要积分的项转化了一个形式。即把R的绝对位姿转化成相对位姿，相对于每一帧的绝对位姿，即：

现在三个临时量只和当前的加速度计偏移和陀螺仪的偏移有关。而于他们在世界坐标系的绝对位姿没有任何关系。任何帧的P，V，Q的改变不影响积分过程，因此就不用一直重复积分。这三个变量只和偏移ba,bw有关。更新偏移，由于偏移变化是随时间缓慢变化的，因此可以用泰勒一阶展开：

而通过离散积分的方式，可以获取这三个积分项的离散迭代表达：

到这里，我们就可以带入IMU 6个轴的观测值，进行迭代求解，最终解算出这三个积分项；这里的预积分里面还是有IMU偏差的，通常是和视觉联合初始化，可以求出来，补偿一下就行了，更新预积分的值！而不用像以前一样更新PVQ；得到更新后的预积分值后，叠加到以前的PVQ，在转换到世界坐标系下！到这里，我们还不知道这三个积分项的含义是什么，直观上理解，可以认为，bk帧做自由落体运动，以这个帧作为参考系，imu在bk到bk+1这段时间的位移，速度，姿态的变化。从数学上去描述就是：

上面这个预积分的值是我们用公式 推算出来的，代表的意思就是b坐标系下， K+1 时刻PVQ 减去 K 时刻PVQ ；我们还有一个预积分的测量值，如上面的（7）；因此imu的残差vins就定义为：

残差是一个向量，最终的残差是一个标量，因此我们就要把向量转化为一个标量。即要乘以他的协方差矩阵。协方差矩阵可以理解为，因为向量中每个元素变化的情况值不一样，有些元素变化得比较大（方差大），有些元素变化比较小（方差小），为了使得把这种变化统一起来，因此需要有一个标准来量化它，就认为是这个协方差矩阵。为了获取协方差矩阵，需要建立一个线性高斯误差状态传播方程，由线性高斯系统的协方差，就可以推导出方程协方差矩阵了，也就是测量状态的协方差矩阵了。