ORB_SLAM2视觉惯性紧耦合定位技术路线与代码详解1——IMU流型预积分

2.基于流型的IMU预积分
这一部分主要参考论文（2）中的内容，ORB_SLAM2融合惯性数据的算法中预积分处理是参照论文（2）中实现的。前面关于VINS的详解说过预积分计算过程，那篇使用的是四元数旋转求解，和这里的过程是一样的，主要计算三个量，分别是IMU预积分值，预积分测量值的协方差矩阵以及残差的雅各比矩阵，各自用途就不多说，有不懂的可以参照VINS的详解
https://blog.csdn.net/wangshuailpp/article/details/78461171
（1）IMU预积分计算
世界坐标系W、IMU坐标系B以及相机坐标系C的关系和转换如下图2所示，所有的位姿和特征点坐标最终都是在世界坐标系下表示，我们会将相机坐标系下的的状态量首先转换到IMU坐标系下然后再转换到世界坐标系下。TBC代表在IMU坐标系B下相机到IMU之间的外参转换矩阵，TWB=(RWB,wp)代表世界坐标系W下IMU到世界坐标的转换矩阵。关于坐标之间的关系比较重要，刚接触会感觉比较复杂，需要多思考。下面所有的公式和变量前下标代表参考坐标系，例如wp就是世界坐标系下的位移，后下标第一个下标是参考坐标系，例如RWB是世界坐标系下。

由IMU的加速度计和陀螺仪可以获得IMU坐标系下的加速度Ba和角速度BwWB，可以表示成下面的公式（27）（28），公式左边是IMU直接输出的数据。
旋转、平移和速度的导数可表示为公式（29）

世界坐标系下的旋转、平移和速度可由一般的积分公式求得

离散时间下采用欧拉积分可以将上面连续时间积分改写成下面式（30）

联立公式（27）（28）可以得到公式（31）

这时候状态量是世界坐标系下的。设现在有i和j两个相邻关键帧，需要已知i时刻对应的IMU状态量，求解离散时间下j时刻关键帧对应的IMU预积分值，


为避免重复积分采用IMU预积分方法计算i、j时刻的状态量的相对变化。也就是将参考坐标系由世界坐标系转换到IMU的i时刻坐标系下，计算得到IMU状态量在i和j时刻之间的相对量，这个相对量是只取积分算式内的值，而不是旋转、速度和位移的变化量，和VINS的处理方式相同。

从上面的公式中可以看出，整个预积分公式和偏置，噪声都有关系，相当于一个二元方程，因此需要分别分析偏置和噪声对系统的影响，噪声服从高斯正态分布，相对来说比较简单，而偏置服从随机游走模型，关系很复杂，因此两者的处理方式不同。对于噪声分量采用噪声高斯传播方程来求解，而偏置采用一阶泰勒展开方式。总体思路是首先将偏置在i时刻的bi处进行一阶展开，得到公式（44）

这里的数值就是IMU最终的预积分数值，但值得注意的是在初始化使用的IMU预积分是没有偏置的一阶项，也就只是等式右边前面一项而已，等式后面的雅各比矩阵是在初始化完成后进行优化之前才计算而来，得到IMU准确的预计分值（真值），和估计值从而得到IMU的残差。所以我们首先要手推一下上面公式右边的前面一项，注意这里所有的指数映射中都省去了写反对称符号。


至此IMU预积分的数值就计算出来，也就是等式右边的第一项。
（2）预积分雅可比和协方差矩阵
下面需要计算出噪声传播项的协方差矩阵和对应偏置的雅各比矩阵。这部分推导在附录部分A和B，和VINS的过程出奇的一致，只不过这里是将状态量分开讨论的，分别是（deta_Rj,deta_vj,deta_pj）和（deta_Rj-1,deta_vj-1,deta_pj-1）、（nba,nbg）以及（deta_Rj,deta_vj,deta_pj）和（deta_ba,deta_bg）的关系，把两者合并就和VINS相同。，这部分推导相对简单就直接贴公式了。
下面是求协方差矩阵：




系统是服从高斯分布的，协方差矩阵是按照高斯分布计算得到，是个9*9的矩阵。
下面是求预积分对偏置的雅各比矩阵：


（3）残差雅可比



位移残差对应的雅可比如上图所示，J时刻相应的雅可比可按照图片方式进行推导，注意对pi和pj时刻的雅可比公式原文有错误，推导过程已经给出。对于速度残差和旋转的雅可比公式与平移相似，就不进行推导直接给出，至此IMU预计分就完成了。