MSCKF2.0(Mingyang Li-IJRR2013) 论文要点总结

论文：Li M, Mourikis A I. High-precision, consistent EKF-based visual-inertial odometry[J]. The International Journal of Robotics Research, 2013, 32(6): 690-711.

摘要

本篇论文的主要工作包括：

1. 分析和比较MSCKF和几种传统EKF-SLAM方法的理论基础和性能精度；
2. 证明了现有的MSCKF和EKF-SLAM的状态估计过程均是不一致（inconsistent），并给出原因是量测雅各比计算方式不对；
3. 提出一种新的、实时的、能够实现一致估计的EKF-VIO算法（MSCKF2.0），主要贡献包括：1）确保了建立的线性系统模型正确的观测性；2）对相机-IMU的外参进行在线估计校正。

引言

文中主要对以下三类VIO估计器进行分析和比较：
1）EKF-SLAM：滤波状态里包含当前的IMU pose、相机观测到的特征点 pose；
2）滑动窗口算法：滤波状态里只包含一个不同时刻pose构成的滑动窗口，相机观测到的特征点被用来在pose间构造概率约束；
3）MSCKF：滤波状态里只包含多个IMU的pose，此被证明是一种线性化系统的最大后验估计。

在文中，经过蒙特卡洛仿真和实际环境测试表明，MSCKF方法在精度、一致性和计算效率方面均大大优于EKF-SLAM方法。作者认为会产生这种优势的原因是在于MSCKF方法未对特征点的位置进行高斯假设，但是在EKF-SLAM中对特征点位置进行了高斯假设。

此外，经过分析总结了MSCKF产生不一致的两个因素：
1）IMU相对重力方向的旋转（yaw角）在VIO中是不可观测的，但是在MSCKF和EKF-SLAM的线性化系统模型里表现出来确实可观测的。因此在实际中，估计器错误的认为它有更多的观测信息，故而导致低估状态的协方差矩阵，所以此时实际输出的状态协方差并不能真正反映状态量的协方差。
2）相机和IMU间的外参（旋转和平移）被认为是一个预知的较优参数。但是，并且通常，这些标定参数被估计器误认为是精确的，所以导致标定模型中未建模的不确定因素导致状态协方差的再次低估。

为了提高MSCKF的的一致性，文中对上述两个问题进行了分析和解决：
1）在滤波器雅各比计算过程中修复了滤波器线性化系统模型的观测性。此外，还推导了一个闭合的IMU误差状态转移矩阵；
2）将camera-IMU间的外参数包含在了MSCKF的状态向量里，随着IMU状态进行一起在线估计。

作者将更改后的MSCKF算法称为MSCKF2.0，与EKF-SLAM相比，其具有更高的一致性、精度、计算效率。甚至可以与使用滑动窗口迭代最小化的算法进行比较，但是计算效率更高。通过分析和论证，作者认为，对于非线性测量模型，使建立的线性系统模型的观测性更加匹配可能比重新线性化更加重要。

相关工作

松耦合VIO：将IMU和图像测量进行单独处理。
优点：计算消耗少，特别适合于计算资源有限的系统；
缺点：会引起信息损失，比如在图像对间使用特征测量进行位姿估计忽视了连续时间间的相关性，以及单独处理IMU测量并不能确保传感器零偏的最优估计。

文中主要贯注于紧耦合方法，即直接融合视觉和惯性数据，从而实现更高的精度。经过分析表明，一个设计适当EKF估计器可以得到比迭代最小化更高的性能，并且计算资源消耗更小。

最常见的紧耦合EKF估计器是EKF-SLAM，在这种方法里当前相机的pose和特征点位置被进行联合估计。缺点是计算量大，与状态向量里的特征点数量的三次方成正比。甚至在某些时候，比迭代最小化的计算消耗更大。为了解决计算量大的问题，MSCKF应运而生，其在状态向量里维护一组pose的滑动窗口，使用相机测量的特征点在这些pose间建立约束，这样算法的复杂度只与特征点数量成线性关系。而且经过作者的分析和比较，证明MSCKF比EKF-SLAM更快、精度更高，一致性更好。

文中的重点贡献工作是分析和提高基于EKF的视觉惯性系统的一致性。过去在3D视觉定位的一致性中重点关注于特征点位置的参数化。（Civera et al., 2008）已经表明笛卡尔座标系（XYZ）参数会对特征点的估计导致严重非高斯属性，从而降低精度和一致性，所以提出了一种逆深度特征参数化方法，其可以更好的适配相机的测量模型，并提高性能。（Sola, 2010）提出一种固定齐次特征参数化方法，表明可以进一步提升滤波器一致性。文中对以上三种特征参数化方法进行了测试，尽管固定齐次特征参数化优于其他方法，但是其性能依旧弱于MSCKF算法（MSCKF中使用的是齐次座标系下的逆深度特征参数化）。

与之前其他的论文相比，作者有以下方面的不同：
1）比较了MSCKF和其他EKF-SLAM方法的性能；
2）发现了影响EKF-SLAM方法的相同的，错误的观测性问题；
3）解决了由于预知的不精确的相机-IMU外参标定参数导致的不一致问题；
3）进行了大规模的仿真和实验进行分析和比较。

