Fast-LIO

2021

Paper: FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter

Code: https://github.com/hku-mars/FAST_LIO

FAST‑LIO 以紧耦合 iEKF 为核心，将高频 IMU 前后向传播与点到地图几何约束在滤波框中统一，关键在于以状态维度为主的更新复杂度，使得一次性融合大量点云观测仍具实时性。系统由两条主线程协同：IMU 高频预测与 LiDAR 批量更新，并配套地图管理与数据关联

背景

小 FoV、稀疏特征 → 传统 LOAM 类方法易退化。 UAV 载荷受限 → 算法必须极轻量，不能靠强算力“硬抗”.

FAST‑LIO 直接在滤波器里融合原始 LiDAR 特征点 + IMU 数据（紧耦合），保留姿态/速度与观测的相关性，退化场景下仍有鲁棒性

核心贡献:

  • 正反向传播：IMU 前向积分 + 反向积分做逐点去畸变（deskew）。
  • 迭代扩展卡尔曼滤波 (iEKF)：每一批特征点直接进入滤波更新。
  • 卡尔曼增益新公式：计算复杂度依赖状态维而非观测维，能一次融合上千特征点而不卡顿

Preliminary

iEKF（Iterated Extended Kalman Filter，迭代扩展卡尔曼滤波） 和 EKF（Extended Kalman Filter，扩展卡尔曼滤波）两者其实是“同宗”的滤波框架，只是在更新阶段的处理策略上有关键差异

EKF

一次线性化，一次更新 流程:

  1. 预测（Prediction）：用非线性系统模型推进状态和协方差
  2. 更新（Update）：在当前预测状态 $ x_k $ 处，对观测模型 $ h\left( \cdot \right) $ 一阶泰勒展开，得到一次观测雅可比 $H$
  3. 用这个固定的 $H$ 计算卡尔曼增益 $K$，然后修正状态

特点:

  • 计算开销小（只算一次雅可比和一次增益）
  • 但如果观测方程高度非线性，或初始预测偏差大，一次线性化误差可能很大，导致更新不准甚至发散

EKF 像是只看一次地图就决定怎么走，结果如果起点离路很远，方向可能有偏

iEKF

多次迭代线性化与更新

在更新阶段引入迭代:

  1. 初始迭代从预测状态 $ x_k^{\left( 0 \right)} $ 开始
  2. 在第 $i$ 次迭代, 用当前迭代状态 $ x_k^{\left( i \right)} $ 重新计算观测预测值 $ h\left( x_k^{\left( i \right)} \right) $ 和雅可比 $ H^{\left( i \right)} $
  3. 根据新的残差 $ z + H^{\left( i \right)} \delta x^{\left( i \right)} $ 计算增量 $ \delta x^{\left( i \right)} $, 更新 $ x_k^{\left( i+1 \right)} = x_k^{\left( i \right)} \oplus \delta x^{\left( i \right)} $
  4. 重复 2–3 步直到收敛（ $ \delta x $ 很小）或达到迭代次数上限

特点:

  • 在每次迭代中，都用更接近真实值的状态重新线性化观测方程，从而减少一次性线性化带来的截断误差。
  • 收敛后，相当于在当前时刻解了一个非线性最小二乘问题的近似。

iEKF 则是一边走一边重新看地图、不断修正路线，直到站到合适的位置才停下

PCA

PCA 的全称是 Principal Component Analysis，中文叫 主成分分析。它是一种经典的数据降维与特征提取方法

核心思想：

  • 通过线性变换，找到数据中方差最大的方向（称为主成分）。
  • 将数据投影到这些方向上，使得用更少的维度保留尽可能多的原始信息。
  • 主成分之间是正交（互不相关）的

数学流程：

  1. 数据标准化（可选）：不同特征量纲差异大时，需要先让每个特征均值为 0、方差为 1
  2. 计算协方差矩阵：描述各特征之间的线性相关性
  3. 特征值分解 / SVD：
    • 特征值：表示该方向上的方差大小（信息量）。
    • 特征向量：表示主成分的方向
  4. 选择主成分 按特征值从大到小排序，选取前 k 个累计方差贡献率达到目标（如 95%）的主成分
  5. 数据投影 将原数据投影到选定的主成分方向，得到降维后的数据

