VINS-MONO翻译以及理论推导系列(一)：IV测量预处理

                                                          IV：测量预处理

该部分描述惯性以及单目视觉测量预处理步骤。对于视觉测量，我们追踪连续帧间的特征点以及在最近的帧中检测新的特征点。对于IMU测量，对两连续帧进行预计分。注意到我们使用的廉价的IMU，其测量值会受到偏置和噪声的双重影响。因此我们在IMU预计分的过程中特别考虑偏置。

A.视觉处理前端

对每一帧新的图像，KLT稀疏光流算法[29]对现有的特征点进行跟踪.同时，检测新的角点特征[30]以维持每帧图像特征最低数目（100-300）。检测器通过设置两相邻特征点的最小像素间隔来保证统一的特征点分布。首先对2D特征点去畸，然后在去除外点之后将其投影到单位球面上。使用基础矩阵模型[31]的RANSAC去除外点.

关键帧也在该步骤中选取。我们有两个选取关键帧的标准。一种是平均视差与前一帧差距较大。如果最近关键帧与当前帧之间被追踪的特征点的平均视差大于某一阈值，我们认为当前帧为新的关键帧。旋转和平移都可以引起视差。然而，由于特征点在纯旋转的运动下没有办法被三角化。为避免这种情况，我们在计算视差时使用陀螺仪测量值的短期积分来补偿旋转。注意，这种旋转补偿只用在关键帧选取，不会用在VINS框架中的旋转计算。那么即使陀螺仪包含很大的噪声或者是偏置，只会导致次优的关键帧选择结果，不会直接影响估计的质量。另外一个标准是追踪的质量。如果追踪的特征点低于某个阈值，我们会把该帧作为新的关键帧。这种准则避免完全丢失特征追踪。

B.IMU预积分

IMU预计分在文献[22]首次提出, 它将欧拉角的旋转误差参数化。在我们之前的工作中[7]，我们提出了一种流形上的IMU预计分旋转公式。该工作使用连续时间IMU误差状态动力学推导协方差传递函数。但是忽略了IMU的偏置。这种预计分原理在文献[23]中通过添加后验IMU偏置矫正进行改进。本文中，通过包含IMU偏置矫正对之前工作[7]中提到的IMU预计分进行拓展。原始的陀螺仪和加速度计测量值和如下：
　　　　　　　　　　　

IMU测量值是在本体坐标系中测量的，它是平衡重力和平台动力的合力，并受到加速度偏置陀螺仪偏置和附加噪声的影响。我们假设加速度、陀螺仪测量的噪声是高斯白噪声，,。加速度计偏置和陀螺仪偏置被建模为随机游走，其导数为高斯白噪声，,：
　　　　　　　　　　　

       给定对应于体坐标系和的两个时刻，位置、速度和方向状态可以在时间间隔间，在世界坐标系下中通过惯性测量值传递：
　　　　　　　　　　　

是时间间隔之间的持续时间。

公式（3）中对旋转的推导：

可见，IMU状态传递需要坐标系的旋转、位置和速度。当这些起始状态改变时，我们需要重新传递IMU测量值。特别是在基于优化的算法中，每次调整位姿时，都需要在它们之间重新传递IMU测量值。这种传递策略在计算上要求很高。为了避免重新传递，我们采用了预积分算法。

将参考坐标系从世界坐标系转变为局部坐标系后，我们只能对线性的加速度和角速度相关的部分进行预积分，如下所示：
 

可以看出预积分项(6)能通过将视为参考帧的IMU测量值单独得到。、、​只与和中的IMU偏置有关，与其他状态无关。当偏置估计发生变化时，若偏置变化很小，我们将、、​按其对偏置的一阶近似来调整，否则就进行重新传递。这种策略为基于优化的算法节省了大量的计算资源，因为我们不需要重复传递IMU测量值。

对于离散时间的实现，可以采用不同的数值积分方法，如欧拉积分、中点积分、RK4积分等。这里选择了欧拉积分来演示易于理解的过程(我们在代码中使用了中点积分)。

在开始时，、是0，​​是单位四元数。，β，γ在(6)中的平均值是如下逐步传递的。注意，增加的噪声项，是未知的，在实现中被视为零。这得到了预积分的估计值，标记为：

是在中IMU测量值对应的离散时刻，是IMU测量值和之间的时间间隔。

然后讨论协方差传递问题。由于四维旋转四元数被过参数化，我们将其误差项定义为围绕其平均值的扰动：

其中是三维小扰动。

我们可以导出误差项的连续时间线性化方程(6)：

可以通过初始协方差的一阶离散时间协方差更新递归计算：

其中是噪声的对角线协方差矩阵.

同时，的一阶雅可比矩阵相对于也可以用初始雅可比矩阵递归计算。

利用这个递推公式，得到协方差矩阵和雅可比矩阵。关于偏置的一阶近似、、​可以写为：

其中是中的子块矩阵，其位置对应于..也使用同样的含义。当偏置估计发生轻微变化时，我们使用(12)近似校正预积分结果，而不重新传递。

现在我们可以写下IMU测量模型所对应的协方差​：

参考：

1.https://blog.csdn.net/pancheng1/article/details/81008081的论文翻译。

2.崔华坤的《VINS代码推导以及代码解析》（注：该文档已经在本博主的上传资料中上传，希望对大家有帮助）