Description：MSCKF 论文笔记 — 滤波派 VIO 的奠基工作，多状态约束 EKF：多历史相机位姿入状态、特征不入状态、零空间投影 (null-space trick)、QR 降维、延迟线性化，附 S-MSCKF 代码走读
My Notion Note ID：K2E-B-K1-1
Created：2023-04-10
Updated：2026-06-11
License：转载欢迎：转载请注明作者 Yu Zhang 并附原文出处（yuzhang.io）

1. Summary

Title: A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation Authors: Anastasios I. Mourikis, Stergios I. Roumeliotis (University of Minnesota) Paper: ICRA 2007 Github: 无官方代码

MSCKF 是滤波派 VIO 的奠基之作。核心创新：把多个历史相机位姿保留在 EKF（扩展卡尔曼滤波器，对非线性系统做一阶线性化近似的递归状态估计器）状态里，让一个特征点跨多帧的观测形成"多状态约束"，而不把特征点本身放进状态——避免状态随地图无限膨胀，实现计算量仅与特征数线性相关（vs SLAM 类方法的二次复杂度）。

算法在 Minneapolis 室外城区 3.2 km 驾车实验中验证（数据先采集存储，事后离线处理），最终位置误差 ~10 m，漂移仅 0.31%。无 GPS 辅助、无回环。处理速度 14 Hz（Intel T7200 2GHz，高于 3 Hz 采图率），证明算法具备实时运行能力。社区传 Apple ARKit 借鉴了 MSCKF 思路（Apple 未公开证实）。

衍生：S-MSCKF（双目扩展，Sun 2018）、OpenVINS（MSCKF 研究平台，含 FEJ + 在线标定），均已成为 VIO 研究基础设施。

2. Key Contributions

特征不入状态：跨多帧的特征观测形成"多状态约束"，EKF 更新后特征丢弃——状态规模有界
计算量线性：复杂度 $O(L \cdot n)$ ， $L$ 为特征数， $n$ 为状态维度（≤ $6N+15$ ）；vs SLAM-EKF 的 $O(n^2)$
零空间投影：消去残差中的特征误差项，使不含特征坐标的约束直接用于 EKF 更新
QR 降维：大幅降低高维 $\mathbf{H}_\mathbf{X}$ 的 EKF 更新代价
鲁棒性：逆深度参数化 + 延迟线性化提高对非线性的抗性

3. Method

3.1 IMU 状态

\mathbf{X}_\text{IMU} = \bigl[{}^I_G\bar{q}^T,\;\mathbf{b}_g^T,\;{}^G\mathbf{v}_I^T,\;\mathbf{b}_a^T,\;{}^G\mathbf{p}_I^T\bigr]^T \tag{1}

${}^I_G\bar{q}$ ：unit quaternion（global→IMU）； $\mathbf{b}_g, \mathbf{b}_a$ ：陀螺/加速度计 bias； ${}^G\mathbf{v}_I$ ：IMU 在全局系速度； ${}^G\mathbf{p}_I$ ：IMU 位置。

3.2 完整状态（含 N 历史相机位姿）

\hat{\mathbf{X}}_k = \bigl[\hat{\mathbf{X}}_{\text{IMU}_k},\;{}^{C_1}_G\hat{q}^T,\;{}^G\hat{\mathbf{p}}_{C_1}^T,\;\dots,\;{}^{C_N}_G\hat{q}^T,\;{}^G\hat{\mathbf{p}}_{C_N}^T\bigr]^T \tag{2}

状态维度 $6N+15$ ； $N$ 上限为 $N_\text{max}$ （论文用 30）。

3.3 IMU 传播

连续时间 IMU 运动学（误差状态）：

\dot{\tilde{\mathbf{X}}}_\text{IMU} = \mathbf{F}\tilde{\mathbf{X}}_\text{IMU} + \mathbf{G}\mathbf{n}_\text{IMU} \tag{3}

$\mathbf{F}$ 为 $15\times15$ 系统矩阵， $\mathbf{G}$ 为噪声输入矩阵， $\mathbf{n}_\text{IMU} = [\mathbf{n}_g,\mathbf{n}_{wg},\mathbf{n}_a,\mathbf{n}_{wa}]^T$ 。协方差传播：

