K2E-B-S18-1 · Paper Note

FAST-LIO

Created 2023-08-25Updated 2026-06-08slam / papers / s-slam-systems / s18-fast-lio
  • Description：FAST-LIO 论文笔记 — 紧耦合迭代卡尔曼滤波激光惯性里程计，前向/反向传播运动补偿、IEKF 迭代状态更新、卡尔曼增益新形式（避免高维矩阵求逆）、源码架构走读
  • My Notion Note ID：K2E-B-S18-1
  • Created：2023-08-25
  • Updated：2026-06-08
  • License：转载欢迎：转载请注明作者 Yu Zhang 并附原文出处（yuzhang.io）

Table of Contents

1. Summary

Title: FAST-LIO: A Fast, Robust LiDAR-Inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter Authors: Wei Xu, Fu Zhang (HKU MARS Lab，DJI 资助) Paper: arXiv:2010.08196 (IEEE RA-L 2021, 6(2)) Github: hku-mars/FAST_LIO

FAST-LIO 提出用紧耦合迭代扩展卡尔曼滤波 (IEKF) 融合 LiDAR 特征点与 IMU，实现快速鲁棒的激光惯性里程计。论文核心贡献是一个新的卡尔曼增益公式，把计算复杂度从测量维度（激光点数 mm，可达上千）降到状态维度（n=18n=18，固定），使高密度点云的实时融合成为可能。

系统在流形 SO(3)×R15SO(3)\times\mathbb{R}^{15} 上运作，用 ⊞/⊟\boxplus/\boxminus 算子统一处理旋转量。反向传播对激光扫描期间的运动畸变进行去除，然后 IEKF 迭代重线性化收敛，最后把当前帧配准进 kd-tree 地图。

在无人机平台（DJI Manifold 2-C）上实时运行：户外建筑物序列（140 m）漂移仅 0.05%（0.07 m）；相比 LOAM 处理时间 59 ms，FAST-LIO 在更多点的条件下（1430 vs 1107 points）仅需 23 ms。

FAST-LIO 系统框架：LiDAR 输入->地图检索->残差计算->IEKF 迭代->状态更新（10–50 Hz 输出）

2. Key Contributions

  • 新卡尔曼增益公式：利用矩阵求逆引理将 m×mm\times m 求逆改写为 n×nn\times n，复杂度从测量维度降到状态维度
  • 紧耦合 IEKF：在流形上迭代重线性化，处理点-面残差的强非线性
  • 反向传播运动补偿：以扫描结束时刻为基准，IMU 反向积分去除帧内运动畸变
  • 轻量、鲁棒，适合无人机等算力受限平台

3. Method

3.1 状态与流形

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

x=[RGI,  pGI,  vGI,  bω,  ba,  g](1)\mathbf{x} = \bigl[\mathbf{R}_{GI},\;\mathbf{p}_{GI},\;\mathbf{v}_{GI},\;\mathbf{b}_\omega,\;\mathbf{b}_a,\;\mathbf{g}\bigr] \tag{1}

RGI∈SO(3)\mathbf{R}_{GI}\in SO(3)：IMU 到全局系旋转；pGI\mathbf{p}_{GI}：位置；vGI\mathbf{v}_{GI}：速度；bω,ba\mathbf{b}_\omega, \mathbf{b}_a：陀螺/加速度计 bias；g\mathbf{g}：重力方向（可观）。

⊞/⊟\boxplus/\boxminus 算子：旋转部分用李代数 so(3)\mathfrak{so}(3) 进行加减，其余分量普通加减，见 李群与李代数。

3.2 前向传播（预测步）

每来一帧 IMU，先按离散运动学传播名义状态均值（ff 为连续 IMU 动力学，uiu_i 为 IMU 输入，噪声取 0，Δt\Delta t 为 IMU 间隔）：

x^i+1=x^i⊞(Δt⋅f(x^i,ui,0))(1b)\hat{\mathbf{x}}_{i+1} = \hat{\mathbf{x}}_i \boxplus \bigl(\Delta t \cdot f(\hat{\mathbf{x}}_i, \mathbf{u}_i, \mathbf{0})\bigr) \tag{1b}

