Description：FAST-LIO 论文笔记 — 紧耦合迭代卡尔曼滤波的快速鲁棒激光惯性里程计，前向传播 + 反向传播运动补偿、迭代状态更新、卡尔曼增益新形式 (避免高维矩阵求逆)
Paper：Xu, W., & Zhang, F. (2021). FAST-LIO: A Fast, Robust LiDAR-Inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter. IEEE RA-L, 6(2).
K2E-B ID：[K2E-B-S18-1]
Max3 PDF：[K2E] SLAM/[K2E-B-S] SLAM Systems/[K2E-B-S18] FAST-LIO/[K2E-B-S18-1][2021] FAST-LIO A Fast Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated KF.pdf (同目录另有 S18-2 FAST-LIO2、S18-3 FasterLIO)
Notion ID：(待创建)
Created：2023-08-25
Updated：2026-06-02
License：转载欢迎 — 请署名 Yu Zhang 并链回 yuzhang.io 原文

1. FAST-LIO 概述
2. 系统框架
3. 状态与流形
4. 前向传播
5. 反向传播与运动补偿
6. 残差与迭代状态更新
7. 卡尔曼增益新形式
8. 地图更新
9. 贡献与地位
References

1. FAST-LIO 概述

FAST-LIO (Xu & Zhang 2021, 港大 Mars Lab) — 紧耦合迭代卡尔曼滤波 (iterated EKF) 的激光惯性里程计 (LiDAR-Inertial Odometry, LIO)。

核心贡献：

紧耦合 IEKF 融合 LiDAR 特征点 + IMU
新的卡尔曼增益公式 — 计算复杂度从"测量维度" 降到"状态维度"，使高密度激光点的实时融合成为可能
反向传播运动补偿 — 处理激光扫描期间的运动畸变
轻量、鲁棒，适合无人机等算力受限平台

迭代 EKF (iEKF) 理论见 高斯滤波家族 §6；流形状态 / 误差状态见 李群与李代数 + VIO 系列 ch02。本笔记是 FAST-LIO 这篇论文的方法笔记。

2. 系统框架

IMU (高频) ─→ 前向传播 (预测状态 + 协方差)
LiDAR (一帧含多点, 扫描期间车在动)
   ↓
反向传播 → 运动补偿 (把一帧内各点去畸变到扫描结束时刻)
   ↓
残差计算 (点到地图平面/边的距离)
   ↓
迭代状态更新 (IEKF, 反复重线性化直到收敛)
   ↓
地图更新 (kd-tree 地图维护)

3. 状态与流形

状态在流形 $\mathcal{M} = SO(3) \times \mathbb{R}^{15}$ 上：

$$ \mathbf{x} = [\mathbf{R}{GI}, \mathbf{p}{GI}, \mathbf{v}_{GI}, \mathbf{b}_g, \mathbf{b}_a, \mathbf{g}] $$

(旋转 + 位置 + 速度 + 陀螺 bias + 加速度计 bias + 重力)。

FAST-LIO 用 $\boxplus / \boxminus$ 算子在流形上做加减 (旋转部分用李代数，见 李群与李代数 §9 扰动模型)，把流形优化统一成误差状态形式。

4. 前向传播

IMU 来一帧就做前向传播 (预测)：

状态传播 — 用 IMU 测量积分状态 (类似预积分，但这里是滤波的 propagate)
协方差传播 — $\hat{\mathbf{P}} = \mathbf{F}_x \mathbf{P} \mathbf{F}_x^T + \mathbf{F}_w \mathbf{Q} \mathbf{F}_w^T$

$\mathbf{F}_x, \mathbf{F}_w$ 是误差状态转移和噪声雅可比。

5. 反向传播与运动补偿

问题：一帧 LiDAR 扫描需要时间 (~100 ms)，扫描期间车在动 → 一帧内不同时刻的点在不同位姿采集 → 运动畸变 (motion distortion)。