\dot{\mathbf{P}}_{II} = \mathbf{F}\mathbf{P}_{II} + \mathbf{P}_{II}\mathbf{F}^T + \mathbf{G}\mathbf{Q}_\text{IMU}\mathbf{G}^T \tag{4}

离散化：原论文 (Mourikis 2007) 用 5 阶 Runge-Kutta 积分 IMU 状态； $\boldsymbol{\Phi}(t_k+T, t_k)$ 用同样方法积分。注意 S-MSCKF 开源实现 (msckf_vio) 改用 4 阶 RK4（实现简化，与原论文不同，见 §7.5）。

3.4 状态扩增（State Augmentation）

新图像到来 → 当前相机位姿克隆进状态，协方差扩增：

\mathbf{P}_{k|k} \leftarrow \begin{bmatrix}\mathbf{I}_{6N+15} \\ \mathbf{J}\end{bmatrix}\mathbf{P}_{k|k}\begin{bmatrix}\mathbf{I}_{6N+15} \\ \mathbf{J}\end{bmatrix}^T \tag{5}

\mathbf{J} = \begin{bmatrix}\mathbf{C}({}^C_I\bar{q}) & \mathbf{0}_{3\times9} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times6N} \\ \mathbf{C}^T_{\hat{q}}\,[{}^I\mathbf{p}_C]_\times & \mathbf{0}_{3\times9} & \mathbf{I}_3 & \mathbf{0}_{3\times6N}\end{bmatrix} \tag{6}

${}^C_I\bar{q}$ ：相机→IMU 外参四元数（已知标定量）； ${}^I\mathbf{p}_C$ ：IMU 系下相机原点位置（已知外参）； $[\cdot]_\times$ ：反对称叉积矩阵。均源自 Eq.(14)： ${}^{C_i}_G\hat{q} = {}^C_I\bar{q} \otimes {}^I_G\hat{q}$ 和 ${}^G\hat{p}_C = {}^G\hat{p}_I + \mathbf{C}^T_{\hat{q}}\,{}^I\mathbf{p}_C$ 。

克隆后状态维度增加 6（新相机位姿）。

3.5 测量模型

特征 $f_j$ 在相机 $i$ 的归一化像素观测：

\mathbf{z}_i^{(j)} = \frac{1}{{}^{C_i}Z_j}\begin{bmatrix}{}^{C_i}X_j \\ {}^{C_i}Y_j\end{bmatrix} + \mathbf{n}_i^{(j)} \tag{7}

线性化后：

\mathbf{r}_i^{(j)} \simeq \mathbf{H}_\mathbf{X}^{(j)}\tilde{\mathbf{X}} + \mathbf{H}_f^{(j)}\;{}^G\tilde{\mathbf{p}}_{f_j} + \mathbf{n}_i^{(j)} \tag{8}

3.6 零空间投影（Null-space Trick）

残差中含特征误差 ${}^G\tilde{\mathbf{p}}_{f_j}$ （不在状态里）。将所有 $M_j$ 次观测残差堆叠，投影到 $\mathbf{H}_f^{(j)}$ 的左零空间 $\mathbf{A}$ （ $\mathbf{A}^T\mathbf{H}_f^{(j)}=0$ ）：

\mathbf{r}_o^{(j)} = \mathbf{A}^T\mathbf{r}^{(j)} \simeq \underbrace{\mathbf{A}^T\mathbf{H}_\mathbf{X}^{(j)}}_{\mathbf{H}_o^{(j)}}\tilde{\mathbf{X}} + \mathbf{A}^T\mathbf{n}^{(j)} \tag{9}

特征误差项被消掉 → 纯状态约束，直接 EKF 更新。投影后 $\mathbf{r}_o^{(j)}$ 维度 $2M_j-3$ （减少 3 维，对应特征三个空间坐标的约束冗余）。

3.7 QR 降维

所有特征残差堆叠 $\mathbf{r}_o = \mathbf{H}_\mathbf{X}\tilde{\mathbf{X}} + \mathbf{n}_o$ ，维度极高。QR 分解：

