3. 卡尔曼滤波五大公式详解：预测与更新步骤的数学推导与物理意义
各位同学，欢迎来到第三章。前两章我们聊了多传感器融合的“为什么”和“是什么”，今天终于要啃硬骨头了——卡尔曼滤波的五大公式。
说实话，我第一次接触这五个公式时，感觉像在看天书。什么状态预测、协方差更新，一堆矩阵符号看得人头皮发麻。但后来我在实际项目中调试IMU+GPS融合时，才真正体会到：这五个公式，其实就是一套“猜-测-改”的循环逻辑。
今天我就带着大家，把这五个公式掰开揉碎了讲。咱们不搞纯数学推导，而是结合物理意义，让你知道每个公式到底在干什么。
3.1 卡尔曼滤波的两大步骤：预测与更新
卡尔曼滤波的核心思想，说白了就是两步走：
- 预测（Predict）：根据上一时刻的状态，猜一下当前时刻的状态。
- 更新（Update）：用当前时刻的测量值，修正刚才的猜测。
你想想看，这像不像你开车时判断位置？你根据上一秒的速度和方向，估计下一秒大概在哪（预测）。然后看一眼路牌或GPS（测量），发现估计有偏差，于是调整一下（更新）。
嗯，这里要注意：预测和更新是交替进行的。没有预测，更新就无从谈起；没有更新，预测会越偏越远。
3.2 五大公式概览
先给大家一个全景图。卡尔曼滤波的五大公式分为两组：
| 步骤 | 公式编号 | 公式内容 | 物理意义 |
|---|---|---|---|
| 预测 | 公式1 | \(\hat{x}_{k|k-1} = F_k \hat{x}_{k-1|k-1} + B_k u_k\) | 状态预测：根据上一时刻状态，猜当前状态 |
| 公式2 | \(P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k\) | 协方差预测：猜的不确定性有多大 | |
| 更新 | 公式3 | \(K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1}\) | 卡尔曼增益：测量值和预测值，该信谁 |
| 公式4 | \(\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - H_k \hat{x}_{k|k-1})\) | 状态更新：用测量值修正预测值 | |
| 公式5 | \(P_{k|k} = (I - K_k H_k) P_{k|k-1}\) | 协方差更新：修正后的不确定性变小了 |
看着有点晕？别急，我一个一个拆开讲。
3.3 预测步骤（公式1和公式2）
公式1：状态预测
\(\hat{x}_{k|k-1} = F_k \hat{x}_{k-1|k-1} + B_k u_k\)
这个公式在干什么？说白了就是：用上一时刻的状态，推算当前时刻的状态。
- \(\hat{x}_{k-1|k-1}\)：上一时刻的最优估计值（带“帽子”表示估计值）
- \(F_k\)：状态转移矩阵。它描述了系统状态如何随时间变化。比如匀速运动模型中，位置 = 上一时刻位置 + 速度 × 时间差
- \(B_k u_k\)：控制输入项。比如你踩了油门（控制量），速度会增加
- \(\hat{x}_{k|k-1}\)：预测的当前状态（注意下标：k|k-1 表示“用k-1时刻的信息预测k时刻”）
我记得第一次做机器人定位时，用的就是最简单的匀速模型。F矩阵就是 [1, dt; 0, 1]，把位置和速度一起预测。当时觉得这公式太简单了，不就是牛顿运动定律嘛！
公式2：协方差预测
\(P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k\)
这个公式很多人觉得难理解。其实它就是在问：我猜的到底有多不靠谱？
- \(P_{k-1|k-1}\)：上一时刻估计的不确定性（协方差矩阵）
- \(F_k P_{k-1|k-1} F_k^T\)：上一时刻的不确定性，经过状态转移后，变成了当前时刻的不确定性
- \(Q_k\)：过程噪声协方差。它表示模型本身的不确定性——比如你假设匀速运动，但实际可能有风、有摩擦
你想想看，为什么预测之后，不确定性会变大？因为预测本身就是一种猜测，猜的时间越长，误差越大。所以 \(P_{k|k-1}\) 一定比 \(P_{k-1|k-1}\) 大（除非Q=0，但现实中不可能）。
3.4 更新步骤（公式3、4、5）
公式3：卡尔曼增益
\(K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1}\)
这是整个卡尔曼滤波的“灵魂”公式。卡尔曼增益 \(K_k\) 决定了：测量值和预测值，到底该信谁？
- \(H_k\)：观测矩阵。它把状态空间映射到测量空间。比如你测量的是位置，但状态包含位置和速度，H就是 [1, 0]
- \(R_k\)：测量噪声协方差。它表示传感器的精度——GPS精度高，R就小；超声波精度低，R就大
- \(K_k\)：取值范围在0到1之间（对于标量情况）
为什么说它是灵魂？因为：
- 如果测量很准（R小），\(K_k\) 接近1，滤波器更相信测量值
- 如果预测很准（P小），\(K_k\) 接近0，滤波器更相信预测值
说白了，卡尔曼增益就是一个“信任分配器”。我在做GPS+IMU融合时，GPS信号好的时候R设小，K变大；GPS信号差的时候R设大，K变小，自动切换信任源。
公式4：状态更新
\(\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - H_k \hat{x}_{k|k-1})\)
这个公式的物理意义非常直观：用测量值修正预测值。
- \(z_k\)：当前时刻的实际测量值
- \(H_k \hat{x}_{k|k-1}\)：预测值在测量空间的投影（即“我猜测量值应该是多少”）
- \(z_k - H_k \hat{x}_{k|k-1}\)：残差（也叫新息），表示“实际测量和预测测量的差距”
- \(K_k \times 残差\)：修正量
你看，这个公式就是在说：先猜一个值，然后看看实际测量和猜的差多少，再乘以一个权重（K），最后加到预测值上。
嗯，这里要注意：如果残差很大，说明预测不准，修正量就大；如果残差很小，说明预测很准，几乎不用修正。
公式5：协方差更新
\(P_{k|k} = (I - K_k H_k) P_{k|k-1}\)
这个公式在说：经过测量修正后，不确定性变小了。
- \(I\)：单位矩阵
- \(K_k H_k\)：一个矩阵，表示“测量带来的信息量”
- \(I - K_k H_k\)：表示“剩余的不确定性比例”
为什么不确定性会变小？因为测量提供了新的信息，减少了我们对状态的无知。你想想看，如果你只看地图（预测），你只知道大概位置；但如果你看了一眼路牌（测量），你的位置就更确定了。
我记得有一次在调试激光雷达+里程计融合时，发现协方差更新后几乎不变。排查了半天，发现是H矩阵写错了，导致测量信息没有被正确注入。从那以后，我每次都会打印协方差矩阵的迹，看看它是不是在收敛。
3.5 五大公式的完整流程
最后，我把这五个公式串成一个完整的循环：
- 预测步骤：
- 公式1：猜当前状态 \(\hat{x}_{k|k-1}\)
- 公式2：算猜的有多不准 \(P_{k|k-1}\)
- 更新步骤：
- 公式3：算卡尔曼增益 \(K_k\)（该信谁）
- 公式4：用测量修正状态 \(\hat{x}_{k|k}\)
- 公式5：更新不确定性 \(P_{k|k}\)
- 回到步骤1，进入下一时刻
这个循环每来一个测量值就执行一次。如果你用的是多传感器，比如同时有GPS和IMU，你可以把它们的测量值依次送入更新步骤，或者用扩展方法处理。
3.6 小结
今天这五个公式，是卡尔曼滤波的“内功心法”。你不需要死记硬背，但要理解每个公式的物理意义：
- 预测：根据模型猜状态，同时算猜的有多不准
- 卡尔曼增益：决定信预测还是信测量
- 更新：用测量修正预测，同时降低不确定性
下一章，我们会把这些公式应用到实际的多传感器融合案例中。到时候你会发现，原来这五个公式真的能解决实际问题。
好了，今天就到这里。如果你在理解公式时遇到困难，不妨画个流程图，把每个变量的维度标出来。我保证，画着画着就通了。