4. 卡尔曼滤波基础：状态空间模型、线性卡尔曼滤波(KF)推导、预测与更新步骤、滤波器调参经验

4.1 状态空间模型——把物理世界装进数学盒子

做传感器融合这么多年，我越来越觉得，卡尔曼滤波本质上就是「用数学给物理世界拍一张X光片」。你想想看，我们手里的传感器数据，哪个不是带着噪声的？GPS飘个几米、IMU零偏慢慢跑、雷达偶尔跳个野值……这些乱七八糟的信号背后，其实藏着一个真实的状态。

状态空间模型就是干这个的。它把系统拆成两部分：

  • 状态方程：描述系统怎么演化。比如「上一秒的位置 + 速度 × 时间 = 下一秒的位置」
  • 观测方程：描述传感器怎么看到状态。比如「GPS测到的位置 = 真实位置 + 噪声」

用数学写出来就是：

x_k = A * x_{k-1} + B * u_k + w_k      // 状态方程
z_k = H * x_k + v_k                     // 观测方程

这里x_k是状态向量，A是状态转移矩阵，B是控制输入矩阵，u_k是控制量，w_k是过程噪声。z_k是观测值，H是观测矩阵，v_k是观测噪声。

核心假设：w_k和v_k都是零均值高斯白噪声，且互不相关。这个假设在实际中不一定成立，但——嗯，先按规矩来，后面再讲怎么放松。

我在项目中遇到过最典型的例子：无人机姿态估计。状态是四元数+角速度偏置，观测来自陀螺仪和加速度计。状态方程就是四元数微分方程，观测方程就是加速度计模型。说白了，就是把物理定律翻译成矩阵乘法。

4.2 线性卡尔曼滤波推导——从贝叶斯视角看

卡尔曼滤波的推导，我个人习惯从贝叶斯滤波的角度去理解。为什么？因为这样能看清每一步在干什么。

贝叶斯滤波的核心思想：

后验概率 ∝ 似然 × 先验概率

在线性高斯假设下，这个公式就变成了卡尔曼滤波的五个方程。我来拆解一下：

预测步骤（先验）

x̂_k|k-1 = A * x̂_{k-1|k-1} + B * u_k
P_k|k-1 = A * P_{k-1|k-1} * A^T + Q

这里x̂_k|k-1表示「用k-1时刻的信息预测k时刻的状态」。P是协方差矩阵，Q是过程噪声协方差。

更新步骤（后验）

K_k = P_k|k-1 * H^T * (H * P_k|k-1 * H^T + R)^{-1}
x̂_k|k = x̂_k|k-1 + K_k * (z_k - H * x̂_k|k-1)
P_k|k = (I - K_k * H) * P_k|k-1

K_k就是卡尔曼增益，它决定了「相信预测多一点」还是「相信观测多一点」。

我的经验：卡尔曼增益的物理意义很直观。当观测噪声R很大时，K_k会变小，滤波器更依赖预测。当过程噪声Q很大时，K_k会变大，滤波器更相信观测。调参说白了就是在调Q和R的比值。

4.3 预测与更新步骤——一个完整的滤波周期

我习惯把卡尔曼滤波比作「猜谜游戏」：

  1. 预测：根据上一轮的结果，猜一下当前状态大概在哪
  2. 更新：拿到传感器数据后，修正刚才的猜测
  3. 循环：重复1和2，每次都在缩小不确定性

下面这张图展示了整个流程：

卡尔曼滤波单步流程 初始状态 x̂₀, P₀ 预测步骤 x̂ₖ|ₖ₋₁ = A·x̂ₖ₋₁|ₖ₋₁ + B·uₖ Pₖ|ₖ₋₁ = A·Pₖ₋₁|ₖ₋₁·Aᵀ + Q 更新步骤 Kₖ = Pₖ|ₖ₋₁·Hᵀ·(H·Pₖ|ₖ₋₁·Hᵀ + R)⁻¹ x̂ₖ|ₖ = x̂ₖ|ₖ₋₁ + Kₖ·(zₖ - H·x̂ₖ|ₖ₋₁) Pₖ|ₖ = (I - Kₖ·H)·Pₖ|ₖ₋₁ 输出 x̂ₖ|ₖ, Pₖ|ₖ