在Fast-LIO中，PCA 用来：

  • 平面点：邻域点的最小特征值方向是法向量。
  • 边缘点：邻域点的最大特征值方向是边的方向向量。

这样就能用少量参数（法向量或方向向量 + 质心）来描述局部几何结构，方便做特征关联和残差计算

框架

把 FAST‑LIO 当作一条紧凑的流水线：IMU 高频“拉着状态往前走”，LiDAR 把每个点“时间校正后”投向地图，形成大量几何约束，全部在同一个 iEKF（迭代扩展卡尔曼滤波）里统一融合。得益于增益公式与流形更新，算法可以一次性吃下成百上千个点，输出高频稳定的姿态与轨迹

用 IMU 的短时可靠性兜住快速运动，用 LiDAR 的空间几何把长时漂移“拴回去”，并尽量让每个 LiDAR 点的几何信息都进入估计器

流程概览:

  1. 预处理: 积累一段时间窗口的点形成一帧扫描，提取边缘/平面特征
  2. 前向传播: 用 IMU 连续推进状态与协方差
  3. 后向传播与补偿: 对每个点从采样时刻回退到扫描末时刻，做运动畸变补偿
  4. 构建残差: 将点投到已建小地图，算点到平面/边的距离作为观测残差
  5. iEKF 迭代更新: 线性化残差，计算更新增量，迭代到收敛
  6. 更新地图: 用最新位姿把特征点写入地图，供下一帧最近邻/平面拟合使用

实现要点: 使用 KD-tree 在亚局部地图里找平面/边；通过新卡尔曼增益公式避免在巨大测量维度上求逆，大幅降时延

点云特征约束

点云移动补偿 → 全局投影 → KD‑tree 检索邻域 → PCA 拟合平面/边 → 距离阈值筛选 → 残差统一建模

基于局部“光滑度”从一帧扫描中提取两类几何特征：平面点（局部光滑度高）与边缘点（局部光滑度低）：

  • 平面特征（Planar）： 局部表面变化小、曲率低的点。常用做法是以点的邻域计算光滑度/曲率指标，取阈值以上的“平滑”点作为平面候选
  • 边特征（Edge）： 局部表面变化大、曲率高的点。与平面相反，取光滑度/曲率较大的点作为边候选

把补偿后的特征点投到全局坐标系后，在最近构建的子地图上为每个点查找其几何对应体（平面或边）：

  1. 邻域检索： 用 KD-tree 在子地图中找最近邻（固定数量或半径），得到该点的局部支持集，用于拟合对应的平面或直线
  2. 对应选择： 假设特征点“属于”其周围最接近的平面或边，基于邻域拟合出几何体后，计算该点到几何体的距离作为残差；若残差超阈值（如 0.5 m），视作离群或新结构，暂不参与本次更新

平面拟合

平面拟合（PCA/最小二乘）：

  1. 取领域质心： \(c = \frac{1}{N}\sum_{i}^{N} p_i\)

  2. 协方差矩阵: \(C = \frac{1}{N}\sum_{i}^{N} \left ( p_i-c \right ) \left ( p_i-c \right ) ^\mathrm{T}\)

  3. 特征分解：计算协方差矩阵的三个特征值 $\lambda_{\mathrm{min, mid, max}}$, 取最小特征值对应特征向量 $n$ 作为平面法向；以 $c$ 作为参考点 $q$

质量判据: 要求 $ \lambda_{\mathrm{min}} \ll \lambda_{\mathrm{min}}, \lambda_{\mathrm{max}} $ 以滤除非平面邻域

加权与鲁棒： 可对距离远的邻域点降权，或用加权最小二乘/RANSAC 提升抗噪与遮挡鲁棒性

边拟合（PCA/主方向）

  1. 同上求 $ c, C $
  2. 取最大特征值对应特征向量 $ d $, 作为直线方向; 以 $c$ 作为参考点 $q$

质量判据: 要求 $ \lambda_{\mathrm{max}} \gg \lambda_{\mathrm{mid}} \approx \lambda_{\mathrm{min}} $, 即邻域点高度共线

稳健细节： 可在拟合后检查方向一致性（与历史方向夹角阈值），并限制邻域尺度避免跨越拐角

状态定义

状态向量（18 维）

