MSCKF那些事（一）MSCKF算法简介

转自：https://zhuanlan.zhihu.com/p/76341809

1. 什么是MSCKF?

MSCKF全称Multi-State Constraint Kalman Filter（多状态约束下的Kalman滤波器），是一种基于滤波的VIO算法，2007年由Mourikis在《A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation》中首次提出。MSCKF在EKF框架下融合IMU和视觉信息，相较于单纯的VO算法，MSCKF能够适应更剧烈的运动、一定时间的纹理缺失等，具有更高的鲁棒性；相较于基于优化的VIO算法（VINS，OKVIS），MSCKF精度相当，速度更快，适合在计算资源有限的嵌入式平台运行。在机器人、无人机、AR/VR领域，MSCKF都有较为广泛的运用，如Google Project Tango就用了MSCKF进行位姿估计。

VIO分类

VIO相对于VO的好处：

1. 恢复尺度：IMU能够提供尺度信息(加速度计)，解决单目VO的无法恢复尺度的问题。
2. 应对纯旋转：在纯旋转的情况下，VO位姿解算会出现奇异，VIO可以利用IMU的陀螺仪(角速度计)来估计纯旋转运动。
3. 应对短时间图像特征缺失：在出现图像过曝、图像过暗、环境纹理不足时，VO会无法工作，而VIO能在VO失效的情况下利用IMU积分来进行运动估计，能够应对短时间的视觉特征缺失（IMU积分越久累积误差会越大），比VO具有更高的鲁棒性。
4. 精度更高：VIO融合了两种传感器的信息，位姿估计的精度要更高。

2. MSCKF相关学习资料

• MSCKF开山之作

Mourikis A I, Roumeliotis S I. A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation​www-users.cs.umn.edu

 

• 数学理论相关资料

Indirect Kalman Filter for 3D Attitude Estimation​mars.cs.umn.edu

 

Quaternion kinematics for the error-state Kalman filter​www.iri.upc.edu

 

• Li Mingyang老师的系列论文
  • MSCKF2.0，FEJ

Improving the accuracy of EKF-based visual-inertial odometry​citeseerx.ist.psu.edu

 

• • 时钟同步误差估计

Online temporal calibration for camera-IMU systems​intra.ece.ucr.edu

 

• • 能观性分析，FEJ

High-precision, consistent EKF-based visual-inertial odometry​intra.ece.ucr.edu

 

• Shelly(TUM CVG)的硕士论文，IPhone上跑MSCKF，实现步骤很详细

Monocular Visual Inertial Odometry on a Mobile Device​vision.in.tum.de

 

• Vijay Kumar实验室在github上开源的双目MSCKF和配套论文

KumarRobotics/msckf_vio​github.comRobust Stereo Visual Inertial Odometry for Fast Autonomous Flight​arxiv.org

 

3. MSCKF核心思想

MSCKF的目标是解决EKF-SLAM的维数爆炸问题。传统EKF-SLAM将特征点加入到状态向量中与IMU状态一起估计，当环境很大时，特征点会非常多，状态向量维数会变得非常大。MSCKF不是将特征点加入到状态向量，而是将不同时刻的相机位姿(位置 和姿态四元数 )加入到状态向量，特征点会被多个相机看到，从而在多个相机状态（Multi-State）之间形成几何约束（Constraint），进而利用几何约束构建观测模型对EKF进行update。由于相机位姿的个数会远小于特征点的个数，MSCKF状态向量的维度相较EKF-SLAM大大降低，历史的相机状态会不断移除，只维持固定个数的的相机位姿（Sliding Window），从而对MSCKF后端的计算量进行限定。

3.1 IMU状态向量与INS

MSCKF本质是一个EKF，介绍MSCKF之前，我们先介绍一下INS(Inertial Navigation System)中的IMU状态EKF估计，INS中IMU的状态向量为

其中

• 为单位四元数，表示从世界系( 系)到IMU坐标系( 系)的旋转
• 为加速度计accelerator的bias
• 为IMU在G系下的速度
• 为陀螺仪gyroscope的bias
• 为IMU在G系下的位置

INS的EKF步骤为：

• EKF预测：先利用传感器获得的观测加速度和观测角速度，可以对状态进行估计，显然，该步骤会使得估计的不确定度/协方差越来越大
• EKF更新：然后利用GPS观测构建观测模型，对状态向量的均值和协方差进行更新, 修正预测过程的累积误差，减少不确定度。

3.2 MSCKF中的观测模型

对于MSCKF来说，EKF预测步骤与INS一样，区别在EKF观测更新，需要用视觉信息来构建观测模型，从而对IMU预测的状态进行更新。INS中GPS可以直接给出位置 的观测，而视觉通常只能提供多个相机之间相对位姿关系的约束。那观测模型要怎么构建呢？

视觉中，约束通常都是特征点到相机的重投影误差（空间中一个3D特征点根据相机的姿态和位置投影到相机平面，与实际观测的特征点之间的误差）：

我们希望用这个重投影误差的约束等式来作为观测模型，但前提是需要知道特征点的3D坐标，而实际应用中特征点的3D坐标是未知的。

• EKF-SLAM的做法是将特征点加入到状态向量进行估计，但它的状态向量会随特征点的增多而变得非常大。
• MSCKF的做法是根据历史相机位姿和观测来三角化计算特征点的3D坐标。这又带来了一个问题：如何确保三角化的精度呢？如果三角化误差太大，那么观测模型就会不准，最终会使得VIO精度太差。MSCKF做法是当特征点跟踪丢失后再进行三角化，特征点跟丢表示该特征的观测不会再继续增加了，这时利用所有的历史观测三角化。所以MSCKF中观测更新的时机是特征点跟丢。

4. MSCKF算法步骤

MSCKF算法步骤如下：

1. IMU积分：先利用IMU加速度和角速度对状态向量中的IMU状态进行预测，一般会处理多帧IMU观测数据。
2. 相机状态扩增：每来一张图片后，计算当前相机状态并加入到状态向量中, 同时扩充状态协方差.
3. 特征点三角化：然后根据历史相机状态三角化估计3D特征点
4. 特征更新：再利用特征点对多个历史相机状态的约束，来更新状态向量。注意：这里不只修正历史相机状态，因为历史相机状态和IMU状态直接也存在关系(相机与IMU的外参)，所以也会同时修正IMU状态。
5. 历史相机状态移除：如果相机状态个数超过N，则剔除最老或最近的相机状态以及对应的协方差.

MSCKF状态propagation和update的流程如下图所示：