4. 卡尔曼滤波基础：状态空间模型、预测与更新、KF的数学推导与代码实现

各位同学，欢迎来到第四讲。

说实话，卡尔曼滤波（KF）是咱们定位工程师吃饭的家伙。你想想看，GPS信号飘忽不定，IMU又爱积累误差，怎么把这两者捏在一起？靠的就是KF。我当年刚入行时，觉得这东西就是个黑盒子，直到亲手调过一次参数，才真正明白它的妙处。

4.1 状态空间模型：KF的骨架

卡尔曼滤波的核心，是状态空间模型。说白了，就是把系统抽象成两个方程：

  • 状态方程：描述系统怎么演化
  • 观测方程：描述传感器怎么测量

拿自动驾驶车辆定位举例。假设我们要估计车辆的位置 \( x \) 和速度 \( \dot{x} \)。

状态向量定义为：

x_k = [位置, 速度]^T

状态方程（离散时间）：

x_k = A * x_{k-1} + B * u_k + w_k

其中：

  • A 是状态转移矩阵。比如匀速模型：A = [[1, dt], [0, 1]]
  • B 是控制输入矩阵，u_k 是控制量（比如加速度）
  • w_k 是过程噪声，服从 \( N(0, Q) \)

观测方程：

z_k = H * x_k + v_k

其中：

  • H 是观测矩阵。比如GPS只测位置：H = [[1, 0]]
  • v_k 是观测噪声，服从 \( N(0, R) \)

关键点：状态空间模型假设系统是线性高斯的。如果系统是非线性的，那就得用EKF或UKF了，咱们后面会讲。

4.2 预测与更新：KF的两步走

卡尔曼滤波就两个步骤：预测和更新。我习惯叫它“猜-测-改”。

4.2.1 预测（Time Update）

这一步是根据上一时刻的状态，猜当前时刻的状态。

// 预测状态
x_pred = A * x_prev + B * u

// 预测协方差
P_pred = A * P_prev * A^T + Q

这里：

  • x_pred 是预测的状态
  • P_pred 是预测的协方差，代表不确定性
  • Q 是过程噪声协方差，我调参时最常折腾的就是它

我的经验：Q 设得太大，滤波器会“太相信观测”，导致抖动；设得太小，滤波器会“太相信模型”，导致滞后。我一般先设成单位矩阵的0.01倍，再根据实测数据微调。

4.2.2 更新（Measurement Update）

这一步是用观测值来修正预测值。

// 卡尔曼增益
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)

// 更新状态
x_updated = x_pred + K * (z - H * x_pred)

// 更新协方差
P_updated = (I - K * H) * P_pred

卡尔曼增益 K 是个关键角色。它决定了“你更相信模型还是传感器”。

  • 如果观测噪声 R 很小（传感器很准），K 会变大，滤波器更相信观测
  • 如果过程噪声 Q 很小（模型很准），K 会变小，滤波器更相信模型

避坑指南：我曾经在项目中把 R 设得太小，结果GPS一丢，滤波器直接发散。后来我加了观测异常检测，当残差（z - H*x_pred）超过3倍标准差时，就降低K值。效果立竿见影。

4.3 KF的数学推导：从贝叶斯角度看

嗯，这里咱们稍微深入一点。卡尔曼滤波本质上是一个贝叶斯滤波器。

预测步骤对应的是先验概率：

p(x_k | z_{1:k-1}) = ∫ p(x_k | x_{k-1}) * p(x_{k-1} | z_{1:k-1}) dx_{k-1}

更新步骤对应的是后验概率：

p(x_k | z_{1:k}) = p(z_k | x_k) * p(x_k | z_{1:k-1}) / p(z_k | z_{1:k-1})

因为所有分布都是高斯分布，所以后验的均值和协方差有闭式解。这就是卡尔曼增益的由来。

推导过程我就不一步步展开了，但记住一个结论：卡尔曼增益 K 是使后验协方差最小的最优增益。

4.4 代码实现：手写一个一维KF

理论说完了，咱们来点实际的。下面是一个一维卡尔曼滤波的Python实现，用于估计车辆的位置。

import numpy as np

class KalmanFilter1D:
    def __init__(self, x0, P0, Q, R):
        self.x = x0      # 初始状态
        self.P = P0      # 初始协方差
        self.Q = Q       # 过程噪声
        self.R = R       # 观测噪声

    def predict(self, dt):
        # 状态转移矩阵（匀速模型）
        A = np.array([[1, dt],
                      [0, 1]])
        # 预测
        self.x = A @ self.x
        self.P = A @ self.P @ A.T + self.Q
        return self.x

    def update(self, z):
        # 观测矩阵
        H = np.array([[1, 0]])
        # 卡尔曼增益
        S = H @ self.P @ H.T + self.R
        K = self.P @ H.T @ np.linalg.inv(S)
        # 更新
        y = z - H @ self.x  # 残差
        self.x = self.x + K @ y
        self.P = (np.eye(2) - K @ H) @ self.P
        return self.x

# 使用示例
kf = KalmanFilter1D(
    x0=np.array([0, 0]),  # 初始位置0，速度0
    P0=np.eye(2) * 100,   # 初始不确定性很大
    Q=np.diag([0.1, 0.1]),# 过程噪声
    R=np.array([[1]])     # 观测噪声
)

# 模拟数据
dt = 0.1
true_pos = 5.0
measurements = [true_pos + np.random.randn() * 0.5 for _ in range(10)]

for z in measurements:
    kf.predict(dt)
    x_est = kf.update(z)
    print(f"观测: {z:.2f}, 估计位置: {x_est[0]:.2f}, 估计速度: {x_est[1]:.2f}")

调试技巧：我建议你跑代码时，打印一下P矩阵和K值。如果P收敛到0，说明滤波器“太自信”了，可能有问题。正常情况P应该收敛到一个非零的稳态值。

4.5 总结与思考

这一讲我们覆盖了：

  • 状态空间模型的构建
  • 预测与更新的数学公式
  • KF的贝叶斯解释
  • 一维KF的代码实现

说实话，卡尔曼滤波的代码写起来不难，难的是调参和工程化。我建议你：

  1. 先用仿真数据跑通代码
  2. 再换真实传感器数据试试
  3. 最后加上异常检测和自适应调参

下一讲我们会讨论扩展卡尔曼滤波（EKF），处理非线性系统。到时候你会看到，KF的思想其实贯穿始终。

好，今天就到这里。有问题欢迎在评论区交流。

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