（论文 Eq.4）

再传播误差状态协方差：

P^k=Fx Pk−1 FxT+Fw Q FwT(2)\hat{\mathbf{P}}_{k} = \mathbf{F}_x\,\mathbf{P}_{k-1}\,\mathbf{F}_x^T + \mathbf{F}_w\,\mathbf{Q}\,\mathbf{F}_w^T \tag{2}

Fx\mathbf{F}_x：误差状态转移雅可比；Fw\mathbf{F}_w：噪声雅可比；Q\mathbf{Q}：IMU 过程噪声协方差；P^k\hat{\mathbf{P}}_k：预测协方差。

3.3 反向传播与运动补偿

问题：一帧 LiDAR 扫描耗时 ~100 ms，扫描期间载体在运动，帧内各点在不同位姿下采集->运动畸变。

FAST-LIO 方案：以扫描结束时刻 tkt_k 为基准，对帧内每个点（采集时刻 ρj<tk\rho_j < t_k），用 IMU 从 tkt_k 反向积分得到 IjI_j 相对 IkI_k 的相对位姿 TˇIjIk\check{\mathbf{T}}^{I_k}_{I_j}（只是个刚体变换，不是完整 18 维状态），把点变换到 tkt_k 时刻坐标系->去畸变（paper Eq.10）：

pfjLk=TLI−1 TˇIjIk TLI pfjLj\mathbf{p}^{L_k}_{f_j} = \mathbf{T}_{LI}^{-1}\,\check{\mathbf{T}}^{I_k}_{I_j}\,\mathbf{T}_{LI}\,\mathbf{p}^{L_j}_{f_j}

TLI\mathbf{T}_{LI} 为 LiDAR-IMU 外参，pfjLj\mathbf{p}^{L_j}_{f_j} 为点在采集时刻 LiDAR 系坐标。

3.4 残差

每个去畸变后的点 pj\mathbf{p}_j，在地图中找最近平面，残差 = 点到平面距离：

zj=ujT ⁣(RGI(RIL pj+pIL)+pGI−qj)(3)z_j = \mathbf{u}_j^T\!\left(\mathbf{R}_{GI}(\mathbf{R}_{IL}\,\mathbf{p}_j + \mathbf{p}_{IL}) + \mathbf{p}_{GI} - \mathbf{q}_j\right) \tag{3}

uj\mathbf{u}_j：平面法向量；qj\mathbf{q}_j：平面上一点；RIL,pIL\mathbf{R}_{IL}, \mathbf{p}_{IL}：LiDAR-IMU 外参。论文 Eq.12 是更一般形式 zj=Gj(p^fj−qj)z_j = \mathbf{G}_j(\hat{\mathbf{p}}_{f_j} - \mathbf{q}_j)：平面特征 Gj=ujT\mathbf{G}_j = \mathbf{u}_j^T（如上），边特征 Gj=⌊uj⌋∧\mathbf{G}_j = \lfloor\mathbf{u}_j\rfloor_\wedge（点到线，LOAM 风格）。

3.5 迭代状态更新（IEKF）

标准 EKF 一步更新——对强非线性点-面残差收敛不够精确。FAST-LIO 在当前估计点反复重线性化。第 κ\kappa 次迭代的更新不是简单加增量，而含两项（paper Eq.18）：

x^κ+1=x^κ⊞(−Kzκ−(I−KH)(Jκ)−1(x^κ⊟x^k))(4)\hat{\mathbf{x}}^{\kappa+1} = \hat{\mathbf{x}}^\kappa \boxplus \Bigl(-\mathbf{K}\mathbf{z}^\kappa - (\mathbf{I}-\mathbf{K}\mathbf{H})(\mathbf{J}^\kappa)^{-1}(\hat{\mathbf{x}}^\kappa \boxminus \hat{\mathbf{x}}_k)\Bigr) \tag{4}

