VINS-Mono详解（2）——IMU数学模型推导

        关于IMU数学模型的函数部分，主要存在于vins_estimator/src/factor/integration_base.h之中。这部分主要实现了两个功能：第一个是IMU预积分获得α、β、γ值，另一个是误差传递函数的获取。至于源码这部分的内容，其实在推导完VINS-Mono中对于IMU的数学模型之后再来看，便没有什么难度了。

        以下推导部分主要参照VINS-Mono的论文（主要是附录部分）。

说明：

        有关公式截图中的一些文字，是我在阅读论文时加的注释，截图时懒得把它们去除了，不清楚的注释大家就当没有好了。

一般的IMU误差模型：

        对于理想的IMU，加速度计的3个轴和陀螺仪的3个轴定义了一个共享的正交3D坐标系。加速度计测量沿着每个坐标轴的加速度，陀螺仪测量围绕每个坐标轴的角速度。在实际的IMU内部，由于装配误差使得加速度计和陀螺仪的坐标轴发生了偏移，产生非正交误差。另外传感器的数字信号向物理信号转换的时候会产生误差，而且输出信号会受到非零，可变的零偏的影响。因此IMU的误差来主要来自于四部分，包括噪声(Noise)、零偏(Bias)、刻度因数(Scale errors)和轴偏移(Axis misalignments)。

• 加速度计和陀螺仪的误差可以分为：确定性误差、随机误差；
• 确定性误差可以提前标定，包括：bias,scale,misalignments；
• 随机误差通常假设噪声服从高斯分布：测量噪声、bias随机游走。

        加速度计和陀螺仪完整的误差模型可进行如下表示（其实这里的加速度可以把g加上，虽然不属于IMU本身的误差，但在很多算法中也是优化的对象）：

        I代表IMU参考坐标系，S代表各自非正交的传感器坐标系。T表示轴偏移的变换矩阵，K表示刻度因数，b、n分别表示零偏和噪声。其中：

推导过程：

        对于IMU测量数据：

        VINS-Mono中对IMU测量数据的建模：附加噪声满足高斯白噪声，偏置的导数满足高斯白噪声。

        IMU的连续时间积分：

         上述公式中的R_t^w是后面需要优化的变量，为减少积分次数，采用预积分的方法（两边乘上R_w^b_k转换到前一帧的本体坐标系中）。

         

        积分结果α、β、γ可以理解为bk+1对bk的相对运动量（预积分本质上是将变化量与绝对位姿解耦，这样积分里面的内容只跟IMU的测量量有关，而与被优化的状态量无关）。
       利用中值积分将其离散化（传播时假定加速度计零偏和陀螺仪零偏不变，优化后利用零偏最佳估计值的偏差进行更新），可得到：

        离散时间形式的IMU预积分公式：

         至此，每帧IMU数据预积分量就可以被计算出来了，但该值的置信度还未可知，VINS-Mono中后端优化部分采用误差卡尔曼对每帧预积分量的协方差矩阵进行传播。首先求解预积分量的误差项动力学公式：

        利用中值积分进一步可得到离散化的误差传递矩阵，是一种的形式：

        由误差传递矩阵，可得到

        协方差的迭代形式：

        该协方差矩阵，用于计算后端优化中IMU部分的信息矩阵。
        后端优化之后，可得到一个IMU加速度计零偏和陀螺仪零偏的最佳估计值。当零偏最佳估计值的偏差较小时，利用该值修正IMU预积分量。

        其中，雅可比矩阵的迭代推导：

        由于：

        与误差传递矩阵联立可得到，雅可比矩阵的迭代形式：