第2章：卡尔曼滤波回顾：线性系统下的最优估计，KF的五个核心公式及其物理意义
各位同学，咱们今天聊点基础但极其重要的东西——经典卡尔曼滤波。你可能会想，都2025年了，还讲这个老古董？
嗯，我理解你的想法。但说实话，我做了十几年多传感器融合，见过太多人一上来就搞无迹卡尔曼、粒子滤波，结果连最基础的KF都没吃透。最后出了问题，排查半天，发现是状态预测那一步就错了。
所以，咱们先把地基打牢。今天这章，我会把KF的五个核心公式掰开揉碎了讲，顺便聊聊我在实际项目中踩过的坑。
2.1 卡尔曼滤波到底在解决什么问题？
说白了，卡尔曼滤波干的事就一件：在充满噪声的测量中，找到系统状态的最优估计。
你想想看，我们做传感器融合，IMU有漂移，GPS有噪声，里程计有打滑。每个传感器都在说“我看到的才是真相”，但谁都不完全可信。卡尔曼滤波就像一个公正的裁判，它不偏信任何一方，而是根据各自的“可信度”（也就是协方差）来做加权平均。
我个人习惯把KF理解为一种“递归的、最优的、在线”的状态估计器。递归意味着它不需要记住所有历史数据，只靠上一时刻的状态和当前测量就能更新；最优是指在线性高斯假设下，它给出的估计方差最小；在线就是实时处理，来一个测量更新一次。
核心前提：卡尔曼滤波要求系统是线性的，且噪声服从高斯分布。如果这两个条件不满足，经典KF就会失效——这也是我们后面为什么要讲无迹卡尔曼滤波的原因。
2.2 五个核心公式：从预测到更新
卡尔曼滤波的五个公式，其实就干了两件事：预测和更新。预测是根据系统模型往前推，更新是用测量值来修正预测结果。
我曾经在项目里见过一个同事，把五个公式背得滚瓜烂熟，但问他“为什么卡尔曼增益要这么算”，他答不上来。这就是典型的知其然不知其所以然。咱们今天不仅要记住公式，更要理解每个符号背后的物理意义。
2.2.1 状态预测（先验估计）
x̂ₖ⁻ = A · x̂ₖ₋₁ + B · uₖ₋₁这个公式在干什么？说白了，就是用上一时刻的最优估计x̂ₖ₋₁，乘以状态转移矩阵A，再加上控制输入uₖ₋₁的影响（通过控制矩阵B）。
举个例子：假设你用一个轮式里程计估计机器人位置。上一时刻你知道机器人在x=1.0米处，速度是0.5m/s，时间间隔0.1秒。那么这一时刻的预测位置就是1.0 + 0.5×0.1 = 1.05米。这里的A就是[1, Δt; 0, 1]，B就是控制输入矩阵。
注意，这个预测值我们叫“先验估计”，符号上带个“⁻”，表示还没用测量值修正。它是有误差的，误差大小由下面的协方差矩阵来描述。
2.2.2 协方差预测
Pₖ⁻ = A · Pₖ₋₁ · Aᵀ + Q这个公式很多人觉得难理解，其实它就是描述“我们对预测结果有多不确定”。
Pₖ₋₁是上一时刻的估计协方差，代表上一时刻的不确定性。经过状态转移矩阵A的变换后，不确定性会传播。再加上过程噪声协方差Q——这个Q代表系统模型本身的不准确性。
我建议你记住一个直觉：预测过程一定会增加不确定性。因为模型不可能完美，你预测得越远，不确定性就越大。所以Pₖ⁻一定比Pₖ₋₁大（在矩阵意义上）。
调参小技巧：Q矩阵的取值很关键。Q设得太小，滤波器会过于相信模型，导致响应慢；Q设得太大，滤波器会过于相信测量，导致估计结果抖动剧烈。我一般会先根据物理模型估算一个理论值，然后在仿真中微调。
2.2.3 卡尔曼增益计算
Kₖ = Pₖ⁻ · Hᵀ · (H · Pₖ⁻ · Hᵀ + R)⁻¹这是整个KF最核心的公式。卡尔曼增益Kₖ决定了我们在多大程度上相信测量值。
你看这个公式的结构：分子是Pₖ⁻·Hᵀ（预测的不确定性映射到测量空间），分母是H·Pₖ⁻·Hᵀ + R（预测不确定性+测量噪声）。
所以Kₖ本质上是一个比值：预测不确定性 / (预测不确定性 + 测量不确定性)。
- 如果测量噪声R很小（传感器很准），分母≈预测不确定性，Kₖ≈1，我们更相信测量。
- 如果预测不确定性Pₖ⁻很小（模型很准），分子很小，Kₖ≈0，我们更相信预测。
嗯，这里要注意：Kₖ是一个矩阵，不是标量。在多传感器融合中，Kₖ的维度会随着测量维度的增加而变大。
2.2.4 状态更新（后验估计）
x̂ₖ = x̂ₖ⁻ + Kₖ · (zₖ - H · x̂ₖ⁻)这个公式的物理意义非常直观：最优估计 = 预测值 + 增益 × 残差。
残差(zₖ - H·x̂ₖ⁻)就是“测量值减去预测的测量值”，也叫新息（innovation）。它代表了测量带来的新信息。
如果残差很大，说明预测和测量差异很大，这时候Kₖ会决定我们如何折中。如果Kₖ大，我们就大幅修正预测值；如果Kₖ小，我们就小幅修正。
我曾经在一个GPS/IMU融合项目里，发现残差突然变得很大。排查后发现是GPS出现了多路径效应，测量值严重偏离真实值。这时候如果Kₖ还按正常情况计算，估计结果就会被带偏。所以实际工程中，我们通常会加一个“残差检测”逻辑，当残差超过某个阈值时，降低对应传感器的权重。
2.2.5 协方差更新
Pₖ = (I - Kₖ · H) · Pₖ⁻最后一步，更新协方差矩阵。它表示经过测量修正后，我们对状态估计的不确定性降低了多少。
你看，Pₖ = Pₖ⁻ - Kₖ·H·Pₖ⁻，因为Kₖ·H·Pₖ⁻是一个正定矩阵（在合理条件下），所以Pₖ ≤ Pₖ⁻。这说明测量确实帮助我们减小了不确定性。
这个公式还有一个等价形式，在数值稳定性上更好：
Pₖ = (I - Kₖ · H) · Pₖ⁻ · (I - Kₖ · H)ᵀ + Kₖ · R · Kₖᵀ这个形式叫“Joseph形式”，我在实际代码中更推荐使用它，因为能保证Pₖ始终是对称正定的，避免数值误差导致发散。
2.3 五个公式的完整流程
把五个公式串起来，就是一次完整的KF迭代：
| 步骤 | 公式 | 物理意义 |
|---|---|---|
| 1. 状态预测 | x̂ₖ⁻ = A·x̂ₖ₋₁ + B·uₖ₋₁ | 根据模型预测当前状态 |
| 2. 协方差预测 | Pₖ⁻ = A·Pₖ₋₁·Aᵀ + Q | 预测不确定性传播 |
| 3. 计算增益 | Kₖ = Pₖ⁻·Hᵀ·(H·Pₖ⁻·Hᵀ+R)⁻¹ | 计算预测与测量的权重比 |
| 4. 状态更新 | x̂ₖ = x̂ₖ⁻ + Kₖ·(zₖ - H·x̂ₖ⁻) | 用测量修正预测 |
| 5. 协方差更新 | Pₖ = (I - Kₖ·H)·Pₖ⁻ | 更新不确定性 |
这个流程每来一次测量就执行一次。如果测量频率比预测频率低（比如GPS 10Hz，IMU 100Hz），那就只做预测步骤，等有测量来了再做更新。
2.4 经典KF的局限性
讲到这里，我得泼一盆冷水。经典KF虽然优雅，但在实际工程中局限性很明显：
- 线性假设太强：现实世界几乎没有完美的线性系统。机器人运动是非线性的，传感器模型也是非线性的。强行用线性模型近似，误差会累积。
- 高斯噪声假设：实际传感器噪声往往有厚尾特性，不是严格的高斯分布。比如激光雷达的离群点，GPS的跳变。
- 雅可比矩阵计算麻烦：扩展卡尔曼滤波（EKF）虽然能处理非线性，但需要计算雅可比矩阵。对于高维系统，推导雅可比矩阵简直是噩梦。我当年做视觉SLAM时，光推导雅可比矩阵就花了两周，还经常算错。
避坑指南：我曾经在一个无人机姿态估计项目里，直接用经典KF处理四元数。结果发现滤波器频繁发散，因为四元数的更新本质上是非线性的旋转，用线性模型近似根本不行。后来改用无迹卡尔曼滤波，问题才解决。
2.5 从KF到UKF：为什么需要无迹变换？
经典KF处理不了非线性，EKF用泰勒展开近似，但精度有限且计算雅可比矩阵麻烦。那有没有一种方法，既不用算雅可比，又能处理非线性？
有，这就是无迹卡尔曼滤波（UKF）的核心思想——无迹变换。
无迹变换的思路很巧妙：我不去近似非线性函数，而是去近似概率分布。具体来说，就是选取一组确定的采样点（Sigma点），让这些点的均值和协方差等于原始分布的均值和协方差。然后把这些点通过非线性函数传播，再重新计算均值和协方差。
这样做的好处是：不需要计算雅可比矩阵，而且精度可以达到泰勒展开的三阶（对于高斯分布）。
嗯，关于无迹变换的具体细节，我们下一章再详细展开。今天先把经典KF的底子打好。
最后总结一下：卡尔曼滤波的五个公式，本质上就是“预测-测量-修正”的闭环。理解了这个闭环，你就理解了状态估计的精髓。下一章，我们正式进入无迹卡尔曼滤波的世界，看看它是如何优雅地处理非线性问题的。