VINS-Mono 代码详细解读——IMU预积分的残差、Jacobian和协方差
前言：
对第k帧和第k+1帧之间所有的IMU进行积分，可得到第K+1帧的PVQ（位置、速度、旋转），作为视觉估计的初始值。
每次qwbt优化更新后，都要重新进行积分，运算量较大。将积分模型转为预积分模型：
PVQ积分公式中的积分项变为相对于第i时刻的姿态，而不是相对于世界坐标系的姿态。
目录
一、IMU离散中值预积分理论基础
1、没预积分：PVQ连续、离散积分
1、IMU模型
2、连续时间IMU运动模型，积分 PVQ（两帧之间）
3、PVQ的中值离散积分（前后IMU）
2、预积分：PVQ连续、离散积分、预积分误差、bias 预积分量
1、 PVQ连续积分（预积分）
2、预积分量
3、 预积分中值离散形式
4、预积分误差
5、bias 预积分量（bias发生变化）
二、预积分的Jacobian和协方差
1、预积分的误差以及线性传递方程
1.预积分误差
2.相邻时刻误差的线性传递方程
3.预积分协方差矩阵
2、预积分的Jacobian和协方差
1、预积分的Jacobian（rv、rq）
2、预积分的协方差
一、IMU离散中值预积分理论基础
1、没预积分：PVQ连续、离散积分
1、IMU模型
测量值：加速度计
、陀螺仪
, 加上了bias游走
和随机白噪声
。
真实值：加速度计a、陀螺仪w。
实际情况下，可以获得测量值a^和w^，需要反推真实值。一般忽略随机游走高斯噪声n
w=-bg; a=qwb(-ba)-gw;
2、连续时间IMU运动模型，积分 PVQ（两帧之间）
将第k帧和第k+1帧所有的IMU进行积分，可得到第k+1帧的 PVQ，作为视觉估计的初始值。
和
是IMU测量的加速度和角速度，相对于Body坐标系。
world坐标系是IMU所在的惯导系。Body系和惯导系关系为：
3、PVQ的中值离散积分（前后IMU）
从第 i个IMU时刻到第 i+1个IMU时刻的积分过程。两个相邻时刻k到k+1的位姿是由第k时刻测量值a^,w^计算得出的。
这与Estimator::processIMU（）函数中Ps[j]、Rs[j]、Vs[j]是一致的，代码中j就是此处的i+1
IMU积分出来第 j 时刻数值作为第 j 帧图像初始值。
欧拉法
中值法
2、预积分：PVQ连续、离散积分、预积分误差、bias 预积分量
1、 PVQ连续积分（预积分）
每次qwbt优化更新后，都要重新进行积分，运算量较大。
将积分模型转为预积分模型：
PVQ积分公式中的积分项变为相对于第i时刻的姿态，而不是相对于世界坐标系的姿态
2、预积分量
IMU预积分量只与IMU测量值有关。（加速度计、陀螺仪）
3、 预积分中值离散形式
中值法：k到k+1时刻位姿由两时刻的测量值a w的平均值来计算。
4、预积分误差
一段时间内IMU构建的预积分量作为测量值，与估计值进行相减。
5、bias 预积分量（bias发生变化）
因为 i 时刻的 bias 相关的预积分计算是通过迭代一步一步累计递推的，可以算但是太复杂。所以对于预积分量直接在 i 时刻的 bias 附近用一阶泰勒展开来近似，而不用真的去迭代计算。
二、预积分的Jacobian和协方差
1、预积分的误差以及线性传递方程
预积分模型减小了计算量，但是丢失了一些东西。比如当我们用1个结果代替100个数据点时，转化之前100个点每一个点的不确定度是知道的（IMU数据作为测量值的噪声方差能够标定），但是转化后的1个结果不确定度不知。100个数据积分形成的预积分量方差是多少？这就需要我们在得到IMU预积分后，推导预积分量的协方差，需要直到IMU噪声和预积分量之间的线性递推关系。
IMU在每一个时刻积分出来的值是有误差的，下面对误差进行分析。
1.预积分误差
一段时间内IMU构建的预积分量作为测量值，与估计值进行相减。
上面误差中位移，速度，偏置都是直接相减得到。第二项是关于四元数的旋转误差，其中 [·] xyz 表示只取四元数的虚部 (x,y,z) 组成的三维向量。
2.相邻时刻误差的线性传递方程
误差的传递分为两部分：1）当前时刻误差传递给下一时刻；2）当前时刻测量噪声传递给下一时刻。
状态量误差为：
测量噪声为：
3.预积分协方差矩阵
协方差矩阵矩阵通过递推为
其中，
是测量噪声的协方差矩阵，方差从i=0时刻开始递推。
F和G是两时刻间的协方差传递矩阵，
表示各时刻的误差。
2、预积分的Jacobian和协方差
两帧之间PVQ和bias变化量的残差为：
残差是15维的，状态量是7+9+7+9维的，所以说，每增加一个IMU约束，总的Jacobian矩阵增加了15行，增加了7+9+7+9列！(需要注意的是，四元数是4维的，但是Localdemension是3维的)。
1、预积分的Jacobian（rv、rq）
优化变量主要包括第i、j时刻的p、q、v、ba、bg：
残差对状态量的Jacobian是什么，对应位置补充上这个J的矩阵块就行了，其他位置还是0。
VINS源码中，在imu_factor.h中的class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9> 的函数virtual bool Evaluate()中实现，其中parameters[0~3]分别对应了以上4组优化变量的参数块。对于 Evaluate 输入 double const *const *parameters, parameters[0], parameters[1], parameters[2],parameters[3]分别对应 4 个输入参数, 它们的长度依次是 7,9,7,9，
方式1：整个残差对某个变量求导；
例如：
指的是
主要的四部分（参考崔华坤笔记）为：
方式2：推导关于第i、j帧PVQ的Jacobian（以rv和rq为例）
方式为：
、
、
、
、
残差某一行对5个变量分别求导。
下面为详细推导，如果不感兴趣，可以直接记住上面方式1的四部分即可。
- rv
由于 rp 和 rv 的误差形式很相近，对各状态量求导的 Jacobian 形式也很相似，所以这里只对 rv 的推导进行详细介绍。
- rq
2、预积分的协方差
