4、卡尔曼滤波原理(上)：状态空间模型、预测与更新方程、线性卡尔曼滤波的数学推导

各位同学，欢迎来到多目标跟踪的核心环节。

今天我们要啃的，是卡尔曼滤波。说实话，这玩意儿当年我第一次接触时，也觉得头大。一堆矩阵、协方差、预测更新...但后来我在做行人跟踪项目时，发现没有它，目标框简直是在屏幕上“蹦迪”。

所以，咱们今天就把这块硬骨头，用最接地气的方式啃下来。

4.1 状态空间模型：你在跟踪什么？

先问个问题：你跟踪一个目标，到底在跟踪它的什么？

位置？速度？还是加速度？

嗯，这些都属于目标的状态。状态空间模型，说白了就是用一个向量，把目标当前的所有信息装起来。

核心定义：

在 t 时刻，目标的状态用一个 n 维向量 xt 表示。

比如在 SORT 算法里，我习惯用 7 维状态：

x = [cx, cy, s, r, vcx, vcy, vs]

其中：cx, cy 是中心坐标，s 是面积，r 是宽高比，后面三个是各自的速度。

你想想看，为什么要有速度？

因为目标不会瞬移。上一帧在左边，下一帧大概率还在左边附近。速度就是用来描述这种“惯性”的。

我的经验：

我在做车辆跟踪时，发现只跟踪位置不够。车辆转弯时，速度方向会变。所以我又加了角速度。状态空间不是死的，要根据你的场景来定。

4.2 预测方程：下一帧它会去哪？

有了状态，我们就要预测下一帧的状态。这就是预测方程干的事。

数学上长这样：

x̂t|t-1 = F · x̂t-1|t-1 + B · ut

别被符号吓到。我来拆解一下：

  • x̂t|t-1：基于 t-1 帧的信息，预测 t 帧的状态。注意那个“帽子”，表示估计值。
  • F：状态转移矩阵。它描述了“上一帧的状态如何变成这一帧的状态”。
  • B · ut：控制输入。比如你按了刹车，这就是一个控制。在多目标跟踪里，我们通常没有控制输入，所以这一项直接为 0。

举个最简单的例子：匀速运动模型。

如果目标匀速运动，那么：

位置t = 位置t-1 + 速度t-1 × Δt
速度t = 速度t-1

写成矩阵 F 就是：

F = [[1, 0, Δt, 0],
     [0, 1, 0, Δt],
     [0, 0, 1, 0],
     [0, 0, 0, 1]]

你看，是不是很直观？

注意：

预测不是万能的。它只是基于“模型”的猜测。如果目标突然急转弯，预测就会不准。这时候，就需要观测数据来修正。

4.3 更新方程：用观测数据修正预测

预测完了，我们拿到了观测数据（比如检测器给出的目标框）。

现在问题来了：预测值和观测值，到底信谁？

卡尔曼滤波的聪明之处就在这里——它不偏信任何一方，而是加权平均。

更新方程：

x̂t|t = x̂t|t-1 + Kt · (zt - H · x̂t|t-1)

我来解释一下每个部分：

  • zt：t 时刻的观测值，比如检测器给出的 [cx, cy, s, r]。
  • H：观测矩阵。它把状态空间映射到观测空间。比如状态是 7 维，观测是 4 维，H 就是 4×7 的矩阵。
  • zt - H · x̂t|t-1：这叫“残差”或“创新”。说白了，就是“观测值”和“预测的观测值”之间的差距。
  • Kt：卡尔曼增益。这是整个算法的灵魂。

卡尔曼增益 K 决定了：

  • 如果观测很可靠（噪声小），K 就大，我们更相信观测。
  • 如果预测很可靠（模型准），K 就小，我们更相信预测。

一句话总结：

卡尔曼滤波 = 预测 + 观测 + 加权平均。

加权系数就是卡尔曼增益 K。

4.4 线性卡尔曼滤波的数学推导

好，现在我们来推导一下 K 是怎么来的。

这部分有点数学，但我尽量讲得通俗。

首先，我们定义两个协方差矩阵：

  • Pt|t-1：预测状态的协方差。它衡量了预测的不确定性。
  • R：观测噪声的协方差。它衡量了观测的不确定性。

卡尔曼增益 K 的公式是：

Kt = Pt|t-1 · HT · (H · Pt|t-1 · HT + R)-1

别急着背公式。我们来理解它的含义：

  1. Pt|t-1 · HT：预测的不确定性，映射到观测空间。
  2. H · Pt|t-1 · HT + R：总的不确定性 = 预测的不确定性 + 观测的不确定性。
  3. 两者相除，得到的就是“预测不确定性占总不确定性的比例”。

说白了，K 就是：

K = 预测的不确定性 / (预测的不确定性 + 观测的不确定性)

如果预测很准（P 很小），K 就小，我们更信预测。

如果观测很准（R 很小），K 就大，我们更信观测。

我曾经踩过的坑：

刚开始调参时，我把 R 设得特别小，觉得检测器很准。结果目标一被遮挡，卡尔曼滤波就“放飞自我”了，因为观测噪声太小，它完全相信了错误的观测。后来我学乖了，R 要留点余量。

最后，更新完状态后，还要更新协方差：

Pt|t = (I - Kt · H) · Pt|t-1

这个公式表示：更新后，不确定性降低了。因为观测提供了新信息。

4.5 总结与实战要点

好了，我们来捋一捋今天的内容：

步骤 公式 含义
预测状态 x̂t|t-1 = F · x̂t-1|t-1 用模型猜下一帧的状态
预测协方差 Pt|t-1 = F · Pt-1|t-1 · FT + Q 预测的不确定性会变大（加了过程噪声 Q）
计算增益 Kt = Pt|t-1 · HT · (H · Pt|t-1 · HT + R)-1 算一下该信谁
更新状态 x̂t|t = x̂t|t-1 + Kt · (zt - H · x̂t|t-1) 用观测修正预测
更新协方差 Pt|t = (I - Kt · H) · Pt|t-1 不确定性降低了

我个人建议，刚开始学的时候，不要纠结于矩阵推导。先理解“预测-观测-加权”这个流程。等代码写熟了，再回头啃数学，会轻松很多。

下一讲，我们会把卡尔曼滤波的代码实现一遍。到时候你会发现，数学公式虽然唬人，但代码其实就那么几行。

嗯，今天就到这里。记住：卡尔曼滤波不是魔法，它只是用数学告诉你——该信谁。

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