\mathbf{H}_\mathbf{X} = [\mathbf{Q}_1\;\mathbf{Q}_2]\begin{bmatrix}\mathbf{T}_H \\ \mathbf{0}\end{bmatrix} \tag{10}

保留有效部分： $\mathbf{r}_n = \mathbf{Q}_1^T\mathbf{r}_o = \mathbf{T}_H\tilde{\mathbf{X}} + \mathbf{n}_n$ ，维度从 $d=\sum(2M_j-3)$ 降到 $r$ （ $\mathbf{T}_H$ 的列数），节省 EKF 更新代价（ $O(r^2d) \ll O(d^3)$ ，因 $d\gg r$ ）。

3.8 EKF 更新

\mathbf{K} = \mathbf{P}\mathbf{T}_H^T(\mathbf{T}_H\mathbf{P}\mathbf{T}_H^T + \mathbf{R}_n)^{-1} \tag{11}

\Delta\mathbf{X} = \mathbf{K}\mathbf{r}_n, \quad \mathbf{P}_{k+1|k+1} = (\mathbf{I}-\mathbf{K}\mathbf{T}_H)\mathbf{P}_{k+1|k}(\mathbf{I}-\mathbf{K}\mathbf{T}_H)^T + \mathbf{K}\mathbf{R}_n\mathbf{K}^T \tag{12}

3.9 何时触发 EKF 更新

特征失跟：完整观测轨迹齐了，用于更新
窗口满（ $N_\text{max}$ 位姿）：选 $N_\text{max}/3$ 个均匀分布的位姿（从次老位姿开始）做更新后丢弃；最老位姿始终保留（基线最长、信息最大）

4. Experiments & Results

平台：Pointgrey FireFly 相机（640×480，3 Hz 采图，14 Hz 处理）+ Inertial Science ISIS IMU（100 Hz）；车载 Minneapolis 市区驾驶

关键参数： $N_\text{max}=30$ ，SIFT 特征，Intel T7200 2GHz（实时）

估计轨迹叠加在 Minneapolis 地图上（蓝色路径）；红色方块为起始点，总行程 3.2 km

位置（a）、姿态（b）、速度（c）的 3σ 误差界；姿态精度优于 1°，速度优于 0.35 m/s

实验结果：

指标	数值
总行程	3.2 km
最终位置误差	~10 m（0.31% 漂移）
姿态精度（3σ）	优于 1°
速度精度（3σ）	优于 0.35 m/s
处理特征总数	142，903
实时性	14 Hz（Intel T7200 2GHz）

轨迹在城区街道地图上清晰可辨，起终点位置误差约 10 m（真实终点约 $[0, 7, 0]^T$ m，估计值 $[-7.92, 13.14, -0.78]^T$ m）。无 GPS 辅助、无回环检测。

5. Ablation & Discussion

原论文无消融实验（单一方法提出论文），以下为设计权衡讨论：

特征不入状态 vs EKF-SLAM：避免状态膨胀，复杂度 $O(n)$ vs $O(n^2)$ ；代价是三角化引入近似误差
N_max 选择： $N_\text{max}=30$ 够利用大部分约束，超过后边际收益递减；更大窗口增加更新代价（ $O(r^2 d)$ ）
延迟线性化：多次观测后再线性化 → 初值更接近真值 → 雅可比更准 → 更新质量更高（类似迭代 EKF 的思路）
逆深度参数化：远点在欧氏深度下病态；逆深度保持数值稳定（见三角化）

6. Strengths / Limitations / Future Work

优点：

滤波派 VIO 的范式定义者——后续所有 MSCKF 变体的基础
计算量仅与特征数线性相关，适合资源受限平台
无需地图持久化，内存占用有界
代码实现（S-MSCKF）极为整洁，广泛用于研究

局限：

无回环（纯里程计）
EKF 线性化误差积累，长时间漂移高于优化派（OKVIS/VINS）
无局部地图重用（特征用完即丢）
原版单目，双目需 S-MSCKF 扩展

后续工作：

