vins-fusion代码解读[五] imu在vins里的理解

SLAM新手，欢迎讨论。

IMU作用

vins中，IMU只读取IMU六轴的信息，3轴线加速度（加速度计）和3轴角速度（陀螺仪）。
通过对陀螺仪的一次积分，直觉上可以获取姿态的变化情况，对加速度的一次积分和二次积分结合陀螺仪的信息，可以获取位置的变换情况。

背景知识：
IMU: 通常6轴陀螺仪，包括3轴线加速度和3轴角速度。9轴陀螺仪增加3轴地磁信息。
MEMS 陀螺仪没有姿态的输出，所有的MEMS陀螺都是非旋转装置，通过获取一个振动机械元件上的哥氏加速度效应，实现角速率检测。即一个验证质量在一个平面内做正弦振动，如果此平面以角速率Ω旋转，那么哥氏力就会使该质量以垂直于该平面的方向做正弦振动，其幅值正比于Ω。对哥氏力所引起的运动进行测量，就可获得Ω的信号，这就是微机械陀螺的基本工作原理。
市面上IMU可以输出姿态四元数或者欧拉角，是通过3轴线加速度和3轴角加速度或者增加3轴地磁信息融合求解出来。
加速度a能够提供重力g在俯仰角和翻滚角平面的投影，但是由于IMU本身在不断运动变化，a不等于g，因此不能很好的反应翻滚角和俯仰角，但是长期来看，这种方法不会产生大的漂移。
角速度w能够预测三个角度的变化量，在短期来看，积分能够预测两个时间段内角度的变化，长期运用会造成角度的漂移。
因此通常的做法是融合两者计算结果（卡尔曼滤波），能够得到精确的俯仰角和翻滚角，由于g在偏航角的平面上没有投影，因此偏航角是不准确的（因为相当于只能依靠角速度的积分获取），会有很大的漂移。融合地磁信息之后，可以提高偏航角的准确度，但是同样存在偏移。而且地磁的信息很容易受到环境的干扰，大型设备用电，电机转动都可能产生磁场，干扰地磁信息。

IMU的模型：

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

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

这里假设噪声na,nb是服从高斯正态分布，而ba,bw偏置服从随机游走模型(倒数服从高斯正态分布)。

因此，我们通过这个IMU模型，可以获取得到任意时刻的位置，速度，以及姿态。即，我们在已知初始的P1,Q1,V1,ba1,bg1的情况下，并且知道每个时刻ba,bg的时候，通过下面的这个式子的离散化表示，我们就能够通过积分得到后面这每一个时刻的P,Q,V。

即可以通过这个式子：

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

即：

第二帧位姿发生了改变，导致最终从第二帧开始，所有的积分项都要重新进行积分，非常耗时。
为了避免这一个重复积分运算的过程，vins提出了预积分的概念，通过了坐标系的转换，将绝对的坐标姿态转化为了相对的坐标转换，把要积分的项转化了一个形式。即把R的绝对位姿转化成相对位姿，相对于每一帧的绝对位姿，即：

现在三个临时量只和当前的加速度计偏移和陀螺仪的偏移有关。而于他们在世界坐标系的绝对位姿没有任何关系。任何帧的P，V，Q的改变不影响积分过程，因此就不用一直重复积分。

这三个变量只和偏移ba,bw有关。更新偏移，由于偏移变化是随时间缓慢变化的，因此可以用泰勒一阶展开。

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

到这里，我们就可以带入IMU 6个轴的观测值，进行迭代求解，最终解算出这三个积分项。

到这里，我们还不知道这三个积分项的含义是什么，直观上理解，可以认为，bk帧做自由落体运动，以这个帧作为参考系，imu在bk到bk+1这段时间的位移，速度，姿态的变化。从数学上去描述就是：

因此imu的残差vins就定义为：

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

噪声对角线协方差矩阵就是我们需要标定imu的四个参数。两个测量噪声的方差，两个偏置倒数的方差。

总结预积分：

为了使得非线性优化中，不用每次改变位姿后，积分需要重新计算的问题。
现在改变位姿（速度，位移，旋转四元数关于世界坐标系的），预积分内的项保持不变。
改变ba,bw后，预积分的项可以通过泰勒一阶展开来进行更新。