转载自：https://mp.weixin.qq.com/s/MECuRKUw2F5LtelqLK-now
原创 魏宏宇 3D视觉工坊 2020-07-31
点击上方“3D视觉工坊”，选择“星标”
干货第一时间送达
在SLAM后端中，主要有两种主流方法用于优化：基于滤波的方法和基于非线性的方法。基于滤波的方法主要有MSCKF、S-MSCKF、ROVIO等，基于非线性的方法主要有OKVIS、VINS-MONO、VINS-Fusion等。在这一节，主要分析S-MSCKF的理论推导和代码解读。
一、扩展卡尔曼滤波（Extended Kalman Filter）
MSCKF的全称是Multi-State Constraint Kalman Filter，意为多约束状态卡尔曼滤波器。那么什么是卡尔曼滤波器（KF）？
通俗来讲，卡尔曼滤波器是根据当前状态，预测估计下一状态的算法。卡尔曼滤波器方法在一定程度上架设了马尔可夫性，也就是k时刻的状态只与k-1时刻的状态有关。卡尔曼滤波器主要解决线性化问题，而将卡尔曼滤波器的结果扩展到非线性系统中，便形成了扩展卡尔曼滤波器（EKF）。
从k-1时刻到k时刻，存在系统的状态预测方程和系统的状态观测方程：
相比于非线性优化，EKF形式更为简洁，应用较广泛，除了EKF外，还有其他滤波器在SLAM中发挥作用，如IF（信息滤波器）、IKF（Iterated KF）、UKF（Unscented KF）和粒子滤波器等。
二、S-MSCKF理论推导与代码分步解读
MSCKF是出自2007年在ICRA会议上发表的A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation，主要是构造了MSCKF系统，针对单目相机实现，但原作者并没有将代码进行开源，github上仍有很多学者进行了仿真，感兴趣可前往github自行寻找。今天分享的这篇是Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight，基于双目的MSCKF，简称S-MSCKF。在单目的基础上扩增了双目摄像头，而且作者进行了代码开源，相比MSCKF更有助于理解论文。
MSCKF设定了两个状态向量：IMU的状态向量和相机的状态向量。IMU的状态向量为：
其中，和分别是陀螺仪和加速度计的偏置，其导数服从高斯白噪声分布。而由于误差表示为：,同时,因此，IMU的误差状态表示为：
k时刻时，假定有N个相机位姿在EKF的状态向量中，则，此时，状态向量表示为：
误差状态向量为：
S-MSCKF代码中，首先对IMU的偏置、噪声进行了初值定义：
对于连续时间模型中，IMU的状态更新：
基于上式，IMU误差状态可表示为：
在论文的附录中已给出了F和G的公式，我们在这边进行简单的推导：
其中， 和属于随机噪声，而和是相机和IMU外参的导数，误差不变，导数为0.
IMU采样和的信号，周期为T，在EKF中这些量主要用于状态传播，每次收到新的IMU测量量，均使用IMU状态估计传播方程的五阶/四阶Runge-Kutta积分传播IMU状态估计。同时，EKF的协方差矩阵也被传播。在这里，给出论文中没有详细说明的IMU状态更新，对于IMU状态中的P,V,Q来说，P和V的状态更新是通过Runge-Kutta四阶来进行更新，Runge-Kutta公式详细如下：
对于P和V来说，更新公式如下图：
而Q的更新是假设匀速运动，用角速度与时间相乘：
协方差的更新中，先对协方差矩阵进行划分：
其中，为21*21的IMU状态协方差，为6N*6N的相机位姿估计协方差，是IMU状态误差与位姿估计之间的相关性。由于在一开始已假设所有状态量和噪声都是服从高斯分布，因此只需要关心协方差的更新即可。
协方差更新如下：
在代码开始初始化部分，对进行了初始化预设：
同时对连续噪声协方差Q进行了初始化预设：
量测模型中，根据失去跟踪的特征点建立残差方程：
通过平价空间法将投影至的左零空间，使得:
三、S-MSCKF代码分析
S-MSCKF的代码分为两部分，图像处理部分(image_processor.cpp)和滤波部分（msckf_vio.cpp）。
【图像处理部分】
图像首先进行初始化操作，从image_processor_nodelet.cpp进入image_processor.cpp
Initialize()函数中先通过loadParameter函数进行相机内外参数的加载，后进入createRosIO()，创建ROS IO，在此函数中，定义两个回调函数：stereoCallback和imuCallback。当有图像传入时，调用stereoCallback函数，进行双目图像的处理，imu数据传入时，调用imuCallback，缓存Imu数据。
主要梳理stereoCallback函数。
双目相机的左右图像传入stereoCallback函数，接着，建立图像金字塔（createImagePyramids()），检测是否是第一张图像，如果是第一张图像，对第一张图像进行图像初始化（initializaFirstFrame()），并提取特征点、进行匹配，通过drawFeaturesStereo()在图像中画出特征点。如果不是第一张图像，首先通过trackFeatures()函数对上一张图像进行特征跟踪，再通过addNewFeatures()加入当前图像中新检测的特征，同样使用drawFeaturesStereo画出图像中的特征点。
Image_process部分的代码较为简单，不用过多分析。接下来主要分析msckf_vio的相关代码，这部分的代码分析和论文较为对应。
和图像处理部分一样，msckf部分从msckf_vio_nodelet.cpp进入initialize(),进入函数后，先进行参数加载，这部分主要有定义坐标系、加载imu的噪声和偏置噪声参数，定义了初始的imu速度，设定了速度协方差、陀螺仪偏置协方差、加速度计偏置协方差、以及外参旋转协方差和外参平移协方差，同时，还定义了整个系统状态协方差的初始化，以及坐标系之间的转换关系。
进入createRosIO(),创建里程计，进行位姿解算。在这一节中，定义两个回调函数，imuCallback和featureCallback，其中，imuCallback的作用仍然是将imu数据输入缓存区间。
featureCallback是整个代码的核心部分，也是最不好理解的部分。
对进行更新后，更新协方差矩阵，先根据论文，定义和
经过化简，可得到：,
根据附录：
扩充方法如下：
扩充后，协方差矩阵的维数将从21*21扩增为27*27，每次扩增增加6维。
以上部分是process model全部内容和代码，接下来，进入量测模型（measurement model）,在这部分使用双目相机中观测到的特征位置计算残差。
对应代码removeLostFeatures()函数。由于：
设定为观测到特征点的相机个数，维数为，其左零空间的维数为，因此是的向量。
在featureJacobian函数中定义4*6的,4*3的以及4*1的向量,对应附录中的（measurementJacobian()）：
计算出和后，进入measurementUpdate(),进行量测更新，首先计算卡尔曼增益：
然后是更新IMU状态：
更新相机状态：
最后更新状态协方差：
至此，完成了整个系统状态更新。
【结束语】
MSCKF是视觉惯性SLAM上的一个经典里程碑，也有很多工作基于MSCKF做了改进，如2019年CVPR会议上发表的SEVIS,后续也会进行讲解分析。
本文仅做学术分享，如有侵权，请联系删文。