第一项 −Kzκ-\mathbf{K}\mathbf{z}^\kappa 是观测修正；第二项是先验残差拉回（x^k\hat{\mathbf{x}}_k 为预测先验、Jκ\mathbf{J}^\kappa 为 ⊞\boxplus 引入的流形雅可比）——正是它把迭代更新与普通 EKF 单步区分开。迭代直到 ∥x^κ+1⊟x^κ∥<ε\|\hat{\mathbf{x}}^{\kappa+1} \boxminus \hat{\mathbf{x}}^\kappa\| < \varepsilon 收敛（ε\varepsilon 为预设收敛阈值，典型取 10−310^{-3} 量级；见 高斯滤波家族 §6）。

3.6 卡尔曼增益新形式

核心创新。标准增益：

K=PHT(HPHT+R)−1(5)\mathbf{K} = \mathbf{P}\mathbf{H}^T(\mathbf{H}\mathbf{P}\mathbf{H}^T + \mathbf{R})^{-1} \tag{5}

括号内矩阵维度 = 测量维度 mm（激光点数，可达上千）-> O(m2)O(m^2) 计算复杂度（论文原文），极贵。

用矩阵求逆引理（Sherman-Morrison-Woodbury）改写：

K=(HTR−1H+P−1)−1HTR−1(6)\mathbf{K} = \bigl(\mathbf{H}^T\mathbf{R}^{-1}\mathbf{H} + \mathbf{P}^{-1}\bigr)^{-1}\mathbf{H}^T\mathbf{R}^{-1} \tag{6}

括号内矩阵维度 = 状态维度 n=18n = 18，与 mm 无关，O(n3)O(n^3) 求逆。见 高斯滤波家族 §2。

4. Experiments & Results

平台：Livox AVIA LiDAR + DJI Manifold 2-C（i7-8550U，8 GB）+ 定制 280 mm 轴距无人机。

计算复杂度对比——两种增益公式的实际耗时 (ms)（Table II）：

激光点数 标准公式（旧） 新公式
307 7.1 0.07
717 23.4 0.11
998 109.3 0.25
1243 251 0.37
1453 1219 0.59
1802 1621 1.16

1243 点时旧公式需 251 ms（无法实时），新公式仅 0.37 ms——降低约 680 倍。

系统对比（室内高旋转场景，Table III）：

方法 有效特征点数 处理时间
LOAM 1107 59 ms
LOAM + IMU 1107 44 ms
FAST-LIO 1430 23 ms

FAST-LIO 用更多点、跑得更快。

轨迹漂移：

  • UAV 室内圆形轨迹（半径 1.8 m，重复 4 圈）：漂移 0.3%（0.08 m / 32 m）
  • 户外建筑物手持扫描（140 m）：漂移 0.05%（0.07 m）

室内大旋转场景：FAST-LIO 地图（右下）远比 LOAM / LOAM+IMU（上两格）干净

UAV 室外轨迹：圆形路径 4 圈，起终点基本重合，蓝色方块 = 起点

5. Ablation & Discussion

论文无系统性消融实验，以下是主要设计权衡：

  • IEKF vs EKF：点-面残差强非线性，单步 EKF 收敛不够；迭代收敛后精度更高，论文未给精度对比数字，但实验结果隐含
  • 新增益公式的意义：仅降低计算量，不改变估计精度（数学等价）；量化见表 Table II
  • 静态 kd-tree vs ikd-Tree：FAST-LIO v1 用静态 kd-tree，新点加入需局部重建；FAST-LIO2 引入 ikd-Tree（自平衡增量插入/删除），避免每帧重建整棵树->进一步提速
  • 反向传播 vs 前向插值去畸变：反向传播更精确（利用 IMU 全频率），计算量与前向相当

6. Strengths / Limitations / Future Work

优点：

  • 滤波派 LIO 也能达到优化派精度（实验证明）
  • 新卡尔曼增益公式是可复用通用技巧，任何高维测量的 KF 都适用
  • 轻量：在无人机嵌入式计算机上实时运行，无 GPU 依赖
  • 开源，代码结构清晰

局限：

  • 无回环 / 全局优化（纯里程计）
  • 静态 kd-tree，超大场景需频繁重建（FAST-LIO2 解决）
  • 仅支持点-面残差（LOAM 类），边缘特征选用相对保守

