4、卡尔曼滤波入门：状态预测、状态更新、卡尔曼增益、在跟踪中的应用（位置平滑与预测）
各位同学，欢迎来到多目标跟踪的核心环节——卡尔曼滤波。
说实话，我刚入行那会儿，看到卡尔曼滤波的公式就头疼。一堆矩阵、协方差、增益系数，感觉像天书。但后来我发现，这东西其实没那么玄乎。说白了，它就是一个“猜+修”的过程。你猜目标下一步在哪，然后用测量值去修正你的猜测。
今天我们就把它拆开揉碎，看看卡尔曼滤波到底在干什么。
4.1 卡尔曼滤波在解决什么问题？
想象一下，你在跟踪一个行人。摄像头每帧检测到他的位置，但检测总有噪声——有时候框偏左，有时候偏右，甚至偶尔漏检。你直接拿检测结果画轨迹，那轨迹肯定抖得跟心电图似的。
卡尔曼滤波就是干这个的：它帮你把噪声滤掉，同时预测下一帧目标可能在哪。
我个人习惯把卡尔曼滤波看作一个“带脑子的平滑器”。它不只看当前帧，还结合了历史运动趋势。你想想看，一个匀速运动的人，上一帧在左边，这一帧突然跑到右边老远，那大概率是检测出错了。卡尔曼滤波会“怀疑”这个测量值，不会盲目相信。
4.2 核心公式：状态预测与状态更新
卡尔曼滤波就两个阶段：预测（Predict） 和 更新（Update）。循环往复。
4.2.1 状态预测
预测阶段，我们根据上一帧的状态，猜当前帧的状态。公式长这样：
x̂_k|k-1 = F * x̂_k-1|k-1 + B * u_k
P_k|k-1 = F * P_k-1|k-1 * F^T + Q别怕，我解释给你听：
- x̂ 是状态向量。比如 [x, y, vx, vy]——位置和速度。
- F 是状态转移矩阵。它描述目标怎么从上一帧运动到这一帧。匀速运动的话，F就是个简单的线性变换。
- P 是协方差矩阵。它表示你对当前状态有多“不确定”。值越大，越没底。
- Q 是过程噪声。它代表你允许模型有多大的“随机抖动”。
我在项目中遇到过一个问题：Q设得太小，模型太“自信”，结果目标突然加速转弯，卡尔曼滤波半天反应不过来。后来我把Q调大了一点，让它允许一些不确定性，跟踪反而更稳了。
4.2.2 状态更新
预测完了，我们拿到了检测器的测量值 z_k。这时候就要做更新：
y_k = z_k - H * x̂_k|k-1
S_k = H * P_k|k-1 * H^T + R
K_k = P_k|k-1 * H^T * S_k^(-1)
x̂_k|k = x̂_k|k-1 + K_k * y_k
P_k|k = (I - K_k * H) * P_k|k-1这里最关键的是 卡尔曼增益 K_k。它决定了你更相信预测，还是更相信测量。
核心思想：K_k 大，说明测量噪声小，你更相信检测器；K_k 小，说明预测更可靠，你更相信模型。
4.3 卡尔曼增益：信任的天平
卡尔曼增益 K_k 是个动态值。它会根据当前的不确定性自动调整。
- 如果测量噪声 R 很小（检测器很准），K_k 会变大，更新时更依赖测量值。
- 如果过程噪声 Q 很小（模型很准），K_k 会变小，更新时更依赖预测值。
我曾经调试一个无人机跟踪项目，发现卡尔曼增益一直在0.9附近徘徊。这意味着它几乎完全相信检测器，那还要卡尔曼滤波干嘛？后来发现是我把R设得太小了。调大R之后，K_k降到0.6左右，轨迹平滑了很多。
避坑指南：我曾经把R设成0，结果卡尔曼增益直接变成1，完全忽略预测。跟踪轨迹跟原始检测一样抖。记住，R不能为0，要给测量留点“怀疑空间”。
4.4 在跟踪中的应用：位置平滑与预测
卡尔曼滤波在跟踪里主要干两件事：
- 位置平滑：把检测噪声滤掉，让轨迹更自然。
- 位置预测：当目标被遮挡或漏检时，用预测值顶上去。
举个例子。假设你跟踪一辆车，第10帧检测到它在(100, 200)，第11帧检测到它在(105, 202)。卡尔曼滤波会认为这符合匀速运动，平滑后轨迹基本就是直线。但如果第11帧检测突然跳到(120, 250)，卡尔曼滤波会认为这不太合理，更新后的位置可能只有(108, 210)——这就是平滑的效果。
再比如，第12帧目标被遮挡，检测器没输出。这时候卡尔曼滤波直接用预测值：x̂_12|11。虽然位置会越来越不准，但至少比直接丢掉轨迹强。
注意：预测时间越长，误差越大。如果连续5帧以上没有检测更新，我建议直接标记轨迹为“丢失”，而不是继续盲目预测。
4.5 一个简单的卡尔曼滤波实现
下面是一个针对二维匀速运动目标的卡尔曼滤波代码。我习惯用Python写原型，方便调试。
import numpy as np
class KalmanFilter2D:
def __init__(self):
# 状态向量 [x, y, vx, vy]
self.x = np.zeros((4, 1))
# 状态转移矩阵 (匀速模型)
self.F = np.array([[1, 0, 1, 0],
[0, 1, 0, 1],
[0, 0, 1, 0],
[0, 0, 0, 1]])
# 测量矩阵 (只观测位置)
self.H = np.array([[1, 0, 0, 0],
[0, 1, 0, 0]])
# 协方差矩阵
self.P = np.eye(4) * 100
# 过程噪声
self.Q = np.eye(4) * 0.1
# 测量噪声
self.R = np.eye(2) * 1.0
def predict(self):
self.x = self.F @ self.x
self.P = self.F @ self.P @ self.F.T + self.Q
return self.x[:2].flatten()
def update(self, z):
z = np.array(z).reshape(2, 1)
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(4) - K @ self.H) @ self.P
return self.x[:2].flatten()这段代码里，我设了初始协方差P很大（100），表示一开始完全不知道目标在哪。随着更新次数增加，P会逐渐收敛，卡尔曼滤波也会越来越自信。
4.6 本章知识体系
下面这张图帮你理清卡尔曼滤波在跟踪中的完整流程：
嗯，这张图把整个流程串起来了。你从初始状态出发，先预测，再算增益，最后更新。更新完的状态又作为下一帧的初始状态，周而复始。
4.7 调参经验总结
最后，我把自己调参的一些经验整理成表格，方便你参考：
| 参数 | 含义 | 设太大 | 设太小 | 我的建议 |
|---|---|---|---|---|
| Q | 过程噪声 | 轨迹抖动，过于相信测量 | 反应迟钝，跟不上机动 | 先设0.1，根据目标机动性调整 |
| R | 测量噪声 | 平滑过度，滞后严重 | 几乎不滤波，跟原始检测一样 | 根据检测器精度设定，一般1~10 |
| P₀ | 初始协方差 | 收敛慢，前几帧不稳定 | 过早自信，可能发散 | 设100左右，让滤波自己收敛 |
记住，没有一套参数能打天下。不同场景、不同检测器，参数都得微调。我每次换数据集，第一件事就是重新调Q和R。
好了，卡尔曼滤波的入门就讲到这里。你只要记住“预测-更新”这个循环，理解卡尔曼增益是信任的天平，剩下的就是调参的体力活了。