第三课：线性卡尔曼滤波复习——预测与更新步骤详解
好，咱们进入正题。在讲扩展卡尔曼滤波之前，我强烈建议你把线性卡尔曼滤波的底子打扎实。说白了，扩展卡尔曼滤波就是在线性版本上打了个补丁，核心骨架完全一样。
我记得刚入行那会儿，带我的老工程师跟我说过一句话：「卡尔曼滤波就两个步骤，预测和更新。你把这俩搞明白了，剩下的全是数学细节。」当时我不信，后来做了十几个项目，发现还真是这么回事。
3.1 卡尔曼滤波的核心思想
你想想看，我们做传感器融合，本质上是在干什么？
是在「猜」状态。比如机器人现在的位置、速度、姿态。但我们有两个信息来源：一个是模型预测（比如根据上一时刻的位置和速度，猜现在应该在哪），另一个是传感器测量（比如GPS告诉你现在在哪）。
问题来了——两个都不准。模型有误差，传感器有噪声。怎么办？
卡尔曼老爷子想了个办法：加权平均。谁的置信度高，就多信谁一点。这个「置信度」就是协方差矩阵。
核心公式就五个，分成两组：
- 预测步骤（时间更新）： 用模型往前推一步
- 更新步骤（测量更新）： 用测量值修正预测结果
3.2 预测步骤——先猜一个
预测步骤说白了就是「盲猜」。我们根据系统的运动模型，从上一时刻的状态推算出当前时刻的状态。
公式长这样：
x̂ₖ|ₖ₋₁ = Fₖ · x̂ₖ₋₁|ₖ₋₁ + Bₖ · uₖ别被下标吓到。x̂ₖ|ₖ₋₁ 的意思是：在 k-1 时刻预测 k 时刻的状态。Fₖ 是状态转移矩阵，Bₖ 是控制输入矩阵，uₖ 是控制量。
举个例子。假设一个机器人在一维直线上运动，位置为 p，速度为 v。状态向量就是 [p, v]ᵀ。如果它匀速运动，那么：
pₖ = pₖ₋₁ + vₖ₋₁ · Δt
vₖ = vₖ₋₁写成矩阵形式：
F = [[1, Δt],
[0, 1 ]]
x̂ₖ|ₖ₋₁ = F · x̂ₖ₋₁|ₖ₋₁嗯，这里要注意：预测不只是状态值的预测，还有不确定性的预测。也就是协方差矩阵的更新：
Pₖ|ₖ₋₁ = Fₖ · Pₖ₋₁|ₖ₋₁ · Fₖᵀ + QₖQₖ 是过程噪声协方差矩阵。它代表了你对模型有多信任。Q 越大，说明模型越不准，后面就会更依赖测量值。
我的调参习惯： Q 矩阵我一般先设成单位矩阵乘以一个很小的数，比如 0.01。然后看滤波效果。如果跟踪太慢，就适当增大 Q；如果噪声太大，就减小 Q。我在做无人机高度估计时，Q 调了整整两天才找到合适的值。
3.3 更新步骤——用测量修正
预测完了，我们有了一个「先验估计」。现在传感器送来了一组测量值，我们要用这个测量值来修正预测结果。
更新步骤分三步走：
- 计算残差（innovation）： 测量值减去预测的测量值
- 计算卡尔曼增益： 决定信模型还是信传感器
- 更新状态和协方差： 得到后验估计
公式如下：
yₖ = zₖ - Hₖ · x̂ₖ|ₖ₋₁
Sₖ = Hₖ · Pₖ|ₖ₋₁ · Hₖᵀ + Rₖ
Kₖ = Pₖ|ₖ₋₁ · Hₖᵀ · Sₖ⁻¹
x̂ₖ|ₖ = x̂ₖ|ₖ₋₁ + Kₖ · yₖ
Pₖ|ₖ = (I - Kₖ · Hₖ) · Pₖ|ₖ₋₁这里 Hₖ 是观测矩阵，把状态空间映射到测量空间。Rₖ 是测量噪声协方差矩阵。
卡尔曼增益 Kₖ 是关键中的关键。它的大小决定了修正的力度：
- 如果 Kₖ 接近 0：说明测量噪声很大，我们更相信模型预测
- 如果 Kₖ 接近 1：说明模型误差大，我们更相信传感器测量
避坑指南： 我曾经在一个项目中，R 矩阵设得太小，导致卡尔曼增益过大，滤波器完全跟着测量值跑。结果测量值一有毛刺，状态估计就剧烈抖动。后来我把 R 调大了两个数量级，滤波曲线瞬间平滑了。
3.4 五个公式的直观理解
咱们用表格总结一下这五个公式，方便你对照记忆：
| 步骤 | 公式 | 直观含义 |
|---|---|---|
| 预测状态 | x̂ₖ|ₖ₋₁ = Fₖ · x̂ₖ₋₁|ₖ₋₁ + Bₖ · uₖ | 用模型往前推一步 |
| 预测协方差 | Pₖ|ₖ₋₁ = Fₖ · Pₖ₋₁|ₖ₋₁ · Fₖᵀ + Qₖ | 不确定性也在增长 |
| 计算增益 | Kₖ = Pₖ|ₖ₋₁ · Hₖᵀ · (Hₖ · Pₖ|ₖ₋₁ · Hₖᵀ + Rₖ)⁻¹ | 谁更准就信谁 |
| 更新状态 | x̂ₖ|ₖ = x̂ₖ|ₖ₋₁ + Kₖ · (zₖ - Hₖ · x̂ₖ|ₖ₋₁) | 用测量修正预测 |
| 更新协方差 | Pₖ|ₖ = (I - Kₖ · Hₖ) · Pₖ|ₖ₋₁ | 不确定性减小了 |
3.5 一个完整的单步示例
光说不练假把式。咱们写个简单的 Python 代码，模拟一维位置估计：
import numpy as np
# 初始化
x = np.array([[0], [1]]) # 位置0，速度1
P = np.eye(2) * 100 # 初始不确定性很大
F = np.array([[1, 1],
[0, 1]]) # Δt = 1
H = np.array([[1, 0]]) # 只观测位置
Q = np.eye(2) * 0.01 # 过程噪声
R = np.array([[1]]) # 测量噪声
# 模拟一次测量
z = np.array([[2.5]]) # 传感器测到位置2.5
# 预测步骤
x_pred = F @ x
P_pred = F @ P @ F.T + Q
# 更新步骤
y = z - H @ x_pred
S = H @ P_pred @ H.T + R
K = P_pred @ H.T @ np.linalg.inv(S)
x_upd = x_pred + K @ y
P_upd = (np.eye(2) - K @ H) @ P_pred
print(f"预测位置: {x_pred[0,0]:.3f}")
print(f"更新位置: {x_upd[0,0]:.3f}")
print(f"卡尔曼增益: {K[0,0]:.3f}")运行一下你会发现，预测位置是 1.0（因为上一时刻位置0，速度1），但测量值是 2.5。卡尔曼增益大约 0.99，说明滤波器非常相信测量值——因为初始协方差 P 设得很大，模型不可信。
注意： 实际项目中，初始 P 不要设得太大，否则前几步会有剧烈跳变。我一般根据物理常识设定，比如位置不确定度设为 1 米，速度不确定度设为 0.1 米/秒。
3.6 调参的核心思路
调参说白了就是调 Q 和 R 的比值。Q/R 比值越大，滤波器响应越快但越 noisy；比值越小，响应越慢但越 smooth。
我个人习惯的调参流程：
- 先固定 R，根据传感器手册标定值来设
- 从小到大调 Q，观察滤波曲线
- 找到「响应够快又不抖动」的临界点
- 如果还是不满意，再微调 R
你想想看，这其实就是在做权衡。没有完美的参数，只有适合你应用场景的参数。
好了，线性卡尔曼滤波的复习就到这。下一章咱们正式进入扩展卡尔曼滤波——当系统不再线性时，该怎么办。