FAST-LIO 的反向传播 (backward propagation)：

以扫描结束时刻为基准
对帧内每个点，用 IMU 反向积分推出该点采集时刻相对结束时刻的相对位姿
把点变换到结束时刻坐标系 → 去畸变

(与 IMU 系列 ch02 B3 激光雷达运动畸变去除的里程计辅助法同思路，但 FAST-LIO 用 IMU 高频积分做)

6. 残差与迭代状态更新

残差

每个去畸变后的点，在地图中找最近的平面/边特征，残差 = 点到该平面/边的距离 (点到平面用法向量):

$$ \mathbf{z}j = \mathbf{u}j^T (\mathbf{R}{GI} (\mathbf{R}{IL} \mathbf{p}j + \mathbf{p}{IL}) + \mathbf{p}_{GI} - \mathbf{q}_j) $$

($\mathbf{u}_j$ 平面法向，$\mathbf{q}_j$ 平面上一点)

迭代状态更新 (IEKF)

不像标准 EKF 一步更新，FAST-LIO 用迭代 EKF：在当前估计点反复重线性化残差 + 更新，直到收敛 (见 高斯滤波家族 §6 iEKF)。每次迭代解：

$$ \mathbf{x}^{\kappa+1} = \mathbf{x}^\kappa \boxplus \Delta\mathbf{x} $$

迭代让强非线性的点-面残差收敛更准。

7. 卡尔曼增益新形式

FAST-LIO 的核心创新。标准卡尔曼增益：

$$ \mathbf{K} = \mathbf{P} \mathbf{H}^T (\mathbf{H} \mathbf{P} \mathbf{H}^T + \mathbf{R})^{-1} $$

求逆的矩阵维度 = 测量维度 (激光点数，可能上千) → 巨贵。

FAST-LIO 用矩阵求逆引理 (Sherman-Morrison-Woodbury，见 高斯滤波家族 §2) 把它改写成：

$$ \mathbf{K} = (\mathbf{H}^T \mathbf{R}^{-1} \mathbf{H} + \mathbf{P}^{-1})^{-1} \mathbf{H}^T \mathbf{R}^{-1} $$

现在求逆的矩阵维度 = 状态维度 (固定 ~18)，与激光点数无关 → 大幅降复杂度，使上千点实时融合可行。这是 FAST-LIO "Fast" 的关键。

8. 地图更新

收敛后的位姿把当前帧点云配准进全局地图。FAST-LIO (v1) 用普通 (静态) kd-tree 维护地图，支持高效最近邻查询；新点加入需重建/局部重建树。后续 FAST-LIO2 引入 ikd-Tree，支持真正的增量插入/删除 + 自平衡，避免每帧重建整棵树。

9. 贡献与地位

证明滤波 (IEKF) 派 LIO 也能达到优化派精度且更快
卡尔曼增益新形式是可复用的通用技巧 (任何高维测量的 KF 都能用)
衍生：FAST-LIO2 (直接用原始点、ikd-Tree)、FasterLIO (iVox)、Point-LIO 等
港大 Mars Lab 的 LIO 系列基石

References

Xu, W., & Zhang, F. (2021). FAST-LIO: A Fast, Robust LiDAR-Inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter. IEEE RA-L, 6(2). — 论文
Xu, W., Cai, Y., He, D., Lin, J., & Zhang, F. (2022). FAST-LIO2: Fast Direct LiDAR-Inertial Odometry. IEEE T-RO. — 续作 (ikd-Tree + 直接点)
FAST-LIO 代码: github.com/hku-mars/FAST_LIO
He, D., Xu, W., & Zhang, F. (2021). Kinematic-ICP / on-manifold Kalman. — 流形卡尔曼基础
iEKF 见 高斯滤波家族 §6；矩阵求逆引理见 高斯滤波家族 §2；流形扰动见 李群与李代数 §9；激光去畸变见 IMU 系列 ch02 B3

Table of Contents