S-MSCKF（Sun 2018）：双目扩展，EuRoC 优于 VINS-Mono 部分序列
OpenVINS：MSCKF + FEJ（First Estimates Jacobian，首估计雅可比：线性化点固定后不随状态更新，保证可观性矩阵结构一致、避免不一致性漂移）+ 在线标定，成为 VIO 研究标准平台（见 OpenVINS 文档）
MSCKF 2.0（改进版）：更好的可观性约束

7. Code Walkthrough

参考实现：S-MSCKF：KumarRobotics/msckf_vio（双目，Sun 2018，ROS）

7.1 目录结构

msckf_vio/
  src/
    image_processor.cpp   ← 前端：特征检测/跟踪/立体匹配
    msckf_vio.cpp         ← 核心：状态传播 + 扩增 + 更新
  include/msckf_vio/
    image_processor.h
    msckf_vio.h
  config/
    camchain.yaml          ← Kalibr 格式外参 + 内参

7.2 image_processor — 前端

两步前端：

stereoCallback()：收到双目帧 → Shi-Tomasi 角点检测 + Lucas-Kanade 光流跟踪 → IMU 旋转预测辅助 + 2-Point RANSAC（利用已知旋转将 5 点法退化为 2 点问题，随机采样 2 对特征即可求解平移，提高外点剔除效率）去外点
publish()：输出跟踪特征点列表到后端

IMU 旋转预测：用帧间 IMU 积分的旋转量补偿特征预测位置，提高光流精度。

7.3 msckf_vio — 核心滤波器

主流程（imuCallback + featureCallback）：

imuCallback()
  └── batchImuProcessing()  ← 4 阶 RK4 积分 IMU（式 3-4）+ 状态传播（原论文为 5 阶 RK）

featureCallback()
  ├── stateAugmentation()   ← 克隆当前位姿 + 扩增协方差（式 5-6）
  ├── addFeatureObservations() ← 更新特征观测记录
  ├── removeLostFeatures()  ← 失跟特征 → processFeatures() EKF 更新
  └── pruneCamStateBuffer() ← 窗口满 → 选 N/3 位姿更新后删除

measurementUpdate()（零空间投影 + QR + EKF 更新）：

for each feature:
  三角化 → 计算 H_X^(j) + H_f^(j)
  左零空间投影 A → 消掉 H_f^(j)（式 9）
stacking → QR 分解（式 10）→ T_H, r_n
compute K（式 11）→ ΔX → update P（式 12）

7.4 关键参数（config/camchain.yaml）

参数	含义
`cam0/T_cam_imu`	相机0—IMU 外参（ $4\times4$ 齐次矩阵）
`cam0/intrinsics`	内参 [fx， fy, cx, cy]
`max_cam_state_size`	$N_\text{max}$ （默认 20）
`feature_translation_threshold`	特征运动最小阈值（过滤静态场景假运动）
`noise_gyro` / `noise_acc`	IMU 随机游走噪声密度

7.5 paper-vs-code 差异

论文（原版单目 MSCKF）	S-MSCKF 代码（双目）
单目，相机从 IMU 状态派生	双目，用立体初始化约束深度（更准）
SIFT 特征	Shi-Tomasi + LK 光流（速度更快）
$N_\text{max}=30$ 作演示	可配置（默认 20）
论文无在线外参估计	S-MSCKF 代码支持相机—IMU 外参在线更新
IMU 积分用 5 阶 Runge-Kutta	代码改用 4 阶 RK4（`predictNewState`，实现简化）

References

类别	链接
论文	Mourikis A.I., Roumeliotis S.I. (2007). A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation. ICRA. PDF
S-MSCKF	Sun K. et al. (2018). Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight. IEEE RA-L. arXiv:1712.00036
代码	KumarRobotics/msckf_vio (S-MSCKF 双目实现)
OpenVINS	Geneva P. et al. (2020). OpenVINS. ICRA. docs.openvins.com
笔记参考	xinliang-zhong.vip/msckf_notes
相关	EKF 见高斯滤波家族；零空间/可观性见 VIO 滑窗优化；逆深度见三角化

MSCKF — Multi-State Constraint Kalman Filter

Table of Contents