第 4 篇：卡尔曼滤波（上）——用直觉理解 5 个公式

卡尔曼滤波是嵌入式领域"含金量"最高的算法之一。 面试常考，项目加分，无人机、机器人、自动驾驶都在用。但很多人被数学劝退了——这篇只用图讲原理，下一篇写代码。

---

1. 一句话理解卡尔曼

用"预测值"和"测量值"的加权平均，但权重（卡尔曼增益）是动态计算的——谁更可靠就多信谁。

假设你在走廊里闭眼走路：

你的预测（步速×时间）：  "我应该走了 10 米"
你的测量（摸到墙）：      "墙在 12 米处"

问题：你到底走了多远？

如果步速很准（预测可信） → 信预测多一点 → 最终估计 ≈ 10.3 米
如果步速不准（预测不可信） → 信测量多一点 → 最终估计 ≈ 11.7 米

卡尔曼做的事情：根据"预测的不确定性"和"测量的不确定性"，自动算出最优权重。

---

2. 卡尔曼滤波的 5 个公式（附直觉解释）

预测阶段（先猜一个）

公式 1：状态预测
x̂⁻[k] = A × x̂[k-1] + B × u[k]

直觉：用上一时刻的状态 + 控制输入，预测当前状态。
例子：小车位置 = 上次位置 + 速度×dt

公式 2：协方差预测
P⁻[k] = A × P[k-1] × Aᵀ + Q

直觉：预测的不确定性 = 上次的不确定性经过系统传递 + 过程噪声 Q。
Q 的含义：系统本身有多"乱"（比如风、摩擦力变化）

更新阶段（用测量修正）

公式 3：卡尔曼增益
K = P⁻ × Hᵀ / (H × P⁻ × Hᵀ + R)

直觉：K = 预测的不确定性 / (预测不确定性 + 测量不确定性)
     = "测量值在最终估计中占多大比重"

如果测量噪声 R 很大 → K 变小 → 更信预测
如果预测噪声 P⁻很大 → K 变大 → 更信测量

公式 4：状态更新
x̂[k] = x̂⁻ + K × (z - H × x̂⁻)

直觉：最终估计 = 预测值 + K ×（测量值 - 预测值）
这就是"加权修正"！

公式 5：协方差更新
P[k] = (I - K × H) × P⁻

直觉：经过测量修正后，不确定性降低了。
    如果 K 很大（很信测量）→ P 大幅减小
    如果 K 很小（不太信测量）→ P 几乎不变

---

3. 一维卡尔曼——用数字跑一遍

一维情况下，A=1, H=1, B=0，公式简单很多：

预测：x̂⁻ = x̂
     P⁻ = P + Q

更新：K  = P⁻ / (P⁻ + R)
     x̂ = x̂⁻ + K × (z - x̂⁻)
     P  = (1 - K) × P⁻

手算示例（测量一个 20℃ 的温度，传感器有 ±3℃ 噪声）：

初始: x̂=20, P=5（完全不确定）

第 1 次测量 z=25℃, Q=0.1, R=9 (传感器噪声方差=3²=9)
  预测: x̂⁻=20, P⁻=5.1
  K = 5.1/(5.1+9) = 0.36
  估计: x̂ = 20 + 0.36×(25-20) = 21.8℃  ← 偏信了预测（K只有0.36）
  P = (1-0.36)×5.1 = 3.3

第 2 次测量 z=24℃
  预测: x̂⁻=21.8, P⁻=3.4
  K = 3.4/(3.4+9) = 0.27
  估计: x̂ = 21.8 + 0.27×(24-21.8) = 22.4℃
  P = (1-0.27)×3.4 = 2.5  ← 置信度持续提高

第 3 次测量 z=22℃
  K = 2.6/11.6 = 0.22
  估计: 22.4 + 0.22×(22-22.4) = 22.3℃

第 4 次测量 z=21℃
  估计收敛到 ≈ 21.8℃  ← 接近真实值 22℃

关键观察：

• K 从 0.36 → 0.27 → 0.22 逐步减小
• 滤波器越来越"自信"（P 在减小）
• 收敛后即使有噪声，也不会抖动太大

---

4. Q 和 R 怎么理解？（最重要的调参）

Q = 过程噪声协方差 → "我有多不信任我的模型"
    Q 大 → 更信测量值 → 响应快但噪声大
    Q 小 → 更信模型预测 → 平滑但响应慢

R = 测量噪声协方差 → "我有多不信任传感器"
    R 大 → 不信任测量值 → 平滑但响应慢
    R 小 → 相信测量值 → 响应快但噪声大

调参口诀：

现象	原因	调整
滤波值跟不上真实值	Q 太小	增大 Q
滤波值噪声太大	R 太小 / Q 太大	增大 R 或减小 Q
收敛太慢	Q 太小 且 R 太大	增大 Q
超调震荡	Q 太大	减小 Q

经验值：先固定 R（用传感器数据手册的噪声方差），再调 Q。

---

5. 卡尔曼 vs 一阶低通 vs 滑动平均

	滑动平均	一阶低通	卡尔曼
代码量	15 行	5 行	30 行
内存	N×4 字节	4 字节	20 字节
动态权重	❌ 固定	❌ 固定	✅ 自适应
需要调参	窗口 N	α	Q, R
适用场景	慢变信号	通用	需要最优估计

结论：大部分场景一阶低通就够了。卡尔曼用在"你真的很在意精度"的场景——导航、定位、多传感器融合。

---

下一篇：卡尔曼滤波（下）——从一维到多维 C 代码，MPU6050 角度估计实战