后续工作：

  • FAST-LIO2（T-RO 2022）：直接用原始点 + ikd-Tree，精度和速度进一步提升
  • FasterLIO：用 iVox（基于体素的近似最近邻）替代 ikd-Tree
  • Point-LIO：去除显式特征提取，直接用全点云
  • 港大 MARS Lab 的 LIO 系列基石

7. Code Walkthrough

代码库：hku-mars/FAST_LIO（当前主分支对应 FAST-LIO2，含 ikd-Tree；算法流程与 v1 一致）

7.1 目录结构

FAST_LIO/
  src/
    laserMapping.cpp   ← 主节点：同步、预处理、调度
    IMU_Processing.hpp ← 前向/反向传播、状态预测
  include/
    ikd-Tree/          ← 增量 kd-tree (FAST-LIO2 引入)
    IKFoM_toolkit/
      esekfom/         ← 流形上 IEKF 库
  config/
    avia.yaml          ← Livox AVIA 参数
    velodyne.yaml      ← Velodyne 参数
  launch/
    mapping_avia.launch
    mapping_velodyne.launch

7.2 主节点流程（laserMapping.cpp）

main()
  ├── 初始化 ROS 参数、订阅者（LiDAR / IMU）、发布者
  ├── 创建 esekf 实例 kf（状态 18 维，输入 12 维）
  └── 主循环:
        sync_packages()          ← 对齐 LiDAR + IMU 时间戳
        p_imu->Process()         ← 前向传播 + 反向传播运动补偿
        kf.update_iterated_dyn_share_modified()  ← IEKF 迭代更新
        map_incremental()        ← 把当前帧点加入 kd-tree 地图

回调：

  • livox_pcl_cbk() / standard_pcl_cbk()：接收 LiDAR 数据，存入 lidar_buffer
  • imu_cbk()：接收 IMU，缓冲到 imu_buffer（含时间戳修正）

7.3 IMU_Processing.hpp — 前向 / 反向传播

  • IMUPredict()（前向）：每帧 IMU 用离散运动方程积分状态；同时传播误差协方差（式 2）
  • UndistortPcl()（反向）：从扫描结束时刻反向积分，给帧内每个点计算相对位姿 check_state，变换到 tkt_k 时刻坐标系（式 3 前的去畸变）

7.4 esekfom — 流形 IEKF

  • 模板库，路径 include/IKFoM_toolkit/esekfom/；主类 esekf<state_ikfom, 12, input_ikfom>
  • update_iterated_dyn_share_modified()：迭代调用用户定义的 h_share_model（计算残差 zjz_j 和雅可比 H\mathbf{H}），用新公式（式 6）计算增益，⊞\boxplus 更新状态，直到收敛

7.5 关键配置参数（avia.yaml）

参数 含义
lid_topic LiDAR 点云话题
imu_topic IMU 话题
extrinsic_T / extrinsic_R LiDAR-IMU 外参（平移 + 旋转矩阵）
extrinsic_est_en 是否在线估计外参
scan_line 激光线数（影响特征提取）
pcd_save_enable 是否保存点云地图

7.6 paper-vs-code 差异

论文 代码
静态 kd-tree（v1 论文） 主分支用 ikd-Tree（v2 的增量树），v1 功能的 branch 已归档
反向传播描述基于 IMU 反向积分 UndistortPcl 实现时维护 check_state 链，逐点变换
新增益公式（式 6） esekfom/esekf.hpp 中 update_iterated_dyn_share_modified 直接实现
论文仅描述单线程 代码有 ROS 多回调线程，mtx_buffer + sig_buffer 保护共享队列

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). arXiv:2010.08196
FAST-LIO2 Xu W., Cai Y., He D., Lin J., Zhang F. (2022). FAST-LIO2: Fast Direct LiDAR-Inertial Odometry. IEEE T-RO. — ikd-Tree + 直接点
代码 github.com/hku-mars/FAST_LIO
相关 iEKF 见 高斯滤波家族 §6；矩阵求逆引理见 高斯滤波家族 §2；流形扰动见 李群与李代数 §9；激光去畸变见本文 §3.3