卡尔曼滤波及 Python 实现

1. 卡尔曼滤波是什么

可以参考油管上的一个视频：https://www.youtube.com/watch?v=IFeCIbljreY

想象你在开车时用手机导航：

• GPS 定位（观测值）：有误差（比如显示在车道中间，实际可能偏左）。
• 车辆运动模型（预测值）：根据速度推算位置，但不够精确（比如没考虑风速）。

卡尔曼滤波就是把不完美的预测（估计）和不完美的观测结合起来，得到更加准确的结果，比如将 GPS 定位和车辆运动模型结合起来而得到更加准确的结果。

2. 核心思想 & 算法流程

• 1. 预测（猜）
  • 根据前一秒的位置和速度，预测现在的位置。
  • 例：车以 10m/s 行驶，1 秒后应前进 10 米。但实际可能有误差（如刹车导致速度变化）。
• 2. 观测（看）
  • GPS 告诉你现在的位置。
  • 例：GPS 显示车在 105 米处，但可能有 $\pm 5$ 米误差。
• 3. 融合（信谁？）
  • 如果预测更可靠（比如 GPS 信号差），就多相信预测
  • 如果观测更可靠（比如车突然刹车），就多相信观测。
  • 卡尔曼滤波自动计算“该信多少”（即卡尔曼增益）

3. 算法细节与公式

卡尔曼滤波分为两个阶段：预测（Predict）和更新（Update）。

假设系统状态为 $x_k$，观测值为 $z_k$，过程噪声和观测噪声均为高斯白噪声（符合正态分布）。

3.1 预测阶段（先验估计）

• 状态预测： $\hat{x}_k^- = F_k \hat{x}_{k-1} + B_k u_k$

  • $\hat{x}_k^-$：先验状态估计（预测值）。
  • $F_k$：状态转移矩阵。
  • $B_k$：控制输入矩阵（可选，外部控制量 $u_k$ 存在时使用）。

• 协方差预测： $P^-_k = F_k P_{k-1} F_k^T + Q_k$

  • $P^-_k$：先验估计误差协方差。
  • $Q_k$：过程噪声协方差矩阵。

3.2 更新阶段（后验估计）

• 卡尔曼增益计算： $K_k = P_k^- H_k^T (H_k P_k^- H_k^T + R_k)^{-1}$
  • $K_k$：卡尔曼增益（权衡预测与观测的权重）。
  • $H_k$：观测矩阵（将状态映射到观测空间）。
  • $R_k$：观测噪声协方差矩阵。
• 状态更新： $\hat{x}_k = \hat{x}_k^- + K_k (z_k - H_k \hat{x}_k^-)$
  • $\hat{x}_k$：后验状态估计（最终输出）
  • $z_k - H_k \hat{x}_k^-$：新息（观测残差，即预测与实测的差异，系统状态的更新信息）。
• 协方差更新 $P_k = (I-K_k H_k)P_k^-$
  • $P_k$：后验估计误差协方差。

“新息和残差这个说法没有固定定义，在不同文献里有不同说辞。有的认为新息和残差是一个东西，见《Estimation with applications to tracking navigation》这本书第 205 页。也有的论文认为是有区别的，如你这篇文章。这个还是要看具体文章里的定义使用。”

伪代码

# 初始化
x_hat = initial_store         # 状态信息估计
P = initial_covariance        # 初始化协方差矩阵
F = state_transition_matrix   # 状态转移矩阵
H = observation_matrix        # 观测矩阵
Q = process_noise_cov         # 过程噪声协方差
R = measurement_noise_cov     # 观测噪声协方差

for epoch new measurement z_k:
    # 1. 预测阶段
    x_hat_minus = F @ x_hat + B @ u_k
    P_minus = F @ P @ F.T + Q

    # 2. 更新阶段
    K = P_minus @ H.T @ np.linalg.inv(H @ P_minus @ H.T + R)  # 卡尔曼增益
    x_hat = x_hat_minus + K @ (z_k - H @ x_hat_minus)          # 状态更新
    P = (np.eye(n) - K @ H) @ P_minus                     # 协方差更新

4. 卡尔曼滤波的应用

关于卡尔曼滤波，理论上是可以应用在任意形式的信号上的（波、图像、音频等），本文中的代码会以卡尔曼滤波在图像目标检测中的应用为例。

可以参考某大佬写的一个应用样例：https://github.com/liuchangji/kalman-filter-in-single-object-tracking