Description：MSCKF 论文笔记 — 滤波派 VIO 的奠基工作，多状态约束卡尔曼滤波：状态含一个 IMU + 多个历史相机位姿、特征观测的零空间投影 (QR/null-space trick)、延迟线性化
Paper：Mourikis, A. I., & Roumeliotis, S. I. (2007). A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation. ICRA. (双目扩展: Sun et al. 2018 S-MSCKF)
K2E-B ID：[K2E-B-K1-1]
Max3 PDF：[K2E] SLAM/[K2E-B-K] Kalman Filter/[K2E-B-K1] MSCKF/[K2E-B-K1-1][2007] MSCKF A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation.pdf (同目录 K1-2/3 S-MSCKF 双目)
Notion ID：(待创建)
Created：2023-04-10
Updated：2026-06-02
License：转载欢迎 — 请署名 Yu Zhang 并链回 yuzhang.io 原文

1. MSCKF 概述

MSCKF (Multi-State Constraint Kalman Filter) — Mourikis & Roumeliotis 2007，滤波派 VIO 的奠基之作。社区普遍传闻苹果 ARKit 的 VIO 借鉴了 MSCKF 思路 (Apple 未公开证实)。

核心创新：把多个历史相机位姿保留在 EKF 状态里 (variable state dimension filter)，让一个特征点跨多帧的观测形成多状态约束，而不把特征点本身放进状态 (避免状态随地图无限膨胀) → 计算量可控。

滤波派 vs 优化派 VIO 见 SLAM 面试题汇总 C9；EKF/iEKF 见 高斯滤波家族；可观性/FEJ 见 VIO 系列 ch03。本笔记是 MSCKF 这篇论文的方法笔记。

2. 状态向量与状态扩增

MSCKF 借鉴 VSDF (Variable State Dimension Filter, McLauchlan 1999) 的滑窗思路 — 状态里维护一窗相机位姿 + 延迟线性化。但论文明确与 VSDF 对比:VSDF 复杂度对特征数至少二次，MSCKF 做到线性。

IMU 状态：

$$ \mathbf{X}_{\text{IMU}} = [\bar{\mathbf{q}}, \mathbf{b}_g, \mathbf{v}, \mathbf{b}_a, \mathbf{p}] $$

完整状态 = 1 个最新 IMU + N 个历史相机位姿：

$$ \mathbf{X}k = [\mathbf{X}{\text{IMU}}, \mathbf{q}{c_1}, \mathbf{p}{c_1}, \dots, \mathbf{q}{c_N}, \mathbf{p}{c_N}] $$

状态扩增 (State Augmentation)

新图像来 → 把当前相机位姿克隆进状态。协方差也对应扩增 (雅可比 $J$ 把 IMU 协方差传播到新相机位姿块)：

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

(注：维度按状态定义；克隆位姿与 IMU 状态相关，故协方差非对角块要算)

3. 何时做 EKF 更新

两种触发：

一个被跟踪的特征不再被检测到 — 它的完整观测轨迹齐了，可以用来更新
新图像到来，相机位姿数达到 $N_{\max}$ — 滑窗满，需剔除旧位姿。论文做法:选 $N_{\max}/3$ 个时间上均匀分布、从次老 (second-oldest) 位姿开始的位姿，用其上特征约束做一次 EKF 更新后再丢弃；最老位姿始终保留 (基线最大、定位信息最有价值)

4. 零空间投影 (null-space trick)

核心技巧。一个特征点 $\mathbf{f}$ 被多个相机位姿观测，但特征点不在状态里。MSCKF 先三角化出 $\mathbf{f}$，残差线性化为：

$$ \mathbf{r} = \mathbf{H}_X \tilde{\mathbf{X}} + \mathbf{H}_f \tilde{\mathbf{f}} + \mathbf{n} $$

含特征误差 $\tilde{\mathbf{f}}$ 项 (不想要，因为 $\mathbf{f}$ 不是状态)。把残差投影到 $\mathbf{H}_f$ 的左零空间 ($\mathbf{A}^T \mathbf{H}_f = 0$)：

$$ \mathbf{r}_o = \mathbf{A}^T \mathbf{r} = \mathbf{A}^T \mathbf{H}_X \tilde{\mathbf{X}} + \mathbf{A}^T \mathbf{n} $$

$\tilde{\mathbf{f}}$ 项被消掉 → 残差只含状态约束 → 可以直接 EKF 更新，不需要把特征放进状态。这就是"multi-state constraint"的来源。

5. QR 分解降维

投影后 $\mathbf{H}_X$ 维度仍很大 (很多特征 × 很多位姿)。做 QR 分解：

$$ \mathbf{H}_X = [\mathbf{Q}_1 ; \mathbf{Q}_2] \begin{bmatrix} \mathbf{T}_H \ 0 \end{bmatrix} $$

$\mathbf{Q}_2$ 对应的部分与状态无关 (零行)，可丢弃。残差降维：

$$ \mathbf{r}_n = \mathbf{Q}_1^T \mathbf{r}_o $$

大幅降低 EKF 更新的计算量 (与 FAST-LIO 的卡尔曼增益新形式异曲同工，都是避免高维矩阵运算)。

滑窗边缘化

滑窗满时 marg 一帧，因为是 EKF (协方差表示)，直接删掉协方差矩阵对应行列即可 (协方差形式下边缘化 = 删行列，见 边缘化 那篇 §4)。

6. 鲁棒性技巧

两个增加对线性化误差鲁棒性的特点：

逆深度参数化 (inverse feature depth) — 远点数值稳定 (见 三角化 D4)
延迟线性化 (delayed linearization) — 等特征有多次观测再线性化，初值更好

7. 贡献与地位

滤波派 VIO 的范式 — 多状态约束 + 特征不入状态
计算量与地图大小解耦 (vs EKF-SLAM 状态随地图膨胀)
衍生：S-MSCKF (双目，Sun 2018)、OpenVINS (MSCKF 研究平台 + FEJ + 在线标定)
与优化派 (OKVIS/VINS) 长期并行，滤波派胜在算力/功耗

References

Mourikis, A. I., & Roumeliotis, S. I. (2007). A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation. ICRA. — 论文
Sun, K., et al. (2018). Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight (S-MSCKF). IEEE RA-L. — 双目扩展 (K1-2)
Geneva, P., et al. (2020). OpenVINS: A Research Platform for Visual-Inertial Estimation. ICRA.
MSCKF 笔记: xinliang-zhong.vip/msckf_notes；S-MSCKF 代码注释 github.com/smilefacehh/S-MSCKF-vio-Note — 原 note 引用
知乎: MSCKF 推导 / 前端
EKF 见 高斯滤波家族；零空间/可观性见 VIO 系列 ch03；逆深度见 三角化；边缘化见 边缘化

MSCKF — Multi-State Constraint Kalman Filter

Table of Contents