3. 状态估计数学基础
FAST-LIO 的核心任务，是在激光雷达与 IMU 数据不断到来的过程中，实时回答一个问题：机器人当前的位置、姿态、速度、零偏究竟是多少？由于传感器有噪声、运动模型有误差、数据存在时间延迟，我们无法得到唯一确定的真值，只能给出一个“最可能的估计”以及“这个估计有多不确定”。这正是状态估计要解决的数学问题，也是卡尔曼滤波系列方法在 FAST-LIO 中反复出现的原因。
一、概率论：把“不确定”变成可计算的量
状态估计的第一步，是把待求量视为随机变量。设机器人在 k 时刻的真实状态为 x_k，它包含位置、姿态、速度、加速度计零偏、陀螺仪零偏等。我们无法直接观测 x_k，只能通过 IMU 得到预测、通过激光雷达得到观测。概率论提供了描述这种不确定性的语言：
- 先验概率
p(x_k)：在获得当前观测之前，对状态的信念，通常来自上一时刻估计与运动模型预测。 - 似然
p(z_k | x_k)：假设状态为x_k时，观测z_k出现的可能性，由传感器噪声模型决定。 - 后验概率
p(x_k | z_k)：融合观测后对状态的信念，是滤波真正要输出的结果。
三者由贝叶斯公式连接：p(x_k | z_k) = p(z_k | x_k) p(x_k) / p(z_k)。分母只是归一化常数，真正决定估计结果的是“先验”和“似然”的乘积。换句话说，滤波不是简单地相信传感器，而是让模型预测与观测证据按各自的不确定度加权融合。
二、高斯分布：让贝叶斯滤波可实时计算
如果对任意概率分布做贝叶斯递推，计算量会随状态维数指数增长，无法满足 FAST-LIO 的实时性。工程上采用一个关键假设：状态和噪声都服从高斯分布。高斯分布只需均值和协方差两个参数即可完整描述：
x ~ N(μ, Σ)
μ：状态均值，即“最可能的值”
Σ：协方差矩阵，对角线为各状态方差，非对角线为状态间相关性高斯假设带来两个重要性质：一是高斯分布经过线性变换后仍是高斯分布；二是两个高斯分布相乘后仍是高斯分布。因此，预测和更新都可以只用均值和协方差递推，计算复杂度可控。协方差矩阵 Σ 在 FAST-LIO 中非常关键，它既表示当前估计的不确定度，也决定后续观测应以多大权重修正状态。
三、贝叶斯滤波与卡尔曼滤波的关系
贝叶斯滤波是理论框架，卡尔曼滤波是它在“线性系统 + 高斯噪声”条件下的解析实现。贝叶斯滤波分为两步：
- 预测：用运动模型把上一时刻后验推到当前时刻，得到先验
p(x_k | z_{1:k-1})。 - 更新：用当前观测
z_k修正先验，得到后验p(x_k | z_{1:k})。
卡尔曼滤波把这两步写成均值和协方差的闭式公式。设状态转移矩阵为 F，控制输入为 u，过程噪声协方差为 Q，观测矩阵为 H，观测噪声协方差为 R，则标准卡尔曼滤波可概括为：
预测：
x_pred = F * x_prev + B * u
P_pred = F * P_prev * F^T + Q
更新：
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)
x_new = x_pred + K * (z - H * x_pred)
P_new = (I - K * H) * P_pred其中 K 是卡尔曼增益，它自动权衡“相信预测”还是“相信观测”：当观测噪声 R 很大时，K 变小，状态更依赖预测；当预测协方差 P_pred 很大时，K 变大，状态更依赖观测。这个自动加权机制，是卡尔曼滤波在多传感器融合中如此有效的原因。
四、扩展卡尔曼滤波：FAST-LIO 中的非线性处理
真实机器人系统几乎都是非线性的：姿态用旋转矩阵或四元数表示，IMU 积分涉及角速度与重力耦合，激光点到地图的残差也是非线性的。标准卡尔曼滤波只能处理线性模型，因此需要扩展卡尔曼滤波（EKF）。EKF 的核心思想是：在当前估计点附近，用一阶泰勒展开把非线性函数线性化，再套用卡尔曼滤波公式。
设非线性运动模型为 x_k = f(x_{k-1}, u_k) + w_k，非线性观测模型为 z_k = h(x_k) + v_k。EKF 用雅可比矩阵代替线性矩阵：
F_k = ∂f/∂x |_{x_{k-1}}
H_k = ∂h/∂x |_{x_pred}
预测：
x_pred = f(x_prev, u)
P_pred = F_k P_prev F_k^T + Q
更新：
K = P_pred H_k^T (H_k P_pred H_k^T + R)^(-1)
x_new = x_pred + K (z - h(x_pred))
P_new = (I - K H_k) P_pred在 FAST-LIO 中，状态维度较高，包含姿态、位置、速度、重力、零偏等，直接构造大矩阵并求逆代价很大。因此 FAST-LIO 采用迭代扩展卡尔曼滤波（IEKF）与流形上的误差状态更新，把状态更新放在切空间中进行，既避免四元数过参数化，又通过多次迭代提升非线性观测下的收敛精度。理解 EKF 的线性化思想，是读懂 FAST-LIO 更新公式的前提。
五、核心概念对比与常见误区
| 方法 | 系统假设 | 核心操作 | 在 FAST-LIO 中的角色 |
|---|---|---|---|
| 贝叶斯滤波 | 任意分布、非线性 | 预测与更新概率分布 | 理论框架，说明融合的本质 |
| 卡尔曼滤波 | 线性、高斯噪声 | 均值与协方差闭式递推 | 提供增益与协方差更新结构 |
| 扩展卡尔曼滤波 | 非线性、高斯噪声 | 一阶线性化 + 卡尔曼递推 | 处理 IMU 积分与激光残差非线性 |
| 迭代扩展卡尔曼滤波 | 强非线性、高斯噪声 | 在更新步多次重线性化 | FAST-LIO 实际采用的更新方式 |
常见误区一：把协方差当成“误差大小”的单一标量。协方差矩阵的非对角线元素表示状态之间的相关性，例如位置与速度、姿态与零偏之间的耦合。忽略相关性会导致滤波增益计算错误，估计发散。
常见误区二：认为 EKF 线性化一次就够。当观测残差较大或初始误差较大时，一阶线性化误差显著，需要迭代重线性化，这也是 IEKF 在 FAST-LIO 中必要的原因。
常见误区三：Q 和 R 随便设。Q 过小会使滤波器过度相信预测，观测修正不足；R 过小会使滤波器过度相信单次激光观测，容易被外点带偏。二者需要根据 IMU 噪声密度和激光点云精度合理标定。
学习建议：先用手写一维匀速运动模型实现标准卡尔曼滤波，观察 K、P 随 Q、R 的变化；再把观测模型改成非线性，手动推导雅可比矩阵并实现 EKF。完成这两步后，再阅读 FAST-LIO 的 update 与 predict 代码，会顺畅很多。
总结来看，概率论提供不确定性描述，高斯分布让递推可计算，贝叶斯滤波给出预测—更新框架，卡尔曼滤波给出线性高斯下的闭式解，扩展卡尔曼滤波把这一套推广到非线性系统。FAST-LIO 正是在 IEKF 框架下，把 IMU 预测与激光观测按协方差加权融合，从而在实时条件下获得稳定、连续、可用的位姿估计。