你看，整个流程就是「预测→更新→预测→更新……」的循环。每次更新后，协方差P都会变小，说明不确定性在降低。但下一次预测时，加上过程噪声Q，P又会变大。这个「缩小→放大→缩小」的过程，就是卡尔曼滤波的精髓。

4.4 滤波器调参经验——那些年我踩过的坑

调参这件事，说实话，比推导公式难多了。公式是死的，参数是活的。我刚开始做融合时，花了两周调一个GPS/IMU融合的滤波器，结果还是发散。后来发现是Q矩阵设得太小了。

下面是我总结的调参经验：

参数 物理意义 调大效果 调小效果 我的建议
Q（过程噪声） 对模型的信任程度 更相信观测，响应快但噪声大 更相信模型，响应慢但平滑 先设小，看发散再逐步增大
R（观测噪声） 对传感器的信任程度 更相信模型，更新幅度小 更相信观测，更新幅度大 根据传感器手册标称值设定
P₀（初始协方差） 对初始状态的信任程度 初始收敛慢 初始收敛快但可能震荡 设大一点，让滤波器自己收敛

避坑指南：我曾经犯过一个低级错误——把Q设成零矩阵。结果滤波器完全不相信新观测，状态永远不更新。嗯，那天的调试日志写满了「为什么滤波不动？」。后来才意识到，Q=0意味着模型完美，但现实世界哪有完美的模型？

调参的实用技巧

  • 先调R，再调Q：R可以从传感器手册里找到，相对确定。Q需要根据经验试。
  • 用残差分析：如果残差（z_k - H*x̂_k|k-1）均值不为零，说明模型有偏。如果残差方差太大，说明Q或R没设好。
  • 自适应调参：我做过一个项目，用协方差匹配法在线调整Q和R。效果不错，但计算量大了点。
  • 别忘了单位：位置用米，速度用米/秒，角度用弧度。单位不统一，协方差矩阵会乱套。

一个小技巧：调试时可以把卡尔曼增益K_k打印出来。如果K_k一直很小（比如0.01以下），说明滤波器太相信模型了，观测基本没用。如果K_k一直很大（比如0.9以上），说明滤波器太相信观测了，模型基本没用。理想情况是K_k在0.1~0.5之间波动。

4.5 一个简单的Python实现

说了这么多，不如看代码来得实在。下面是一个一维卡尔曼滤波的示例，用来估计匀速运动物体的位置：

import numpy as np

class KalmanFilter1D:
    def __init__(self, dt, q, r):
        # 状态：位置和速度
        self.A = np.array([[1, dt],
                           [0, 1]])
        self.H = np.array([[1, 0]])
        self.Q = np.array([[q*dt**3/3, q*dt**2/2],
                           [q*dt**2/2, q*dt]])
        self.R = np.array([[r]])
        self.P = np.eye(2) * 100  # 初始协方差设大
        self.x = np.array([[0], [0]])  # 初始状态
        
    def predict(self):
        self.x = self.A @ self.x
        self.P = self.A @ self.P @ self.A.T + self.Q
        
    def update(self, z):
        y = z - self.H @ self.x  # 残差
        S = self.H @ self.P @ self.H.T + self.R
        K = self.P @ self.H.T @ np.linalg.inv(S)  # 卡尔曼增益
        self.x = self.x + K @ y
        self.P = (np.eye(2) - K @ self.H) @ self.P
        return self.x[0, 0]  # 返回位置估计

这段代码虽然简单，但五脏俱全。你把它跑起来，输入带噪声的位置观测，就能看到滤波效果。我建议你试试把Q和R调成不同数量级，看看滤波器的反应——这是理解卡尔曼滤波最好的方式。

好了，卡尔曼滤波的基础就讲到这里。记住一句话：卡尔曼滤波不是万能的，但没有卡尔曼滤波是万万不能的。在传感器融合这个领域，它就像螺丝刀一样，是最基础也最常用的工具。

公众号:蓝海资料掘金营，微信deep3321

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