\[\mathbf{x} = \left\{ \mathbf{R}_{GI} \in SO(3),\ \mathbf{p}_{GI} \in \mathbb{R}^3,\ \mathbf{v}_I \in \mathbb{R}^3,\ \mathbf{b}^g,\ \mathbf{b}^a,\ \mathbf{g} \right\}\]

其中:

  • $ \mathbf{R}_{GI} $ 表示IMU在世界坐标系下的旋转
  • $ \mathbf{p}_{GI} $ 表示IMU 在世界坐标系的位置
  • $ \mathbf{v}_I $ 表示IMU 的速度
  • $ \mathbf{b}^a, \mathbf{g} $ 表示陀螺仪和加速度计偏置
  • $ g $ 表示重力向量

流性运算: 姿态在 $ SO \left( 3 \right)$ 上，用“⊕/⊖”在切空间内封装最小误差，用 exp/log互转，避免冗余参数化与奇异性

封装运算(旋转和向量): \(R \oplus r = R\,\mathrm{Exp}(r),\quad R_1 \ominus R_2 = \mathrm{Log}(R_2^\top R_1),\quad a \oplus b = a + b\)

指数映射 $ SO \left( 3 \right)$ :

\[\mathrm{Exp}(r) = I + \frac{\sin\|r\|}{\|r\|}\,[r]_\times + \frac{1-\cos\|r\|}{\|r\|^2}\,[r]_\times^2\]

其中 $r$ 表示姿态最小增量（李代数）, $ \left [ r \right ] _\times $表示叉乘矩阵

前向传播

每到一条 IMU 数据就推进一次状态；协方差用线性化雅可比与噪声协方差传播

连续时间模型

\[\dot{p}^G_I = v^G_I,\quad\] \[\dot{v}^G_I = R^G_I\,(a^m - b_a - n_a) + g^G,\quad\] \[\dot{R}^G_I = R^G_I\,[\omega^m - b_\omega - n_\omega]_\times\] \[\dot{b}_\omega = n_{b_\omega},\quad \dot{b}_a = n_{b_a},\quad \dot{g}^G = 0\]

其中:

  • $ {p}^G_I, {v}^G_I, {R}^G_I $ : IMU 在全局系下的位置/速度/姿态
  • $ a^m, \omega^m $ : IMU 加速度计/陀螺测量
  • $ b_a, b_\omega $ : 零偏，建模为随机游走
  • $ n_* $ : 白噪声
  • $ {g}^G $ : 重力向量，作为状态估计

离散时间模型

\[x_{i+1} = x_i \oplus \big(\Delta t\; f(x_i,u_i,w_i)\big) \qquad\]

其中:

  • $ x $: 表示联合状态（姿态、位置、速度、零偏、重力）
  • $ u_i $: 表示IMU 输入 ($ a^m, \omega^m $)
  • $ w_i $: 表示过程噪声

前向传播

\(P_{i+1} = F_x\,P_i\,F_x^\top + F_w\,Q\,F_w^\top\)

其中:

  • $ F_x, F_w $: 误差状态雅可比
  • $ Q $: 过程噪声协方差

后向传播

为避免运动畸变，FAST-LIO 通过“反向传播”把同一扫描内不同时间戳的点补偿到扫描末时刻，再做关联与拟合，这一步显著提升后续几何约束的可靠性

畸变补偿

\[{}^{L_k}\!p^{f}_j = T_{LI}^{-1}\;{}^{I_k}\!T_{I_j}\;T_{LI}\;{}^{L_j}\!p^{f}_{j}\]

其中:

  • $ {}^{I_k}!T_{I_j} $: 点采样时刻j相对扫描末k的体坐标变换, 由反向积分得到
  • $ T_{LI} $: LiDAR-IMU 外参
  • $ {}^{L_j}!p^{f}_{j} $: LiDAR 局部点坐标（原始测量）
  • $ {}^{L_k}!p^{f}_j $: 补偿到扫描末的点

补偿后投影到全局位置:

\[p^G_j = T^G_{I_k}\;T_{LI}\;{}^{L_k}\!p^{f}_j\]

其中 $ T^G_{I_k} $ 为扫描末 IMU 到全局的位姿

残差

\(z_j = \mathbf{G}_j\,(p^G_j - q^G_j)\)

