从零推导FAST-LIO的观测雅可比矩阵

在FAST-LIO的误差状态迭代卡尔曼滤波（ESIKF）框架中，观测方程将激光雷达点云与全局地图关联起来。观测残差对误差状态的雅可比矩阵（通常记为 H）是更新步骤的核心。正确推导这一矩阵是理解代码中 esekfom.hpp 里 observe 函数实现的关键。本文从点到平面的残差定义出发，逐步推导出 H 的闭式表达式。

一、点到平面的残差定义

设当前帧（扫描结束时刻 t_k）中一个激光雷达点 L_k p_f（在雷达坐标系 L_k 中表示）。利用当前估计的状态（名义状态 x_hat）将它转换到全局坐标系 G 中：

text

复制代码

G p_f_hat = G T_I_hat · I T_L · L_k p_f

其中：

G T_I_hat = (G R_I_hat, G p_I_hat) 是IMU坐标系到全局坐标系的位姿变换（待优化）；
I T_L = (I R_L, I p_L) 是激光雷达到IMU的外参（通常事先标定或在线估计）。

为了简化符号，定义：

text

复制代码

G p_f = G R_I · (I R_L · L p_f + I p_L) + G p_I

在全局地图中，已经建立了局部平面模型。假设为点 L_k p_f 在地图中找到一个近邻平面，其单位法向量为 n（在全局坐标系下），且平面上一点为 q。点到平面的有符号残差为：

text

复制代码

z = n^T · (G p_f - q)

理论上，如果状态量和点云均无误差，残差应为零。考虑测量噪声 n_f（激光雷达测距噪声），真实点位置为 L p_f_gt = L p_f - L n_f。代入残差定义并令其为零，得到：

text

复制代码

0 = n^T · (G T_I · I T_L · (L p_f - L n_f) - q)

我们的目标是在当前估计状态 x_hat^k 附近线性化上式，得到观测方程的标准形式：

text

复制代码

0 = z^k + H^k · x_tilde^k + v

其中 x_tilde^k 是误差状态（切空间中的小量），z^k = n^T·(G p_f_hat - q) 是当前残差，v 是噪声项。

二、链式法则分解雅可比

根据链式法则，观测雅可比 H 可以分解为：

text

复制代码

H = ∂z / ∂x_tilde = (∂z / ∂G p_f) · (∂G p_f / ∂x_tilde)

第一项很简单：

text

复制代码

∂z / ∂G p_f = n^T

第二项 ∂G p_f / ∂x_tilde 是核心。误差状态 x_tilde 通常定义为 (δθ, δp, δv, δbω, δba, ...)，其中 δθ 是旋转误差（三维向量，对应李代数扰动），δp 是位置误差。需要知道全局点 G p_f 对这些误差分量的导数。

三、利用右扰动模型求旋转部分的导数

考虑状态中的旋转部分。设名义旋转为 G R_I_hat，真实旋转为 G R_I = G R_I_hat · Exp(δθ)（右乘扰动模型）。将 G p_f 显式写为：

text

复制代码

G p_f = G R_I · t + G p_I

其中 t = I R_L · L p_f + I p_L（一个三维向量，不依赖于待估计的旋转）。

对旋转误差 δθ 的偏导数（在 δθ=0 处）利用右乘扰动模型：

text

复制代码

∂G p_f / ∂δθ = lim_{δθ→0} [ G R_I_hat·Exp(δθ)·t - G R_I_hat·t ] / δθ

根据李代数导数性质，该导数为 - G R_I_hat · (t)^∧，其中 ( )^∧ 表示向量的反对称矩阵。因此旋转部分的导数为：

text

复制代码

∂G p_f / ∂δθ = - G R_I_hat · ( I R_L · L p_f + I p_L )^∧

位置误差 δp 直接作用在全局平移 G p_I 上，有：

text

复制代码

∂G p_f / ∂δp = I_3x3

其他误差分量（速度、偏置等）不影响 G p_f，导数为零。

四、组合得到完整的 H 矩阵

将上述结果代入链式法则，得到观测雅可比 H 对旋转和平移的分量：

text

复制代码

H_δθ = n^T · ∂G p_f / ∂δθ = - n^T · G R_I_hat · ( I R_L · L p_f + I p_L )^∧H_δp = n^T · I = n^T

其余分量为 0（1行3列）。因此，对于一个观测（一个特征点），H 是一个 1×18 的行向量（因为残差是标量）。若将所有 m 个特征点的残差堆叠，H 为 m×18 矩阵。在代码实现中，通常按行构建：

text

复制代码

H.block<1,3>(i,0) = - n.transpose() * R_GI * ( R_IL * p + T_IL ).skew();

复制代码

H.block<1,3>(i,3) = n.transpose();

复制代码

// 第6~17列均为零（对应速度、偏置等状态）

其中 .skew() 是计算反对称矩阵的函数。

五、与代码实现对照

在 FAST-LIO 源码的 laserMapping.cpp 或 esekfom.hpp 中，观测雅可比的计算通常出现在迭代更新循环内。典型代码片段如下（取自 S-FAST_LIO）：

cpp

Eigen::Matrix<double, 1, 18> H;

H.setZero();

// 计算旋转部分雅可比

Eigen::Vector3d point_imu = R_il * p_lidar + T_il; // 将雷达点转到IMU坐标系

Eigen::Vector3d point_global = R_gi * point_imu + T_gi; // 转到全局坐标系

// 最近邻平面法向量 n

H.block<1,3>(0,0) = - n.transpose() * R_gi * Sophus::SO3d::hat(point_imu);

H.block<1,3>(0,3) = n.transpose();

注意 Sophus::SO3d::hat(point_imu) 正是反对称矩阵 (t)^∧。

六、总结

通过链式法则和右扰动模型，我们推导出 FAST-LIO 观测雅可比矩阵的简洁形式：

text

复制代码

H = [ -n^T · R_GI · (t)^∧ , n^T , 0_{1×12} ]

其中 t = R_IL · p_L + T_IL 。这一形式在代码中直接实现，是理解滤波更新的基础。结合 ESKF 的运动雅可比（预测步），即可完整掌握 FAST-LIO 的状态估计流程。

参考资料

FAST-LIO 论文附录 B（雅可比推导）
S-FAST_LIO 代码 esekfom.hpp 中的 Observe 函数
《视觉SLAM十四讲》第4讲：李群与李代数，第9讲：卡尔曼滤波
CSDN博客《FAST-LIO 观测雅可比推导细节》