论文笔记:A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation(MSCKF)

摘要

这篇论文提出了一种基于扩展卡尔曼滤波(EKF)的实时视觉辅助惯性导航算法。这项工作的主要贡献是推导出一个测量模型，该模型能够表达从多个相机姿态观察静态特征时产生的几何约束。该测量模型不需要将三维特征位置包含在状态向量EKF中，并且线性化误差范围里是最优的。我们提出的视觉辅助惯性导航算法，其计算复杂度仅为特征量的线性，能够在大规模的真实环境中进行高精度的姿态估计。大量的实验结果证明了该算法的性能，包括在城市区域内定位的摄像机/IMU系统。

估计量的描述

文章提出的基于EKF的估计量的目标是跟踪IMU固定帧{$I$}相对于全局参考帧{$G$}的三维姿态。为简化地球自转角速度对IMU测量的影响，本文选择全局坐标系是以地球为中心的固定地球坐标系($ECEF$).。
算法1概述了该算法。IMU测量数据一可用就立即处理，以传播EKF状态和协方差(cf. Section 2)。另一方面，每次记录图像时，将当前相机姿态估计值附加到状态向量中(cf. Section 3)。状态增强是处理特征测量的必要条件，因为在EKF更新过程中，每个被跟踪特征的测量值被用来在所有看到特征的摄像机姿态之间施加约束。因此，在任何时刻，EKF状态向量包括(i)IMU状态$X_{IMU}$和(ii)最大$N_{max}$个过去相机的位姿。下面，我们将详细描述算法的各个组件。

1. EKF状态向量的结构

1.1 IMU状态向量的表示

在高斯白噪声矢量$n_{wg}和n_{wa}$的驱动下，将IMU偏差建模为随机游走过程。

1.2 IMU的误差状态

其中，对于速度、位置和偏差采用标准的误差定义：

对于单位四元数的误差定义：

误差四元数的定义：

1.3 EKF状态向量

假设在时刻$k$的EKF状态向量中包含N个摄像机姿态，则该EKF状态向量为:

1.4 EKF误差状态向量

2. Propagation

通过对连续时间IMU系统模型进行离散化，得到滤波传播方程，具体如下:

2.1 连续时间系统模型

$w_m和a_m$分别表示陀螺仪和加速度计的测量值，其表示为：


连续IMU状态估计值的递推公式：


所以，IMU误差状态的线性连续时间模型为：



注意，$I$是3*3的矩阵。

2.2 离散时间实现:

IMU以时间周期$T采样{\omega }_m和a_m$,这些测量值用在EKF卡尔曼滤波器中进行状态预测更新。每接收一次IMU测量值时，利用五阶龙格-库塔方程（公式9）进行数值积分来更新IMU状态估计。(9).此外，还要同步更新EKF协方差矩阵。为此，采用下列分块矩阵来描述和计算协方差：

3. 状态增广

当记录一帧新图片时，基于IMU的位姿估计可以计算相机的位姿估计量：

4. 测量模型

用于使用EKF进行状态估计的系统测量模型。首先定义一个残差$r$，其与状态误差X^~成正比，形式如下：
在本篇论文中，作者根据每个跟踪到的特征将多个相机测量打包成一组观测，（不是使用每个相机作为观测，需要计算两两位姿之间的约束关系）。所有这些相同的3D特征点的相机测量值被用来定义一个约束等式，将所有相机位姿联系起来。如此便实现了在滤波器状态向量里不包含特征点位置。

测量模型描述的情况为：一组$M_j$个相机位姿$(q,p)$观测到同一个单独的特征$f_i$。每一个特征的$M_j$观测可以通过下面的模型描述：


一旦得到特征位置的估计值，便可以计算测量的残差：

$Z_i$可由相机的测量值直接得到。

线性化公式（20）中对相机位姿和特征位置的估计，可得：

5. EKF更新

EKF的更新阶段中使用了上文中提到的来自观察多个特征的约束。
EKF更新由以下两个事件触发:
① 当在许多图像中跟踪的特征不再被检测到时，将使用第4部分介绍的方法处理该特征的所有测量值。这种情况发生的最频繁，一般当特征出了相机视角时便会被触发。
② 每次记录新图像时，当前相机姿态估计会加到状态向量中(cf. Section 3)。如果相机姿态的最大允许数量$N_{max}$已经达到，至少一个旧的必须删除。在丢弃状态之前，将使用在相应时刻发生的所有特征观测值，以便利用它们的定位信息。在算法使用时，选择了平均间隔时间从倒数第二个过去时刻的姿势开始的$N_{max}/3$个位姿pose。这些位姿在进行一次基于相同特征的多位姿约束的EKF更新后被丢掉。我们选择始终保持状态向量中最老的状态位姿pose，因为其与最新姿态的几何约束通常对应于更大的基线，因此更有价值的定位信息。


但是残差的维度可能会非常大，比如如果在10个相机位姿处都看到10个特征，残差的维度将会是170.为了减小EKF更新的计算复杂度，需要对$H_x$进行QR分解：

6. 流程总结

1. Initialization state vector and covariance
2. Integrate IMU and change state vector and covariance（EKF Propagation）
3. State augmentation（add camera state to EKF state vector）
4. Measurement and Update (all States and covariances will change)
5. Slide window operation
6. Repeat the above steps