MSCKF理论推导与代码解析

计算机视觉工坊 | 2020-11-01 21:46:34 阅读：1964

在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速度，设定了速度协方差、陀螺仪偏置协方差、加速度计偏置协方差、以及外参旋转协方差和外参平移协方差，同时，还定义了整个系统状态协方差的初始化，以及坐标系之间的转换关系。