1. 卡尔曼滤波是什么
可以参考油管上的一个视频：https://www.youtube.com/watch?v=IFeCIbljreY
想象你在开车时用手机导航：
- GPS 定位（观测值）：有误差（比如显示在车道中间，实际可能偏左）。
- 车辆运动模型（预测值）：根据速度推算位置，但不够精确（比如没考虑风速）。
卡尔曼滤波就是把不完美的预测（估计）和不完美的观测结合起来，得到更加准确的结果，比如将 GPS 定位和车辆运动模型结合起来而得到更加准确的结果。
2. 核心思想 & 算法流程
- 预测（猜）
- 根据前一秒的位置和速度，预测现在的位置。
- 例：车以 10m/s 行驶，1 秒后应前进 10 米。但实际可能有误差（如刹车导致速度变化）。
- 观测（看）
- GPS 告诉你现在的位置。
- 例：GPS 显示车在 105 米处，但可能有 $\pm 5$ 米误差。
- 融合（信谁？）
- 如果预测更可靠（比如 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