其中:

  • $ \mathbf{G}_j $: 几何算子：平面用法向 $ u_j^T $，边可用方向/投影定义
  • $ q^G_j $: 几何体上的参考点（来自局部地图拟合）
  • $ z_j $: 残差（点到平面或边的距离/向量）

线性化

在 iEKF 的观测模型里，每个 LiDAR 特征点对应一条非线性的几何约束，例如：

\[r_j = h_j\left ( x \right ) + v_j\]

其中:

  • $ h_j $: 是用当前状态$x$预测第$j$个特征的观测值（比如点到平面的有向距离）
  • $ v_j $: 测量噪声

由于 $ h_j\left ( \cdot \right ) $ 通常涉及旋转、平移、坐标变换、最近邻拟合等操作，不可能直接在线性卡尔曼框架下用。iEKF 的做法是：在当前迭代状态 $x_k$ 处进行一阶泰勒展开:

\[h_j\left ( x_k \oplus \delta x \right ) \approx h_j\left ( x_k \right ) + H_j \delta x\]

对等式进行移相, 残差方程近似成：

\[0 \approx z_j + H_j\,\delta x_k + v_j\]
  • $z_j = h_j \left( x_k \right) - \text(实机测量)$（符号可能因定义不同取负号）
  • $H_j = \frac{\partial h_j}{\partial x} _{x_k} $ 是观测雅可比

MAP代价

线性化后，有：

  • 先验：来自前向传播的状态预测 $ x_k $及其协方差 $ P_k $ → 表示“别离当前预测太远”的统计约束
  • 观测约束：来自所有特征点的线性化残差

在高斯噪声假设下，最大后验估计（MAP）就等价于加权最小二乘：

\[\min_{\delta x_k}\ \ \|\delta x_k\|_{P_k^{-1}}^2 + \sum_{j=1}^m \|z_j + H_j\,\delta x_k\|_{R_j^{-1}}^2\]

其中:

  • $ \delta x_k $: 当前迭代的状态增量（切空间），表示要对当前状态作多少修正
  • $ H_j $: 观测雅可比
  • $ P_k $: 先验协方差
  • $ R_j $: 测量噪声协方差
  • $ v_j $: 测量噪声

第一项为先验惩罚偏离当前预测的增量；第二项为观测惩罚修正后残差不为零的情况，按测量噪声协方差 $ R_j $ 加权

卡尔曼增益

\(\text{标准:} \quad K = P H^\top (H P H^\top + R)^{-1}\) \(\text{新:} \quad K = \big(H^\top R^{-1} H + P^{-1}\big)^{-1}\,H^\top R^{-1}\) \(\delta x = Kz\) \(x \leftarrow x \oplus \delta x\) \(P \leftarrow (I - K H)\,P\)

其中:

  • $ H $: 所有测量拼接的雅可比
  • $ R $: 测量噪声块对角矩阵（各点独立）
  • $ P $: 先验协方差
新卡尔曼提速

在 LiDAR-IMU 紧耦合的 iEKF 中，一帧 LiDAR 特征点数 m 往往达到 1000~2000 级，而状态维度 n（姿态+位置+速度+偏置+重力）通常只有 ~18

标准卡尔曼增益的计算公式:

\[K = P H^\top (H P H^\top + R)^{-1}\]

其中:

  • $ P \in \mathbb{R}^{n \times n} $: 先验状态协方差（小矩阵）
  • $ H \in \mathbb{R}^{m \times n} $: 测量雅可比（行数 = 特征点数 m）
  • $ R \in \mathbb{R}^{m \times m} $: 测量噪声协方差（块对角）

关键是这个 $ (H P H^\top + R)^{-1} $ 是一个 $ m \times m $, 矩阵逆运算，复杂度$ O(m^3) $

FAST-LIO 直接从 MAP 二次优化目标出发：

\[\min_{\delta x}\left \| \delta x \right \| ^2_{P^{-1}} + \left \| z+H\delta x \right \| ^2_{R^{-1}}\]

对它求导得到法方程:

\[\left ( H^TR^{-1}H+P^{-1} \right ) \delta x = -H^{T}R^{-1}z\]

于是得到等价的“新”卡尔曼增益写法:

\[K = \left ( H^TR^{-1}H+P^{-1} \right ) ^{-1}H^TR^{-1}\]

这里要求逆的矩阵是 $n \times n$。维度与状态量有关，与测量点数无关