iii 基于EKF的视觉惯性里程计

本部分主要描述了文中使用的IMU状态的参数化过程，并讨论了两种主流的紧耦合EKF-VIO公式，同时比较了它们的性能。

A. IMU状态的参数化

• 估计的global系下的位移误差
  
• 估计的global系到IMU系的旋转误差

上式中$\delta{q}$表示需要将估计的global座标系旋转到真实global座标系的旋转小量。
上式中$\delta\theta$是一个3*1的表示global系下三个座标轴的姿态误差。

• IMU误差状态描述

上式表示IMU的15*1维的误差状态向量。文中姿态误差的选择方式基于（Li and Mourikis, 2012a）论文，此分析指出如上将姿态误差定义为真实的global系和估计的global系间的误差要优于定义为真实的IMU系和估计的IMU系间的误差。需要指出的是，后者（比如在Mourikis and Roumeliotis 2007中所用）在EKF的线性系统的观测模型中引入了额外的误差项 ？。

B. EKF-SLAM

• 滤波器的状态包含当前IMU状态$x_{Il}$, 及多个特征点的位置
  
  本文中，特征点的位置会考虑以下三种参数化方式：1）传统的XYZ座标参数化；2）逆深度参数化（Civera et al., 2008）; 3）固定齐次参数化（Sola. 2010）。这是在实际中比较常用几种特征点位置参数化形式，后两种的目的主要是为了提升滤波器的一致性和精度。

• 滤波器的状态更新方程、状态协方差矩阵计算与常规EKF算法一致
  
  

• 特征点 i 在时刻 $l$ 的观测模型
  
  在EKF-SLAM中，特征点观测被直接用来进行状态估计，此过程使用实际特征点位置和期望的特征点位置间的残差建立, 线性化估计形式为：
  
  上式中的$H_{il}$是$h$相对于滤波状态的雅各比矩阵在状态估计$\hat{x}_{l|l-1}$处的值。这是一个稀疏矩阵，只在IMU状态处和第i个特征点处包含非零块，其他位置均为0。
  
  一旦$r_{il}$和$H_{il}$计算完成, 会进行一个马氏门限测试？，如果通过，则进行标准EKF等式的更新（Maybeck, 1982）。

  C. MSCKF

  与EKF-SLAM相比，MSCKF维护一个poses的滑动窗口作为状态向量，使用特征点观测在这些poses之前构造概率约束。在时间l下的状态向量为：
  
  上式中$\pi_i =[{q_G}^{I_i} p_i^G]^T$， i= l-N, …, l-1，此为过去N个图像被记录时刻的imu的poses。
  
  算法流程：

• 状态传播：基于IMU输出进行状态向量和协方差更新；

• 测量更新：当一次图像被记录时进行更新；
  1.状态增广：基于记录图像时刻的imu位置和姿态对滑动窗口内的状态向量和协方差变量进行增广；
  2.图像处理：角点特征提取，特征匹配；
  3.更新：对每个特征进行跟踪，并计算$r_i^o和H_i^o$，以及进行马氏门限测试。使用通过测试的所有特征进行一次EKF更新；
  4.状态管理：移除所维护状态量滑动窗口里与所有已经处理过的特征相关的imu的pose状态。

D EKF-SLAM与MSCKF对比

• 所有的EKF-SLAM都假设imu状态和特征点的误差在每个时间都是联合高斯分布。但是，由于相机测量模型的非线形，此估计不是很好，尤其是XYZ参数话特征点的时候。通过优化视觉特征参数化形式，比如使用逆深度参数化IDP和固定齐次参数化可以提高EKF-SLAM的精度和一致性，但是这些方法仍旧显著表现差于MSCKF。这是因为在MSCKF中特征点不包括在状态量里，所以不存在特征点的误差假设，能够避免一部分较大的估计不确定性误差源；
• 在EKF-SLAM中，每一次都进行特征测量线性化和处理。然而，在MSCKF中，使用了一个延迟线性化的方法：其只有当所有测量都可用时才处理一次特征（多状态约束）。这意外着在计算雅各比时能够实现更加精确的特征估计，从而得到更加精确的卡尔曼增益，并提高修正精度，达到更好的性能。

MSCKF 2.0

A 增强不可观测子空间的修正维度

在MSCKF中相同状态的不同估计量被用来进行计算雅各比，导致了yaw角融合时引进了虚假的信息。修正这个误差的方法是，保证在涉及雅各比计算的时候，只使用imu位置和姿态的一组估计量。此处总是使用每一个状态的第一次可用的估计值进行雅各比计算。

B 进行在线camera-imu标定

将camera-imu间的转换矩阵包含在滤波器状态里，进行在线估计修正。

测试结果

1. 与msckf比较

2. 与迭代优化slam比较

3. camera-imu在线标定性能

4. 计算性能

MSCKF2.0在i7-2.66GHz的处理器上运行单线程c++代码，每次更新（包含图像处理和估计）是10ms。