3. 卡尔曼滤波基础：状态空间模型、预测与更新方程、协方差矩阵的物理意义

各位同学，欢迎来到漂移补偿的核心章节。

说实话，做定位系统这么多年，我见过太多人把卡尔曼滤波当成一个黑盒子。调几个参数，看效果还行，就完事了。但一旦遇到传感器突然跳变、或者信号长时间丢失，这个黑盒子就彻底崩了。我个人习惯是，先把这个滤波器的“骨架”搭清楚，再谈调参。

这一讲，我们就来拆解卡尔曼滤波的三个核心：状态空间模型、预测与更新方程，以及协方差矩阵的物理意义。你把这些搞懂了，漂移补偿就不再是玄学。

3.1 状态空间模型：把物理世界装进数学公式

卡尔曼滤波的第一步，是定义“状态”。

什么叫状态？说白了，就是你想估计的那些量。比如在定位里，最常见的就是位置和速度。我习惯用一个向量来表示：

X = [位置, 速度]^T

这个向量，就是我们的状态向量。它描述了系统在某一时刻的“完整情况”。

但光有状态还不够，你还得知道状态是怎么随时间变化的。这就引出了状态转移方程：

X_k = A * X_{k-1} + B * u_k + w_k

这里：

  • A 是状态转移矩阵。它告诉你上一时刻的状态，如何演变到当前时刻。比如匀速运动模型里，位置 = 上一时刻位置 + 速度 * 时间间隔。
  • B 是控制输入矩阵。如果你有加速度计或者方向盘转角，这就是控制量 u_k 的影响。
  • w_k 是过程噪声。记住，任何模型都不完美，这个噪声就是模型误差的体现。

我在项目中遇到过一个问题：有人把 A 矩阵写错了，导致滤波器发散。你想想看，模型本身就不对，滤波器怎么可能收敛？所以，状态空间模型是地基，地基歪了，楼就塌了。

3.2 预测与更新方程：两步走，步步为营

卡尔曼滤波的精髓，就是“预测-更新”的循环。我把它叫做“两步走”：

第一步：预测（Time Update）

这一步，我们只靠模型来猜。公式如下：

X_pred = A * X_prev + B * u
P_pred = A * P_prev * A^T + Q

这里：

  • X_pred 是预测的状态。说白了，就是“我猜现在应该在这里”。
  • P_pred 是预测的协方差矩阵。它代表“我对这个猜测有多不确定”。
  • Q 是过程噪声协方差矩阵。它量化了模型本身的误差。

嗯，这里要注意：Q 矩阵不能设为零。我曾经犯过这个错，觉得模型很准，就把 Q 设得很小。结果传感器一丢信号，滤波器直接“飘”到天边去了。因为模型太自信，不相信测量值了。

第二步：更新（Measurement Update）

这一步，我们用传感器数据来修正猜测。公式如下：

K = P_pred * H^T * (H * P_pred * H^T + R)^{-1}
X_upd = X_pred + K * (z - H * X_pred)
P_upd = (I - K * H) * P_pred

这里：

  • K 是卡尔曼增益。它是整个滤波器的“大脑”，决定了你更相信模型还是更相信传感器。
  • z 是测量值，比如 GPS 给出的位置。
  • H 是测量矩阵，它把状态空间映射到测量空间。
  • R 是测量噪声协方差矩阵。它代表传感器有多“吵”。

你看，整个更新过程其实就一句话：最终估计 = 预测值 + 增益 * (测量值 - 预测值)。这个差值 (z - H*X_pred) 叫做“新息”，它代表了传感器告诉你的新信息。

核心理解：卡尔曼滤波不是简单地平均预测和测量，而是根据它们各自的“可信度”来加权。可信度高的，权重就大。

3.3 协方差矩阵的物理意义：不确定性的“地图”

很多同学觉得协方差矩阵 P 很抽象。我换个说法：P 矩阵就是一张不确定性地图。

假设你的状态是 [位置, 速度]。那么 P 矩阵就是 2x2 的：

P = [[σ_xx, σ_xv],
     [σ_vx, σ_vv]]

这里：

  • σ_xx 是位置的不确定性（方差）。值越大，说明你对位置越没底。
  • σ_vv 是速度的不确定性。
  • σ_xv 和 σ_vx 是位置和速度的协方差。它告诉你：位置和速度的变化是否相关。

举个例子。你在高速公路上开车，位置误差和速度误差是强相关的。因为速度估计偏大，位置估计就会偏大。这时候 σ_xv 就是正值。反过来，如果你在频繁刹车，这个相关性可能就变弱了。

避坑指南：我曾经在调试一个无人机定位系统时，发现 P 矩阵对角线元素一直不收敛。排查了半天，发现是 Q 矩阵设得太大了，导致每次预测都引入大量不确定性。记住：P 矩阵的收敛性，直接反映了滤波器是否稳定。如果 P 一直震荡或者发散，赶紧检查 Q 和 R 的比值。

3.4 一张图看懂卡尔曼滤波

说了这么多，不如一张图来得直观。下面这张 SVG 图，展示了卡尔曼滤波的完整流程：

卡尔曼滤波核心流程 初始状态 X₀, P₀ 预测步骤 (Time Update) X_pred = A·X + B·u P_pred = A·P·Aᵀ + Q 计算卡尔曼增益 K K = P_pred·Hᵀ·(H·P_pred·Hᵀ + R)⁻¹ 更新步骤 (Measurement Update) X_upd = X_pred + K·(z - H·X_pred) P_upd = (I - K·H)·P_pred 循环迭代

这张图里，从上到下就是一次完整的“预测-更新”循环。你想想看，每次传感器数据到来，我们就走一遍这个流程。随着时间的推移，P 矩阵会逐渐收敛，你对状态的估计也会越来越准。

3.5 小结：三个核心，一个整体

好了，我们来捋一捋：

  • 状态空间模型：把物理系统装进数学公式。模型不准，一切白搭。
  • 预测与更新方程：两步走，先猜后修。卡尔曼增益 K 是核心，它决定了信任谁。
  • 协方差矩阵 P：不确定性的地图。它告诉你滤波器有多“自信”，也告诉你哪里需要调整。

我个人觉得，卡尔曼滤波最迷人的地方就在这里：它把“不确定性”量化了，并且用数学的方式去管理它。做定位系统这么多年，我越来越觉得，真正的漂移补偿，不是消除误差，而是学会与不确定性共舞。

警告：千万不要把卡尔曼滤波当成万能药。如果你的传感器噪声特性变了（比如 GPS 信号变差），记得动态调整 R 矩阵。否则，滤波器会“睡着”——它对新的测量值不再敏感，漂移补偿也就失效了。

下一节，我们会把这些理论落实到代码里。到时候，你会看到这些公式是如何一行一行跑起来的。

可可精品资料库

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