3、卡尔曼滤波基础：状态空间模型、预测与更新步骤、卡尔曼增益的推导与直观理解

各位同学，欢迎来到卡尔曼滤波的世界。

说实话，我第一次接触卡尔曼滤波时，觉得这玩意儿挺玄乎的。一堆矩阵、协方差、增益系数，看着就头大。但后来在飞控项目里被噪声折磨得够呛，才真正体会到它的价值。今天咱们就把这块硬骨头啃下来。

3.1 状态空间模型：把问题装进数学框子里

卡尔曼滤波的核心，是状态空间模型。说白了，就是把一个动态系统用数学语言描述出来。

我习惯把系统想象成一辆小车。小车有位置、速度，这些就是状态量。你踩油门、打方向盘，这些是控制量。传感器测到的距离、速度，这些是观测量。

状态空间模型由两个方程组成：

  1. 状态方程：描述系统如何随时间演变
  2. 观测方程：描述传感器如何测量状态

写成数学形式：

x(k) = A * x(k-1) + B * u(k) + w(k)
z(k) = H * x(k) + v(k)

这里：

  • x(k)：k时刻的状态向量（比如位置、速度）
  • A：状态转移矩阵（描述上一时刻状态如何影响当前时刻）
  • B：控制输入矩阵（描述控制量如何影响状态）
  • u(k)：控制输入向量（比如油门、舵量）
  • w(k)：过程噪声（模型不准确的地方）
  • z(k)：观测向量（传感器读数）
  • H：观测矩阵（描述状态如何映射到观测）
  • v(k)：观测噪声（传感器本身的误差）

关键点：w(k)和v(k)都是高斯白噪声，均值为0，协方差分别为Q和R。这是卡尔曼滤波能工作的前提。

我在项目中遇到过一个问题：有人把状态量设得太多，结果滤波器发散。记住，状态量不是越多越好。你想想看，如果你不知道系统的某些状态，硬塞进去只会增加计算负担，还可能让滤波器不稳定。

3.2 预测步骤：先猜一个

卡尔曼滤波分两步走：预测和更新。咱们先看预测。

预测，就是根据上一时刻的状态，猜一下当前时刻的状态会是什么样。这就像你开车时，根据前一秒的速度和油门，估计下一秒的位置。

预测步骤有两个公式：

x_pred = A * x_est + B * u
P_pred = A * P_est * A^T + Q

解释一下：

  • x_pred：预测的状态（先验估计）
  • P_pred：预测的协方差矩阵（表示我们对预测的信任程度）
  • Q：过程噪声协方差矩阵

嗯，这里要注意。P_pred的公式里有个A^T，这是矩阵转置。为什么要有它？因为状态转移会改变不确定性的分布。说白了，你预测得越远，不确定性就越大。

个人经验：Q矩阵的调参是个技术活。Q设得太小，滤波器会过于相信模型，跟不上真实变化；Q设得太大，滤波器会过于相信测量，噪声抑制效果差。我一般先从单位矩阵开始，然后根据实际效果微调。

3.3 更新步骤：用测量修正

预测完了，我们有了一个猜测。但光猜不行，还得用传感器数据来修正。这就是更新步骤。

更新步骤有三个公式：

K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)
x_est = x_pred + K * (z - H * x_pred)
P_est = (I - K * H) * P_pred

这里：

  • K：卡尔曼增益（核心中的核心）
  • z：实际测量值
  • R：观测噪声协方差矩阵
  • I：单位矩阵

你可能会问：为什么要用(z - H * x_pred)？这叫残差或新息。它表示实际测量和预测测量之间的差异。差异越大，说明预测越不准，需要修正的幅度就越大。

避坑指南：我曾经在调试时发现滤波器输出剧烈抖动，查了半天才发现是R矩阵设得太小。R太小，滤波器会过度相信传感器，把噪声也当成真实信号。记住，R不是越小越好，要根据传感器的实际噪声水平来设。

3.4 卡尔曼增益的推导：从最小方差出发

卡尔曼增益K是整个滤波器的灵魂。它的推导其实不复杂，核心思想是最小化估计误差的方差。

我们想找到一个K，使得更新后的状态估计误差最小。误差的方差就是P_est矩阵的迹（对角线元素之和）。

推导过程：

  1. 写出更新后的状态估计：x_est = x_pred + K * (z - H * x_pred)
  2. 写出估计误差：e = x_true - x_est
  3. 写出误差协方差：P_est = E[e * e^T]
  4. 对P_est的迹求导，令导数为0
  5. 解出K

最终得到：

K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)

这个公式的直观理解是：

  • 如果预测很准（P_pred小），K就小，我们更相信预测
  • 如果测量很准（R小），K就大，我们更相信测量
  • K本质上是一个权重，在预测和测量之间做权衡

直观理解：卡尔曼增益就像是一个聪明的裁判。当预测和测量打架时，它根据谁更靠谱来分配话语权。预测靠谱就多听预测的，测量靠谱就多听测量的。

3.5 完整算法流程

把预测和更新串起来，就是完整的卡尔曼滤波流程：

// 初始化
x_est = x0
P_est = P0

// 循环
for k = 1 to N:
    // 预测
    x_pred = A * x_est + B * u
    P_pred = A * P_est * A^T + Q
    
    // 更新
    K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)
    x_est = x_pred + K * (z - H * x_pred)
    P_est = (I - K * H) * P_pred
    
    // 输出x_est作为当前时刻的估计值

这个流程看起来简单，但实际应用时有很多细节。我建议你先把一维情况搞明白，再扩展到多维。一维情况下，所有矩阵都退化成标量，公式会简单很多。

3.6 一个简单的例子

假设我们要估计一个无人机的高度。状态量是高度h和速度v，观测量是气压计测到的高度z。

状态空间模型：

h(k) = h(k-1) + v(k-1) * dt + w1
v(k) = v(k-1) + w2
z(k) = h(k) + v_noise

写成矩阵形式：

A = [1, dt; 0, 1]
H = [1, 0]
Q = [q1, 0; 0, q2]
R = [r]

初始化：

x_est = [0; 0]
P_est = [10, 0; 0, 10]  // 初始不确定性较大

然后按照上面的流程循环即可。你会发现，卡尔曼滤波能很好地融合气压计和加速度计的数据，得到比单一传感器更平滑、更准确的高度估计。

调试技巧：我习惯在仿真中先跑一遍，把预测值、测量值、估计值都画出来。如果估计值抖动厉害，检查R是不是太小；如果估计值滞后严重，检查Q是不是太大。调参没有捷径，多试几次就有感觉了。

好了，卡尔曼滤波的基础就讲到这里。下一章我们会讨论扩展卡尔曼滤波（EKF），看看如何处理非线性系统。记住，基础打牢了，后面的内容才能学得轻松。

← 上一章 📖 返回目录 下一章 →