从EKF到MSCKF
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精度相当，速度更快，适合在计算资源有限的嵌入式平台运行。
MSCKF的目标是解决EKF-SLAM的维数爆炸问题。传统EKF-SLAM将特征点加入到状态向量中与IMU状态一起估计，当环境很大时，特征点会非常多，状态向量维数会变得非常大。MSCKF不是将特征点加入到状态向量，而是将不同时刻的相机位姿(位置 p p p和姿态四元数 q q q)加入到状态向量，特征点会被多个相机看到，从而在多个相机状态（Multi-State）之间形成几何约束（Constraint），进而利用几何约束构建观测模型对EKF进行update。由于相机位姿的个数会远小于特征点的个数，MSCKF状态向量的维度相较EKF-SLAM大大降低，历史的相机状态会不断移除，只维持固定个数的的相机位姿（Sliding Window），从而对MSCKF后端的计算量进行限定。
参考：
MSCKF那些事（一）MSCKF算法简介
学习MSCKF笔记——后端、状态预测、状态扩增、状态更新
MSCKF 学习资料整理
本文主要会对msckf与ekf的联系做一个大体的介绍，以便读者在掌握ekf思想的基础上更好地把握msckf的思路，不会涉及msckf内具体的数学推导，也不会像前几节一样对msckf的代码做具体展示，只在最后给出msckf的代码链接和使用指南。
MSCKF状态向量
EKF slam的状态向量：
X E K F S L A M = [ X I M U , f 1 , f 2 , . . . , f M ] X_{EKF SLAM}=[X_{IMU}, f_1, f_2, ..., f_M ] XEKFSLAM=[XIMU,f1,f2,...,fM]
表示的是，在状态向量中，分别包含了IMU的状态 X I M U X_{IMU} XIMU和特征点（也就是前文的路标点）的状态 f 1 . . . f M f_1 ... f_M f1...fM
MSCKF的状态向量：
X M S C K F = [ X I M U , p c 1 , q c 1 , p c 2 , q c 2 , . . . , p c N , q c N ] X_{MSCKF}=[X_{IMU}, p_{c1}, q_{c1}, p_{c2}, q_{c2}, ..., p_{cN}, q_{cN} ] XMSCKF=[XIMU,pc1,qc1,pc2,qc2,...,pcN,qcN]
其中的 p p p和 q q q分别表示了各个时刻的相机位置向量和姿态向量。
具体地，在MSCKF的状态向量中， X I M U X_{IMU} XIMU包含了：
X I M U = [ G I q , b g , G v I , b a , G p I , C I R , C I t ] X_{IMU}=[^I_Gq, b_g, ^Gv_I, b_a, ^Gp_I, ^I_CR, ^I_Ct] XIMU=[GIq,bg,GvI,ba,GpI,CIR,CIt]
其中：
- G I q ^I_Gq GIq 为单位四元数，表示从世界系( G G G系)到IMU坐标系( I I I系)的旋转
- b g b_g bg 为陀螺仪gyroscope的bias
- G v I ^Gv_I GvI 为IMU在G系下的速度
- b a b_a ba 为加速度计accelerator的bias
- G p I ^Gp_I GpI 为IMU在G系下的位置
- C I R ^I_CR CIR 为IMU到相机的旋转
- C I t ^I_Ct CIt 为IMU到相机的平移
由此我们可以看出，除了表示IMU在世界坐标系下的位姿速度以外，状态向量还估计了IMU的内参和IMU与相机的外参，都是为了使所得的结果更准确。
另外，在MSCKF算法中，历史的相机状态会不断移除，只维持固定个数的的相机位姿，使状态向量维持在固定长度。
与EKF不同的是，在MSCKF算法中，机器人的各种“状态”又被分为了“标称状态”和“误差状态”，即：
真实状态 = 标称状态 + 误差状态，其中，真实状态为考虑噪声影响的状态，标称状态为不考虑噪声影响的状态，而噪声影响的部分都被考虑进误差状态，因此真实状态由标称状态和误差状态两者共同描述。具体描述详见：学习MSCKF笔记——真实状态、标称状态、误差状态
MSCKF过程简述
首先回顾EKF的过程：
=== Predict ===
x P r e d = F x t + B u t x_{Pred} = Fx_t+Bu_t xPred=Fxt+But
P P r e d = J F P t J F T + Q P_{Pred} = J_FP_t J_F^T + Q PPred=JFPtJFT+Q
=== Update ===
z P r e d = H x P r e d z_{Pred} = Hx_{Pred} zPred=HxPred
y = z − z P r e d y = z - z_{Pred} y=z−zPred
S = J H P P r e d . J H T + R S = J_H P_{Pred}.J_H^T + R S=JHPPred.JHT+R
K = P P r e d . J H T S − 1 K = P_{Pred}.J_H^T S^{-1} K=PPred.JHTS−1
x t + 1 = x P r e d + K y x_{t+1} = x_{Pred} + Ky xt+1=xPred+Ky
P t + 1 = ( I − K J H ) P P r e d P_{t+1} = ( I - K J_H) P_{Pred} Pt+1=(I−KJH)PPred
对应以上的过程，我们来简要梳理一下MSCKF的过程：
-
通过运动模型，积分出IMU的标称状态的预测值；
-
计算IMU的误差状态的协方差矩阵预测值，计算相机误差状态相对于IMU误差状态的协方差预测值；
-
当一个新的相机位姿出现后，进行状态扩增：
根据第1步得到的IMU标称状态的预测值，计算新的相机位姿的标称状态的预测值，并对误差状态的协方差矩阵扩增；
-
对于每一个特征点，根据特征点在世界系下的空间位置、相机的标称状态的预测值，通过观测模型计算特征点的预测值；
-
用特征点的观测值减去特征点的预测值获得残差 y y y;
-
计算卡尔曼增益 K K K;
-
计算误差状态的更新值： K y Ky Ky
-
用误差状态的更新值加上标称状态的预测值，获得标称状态（注意：这里包括了IMU状态和相机状态）的更新值；
-
更新误差状态的协方差矩阵；
补充一点：在第4点中，如何获得特征点在世界坐标系下的空间位置？MSCKF的做法是根据历史相机位姿和观测来三角化计算特征点的3D坐标，即根据特征点在多个相机中的观测，利用最小二乘优化估计特征点的3D坐标。这又带来了一个问题：如何确保三角化的精度呢？如果三角化误差太大，那么观测模型就会不准，最终会使得VIO精度太差。MSCKF做法是当特征点跟踪丢失后再进行三角化，特征点跟丢表示该特征的观测不会再继续增加了，这时利用所有的历史观测三角化。所以MSCKF中观测更新的时机是特征点跟丢。
MSCKF代码链接
- python版本
stereo_msckf 根据主页指导安装前置要求包、下载数据集，即可运行测试。
- C++版本
原版链接：KumarRobotics/msckf_vio
附加注释的版本：msckf_vio_note