3. 目标跟踪入门

各位同学，今天咱们来啃一块硬骨头——卡尔曼滤波。说实话，我刚入行那会儿，看到卡尔曼滤波的公式就头疼。什么状态预测、协方差更新，一堆矩阵运算，感觉像天书一样。但后来在雷达项目中做目标跟踪，被逼着啃下来了，才发现这东西其实没那么玄乎。

说白了，卡尔曼滤波就是一套“猜+修正”的循环。你想想看，雷达测目标位置，肯定有噪声对吧？卡尔曼滤波就是帮你从这些带噪声的测量值里，把真实状态给“滤”出来。我习惯把它比作一个聪明的猎人：先根据目标上一秒的位置和速度，猜它现在在哪；然后拿雷达实测的数据来修正这个猜测。嗯，就这么简单。

3.1 状态空间模型——你得先定义“状态”

做卡尔曼滤波的第一步，是定义清楚你要跟踪什么。在雷达目标跟踪里，我们通常关心目标的位置和速度。比如一个目标在二维平面上运动，它的状态向量可以写成：

x = [px, py, vx, vy]^T

其中 px、py 是位置，vx、vy 是速度。为什么要把速度和位置放在一起？因为卡尔曼滤波的核心假设是：目标运动是有规律的，我们可以用上一时刻的状态来预测下一时刻的状态。

这个规律，就是状态转移方程。我给大家一个最简单的匀速运动模型：

px(k) = px(k-1) + vx(k-1) * dt
py(k) = py(k-1) + vy(k-1) * dt
vx(k) = vx(k-1)
vy(k) = vy(k-1)

写成矩阵形式就是：

x(k) = F * x(k-1) + w(k)

这里的 F 是状态转移矩阵，w(k) 是过程噪声——说白了就是模型不完美带来的误差。比如目标突然加速了，你的匀速模型就猜不准了。我在项目中遇到过这种情况：跟踪一个转弯的车辆，用匀速模型直接发散掉了。后来加了转弯模型才稳住。

核心要点：状态空间模型就是“用数学描述目标怎么动”。你选的模型越贴近真实运动，跟踪效果越好。

3.2 卡尔曼滤波的两大步骤：预测与更新

卡尔曼滤波的核心就两个步骤：预测（Predict）和更新（Update）。我习惯叫它“先猜后修”。

3.2.1 预测步骤——先猜一猜

预测步骤要做两件事：猜目标现在在哪，以及猜这个猜测有多不准。

// 状态预测
x_pred = F * x_prev

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

这里 x_pred 是预测的状态，P_pred 是预测的协方差矩阵——它表示你对这个预测有多大的信心。Q 是过程噪声协方差矩阵，代表模型的不确定性。

举个例子：假设你看到目标在位置 (10, 10)，速度是 (2, 0)，时间间隔 dt=1 秒。那么你猜它下一时刻的位置是 (12, 10)，速度还是 (2, 0)。但你的猜测不一定准，所以协方差 P 会变大一些。

我的经验：Q 矩阵的调参很关键。Q 设得太小，滤波器会过于相信模型，跟不上突然机动；Q 设得太大，滤波器会过于相信测量，噪声滤不掉。我一般先根据目标最大加速度来估算 Q 的量级。

3.2.2 更新步骤——拿实测数据来修正

预测完了，雷达也测到数据了。这时候就要用测量值来修正你的预测。更新步骤也做两件事：计算增益，然后修正状态和协方差。

// 卡尔曼增益
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

这里 z 是雷达的测量值，H 是测量矩阵（把状态空间映射到测量空间），R 是测量噪声协方差矩阵。卡尔曼增益 K 决定了你更相信预测还是更相信测量。

你想想看，如果测量噪声很小（R 很小），那么 K 会比较大，滤波器会更相信测量值；反之，如果模型很准（P_pred 很小），K 会比较小，滤波器会更相信预测。这个平衡，就是卡尔曼滤波的精髓。

注意：更新步骤中，如果测量值 z 和预测值 H*x_pred 差太多，可能是目标发生了机动，也可能是测量野值。我曾经在项目中遇到过雷达突然丢了一个点，结果滤波器直接发散。后来加了野值检测逻辑才解决。

3.3 一个完整的卡尔曼滤波流程

下面我用一个简单的二维跟踪例子，展示完整的流程。假设目标做匀速直线运动，雷达每 0.1 秒测一次位置。

// 初始化
x = [0, 0, 10, 10]^T  // 初始位置(0,0)，速度(10,10)
P = eye(4) * 100       // 初始协方差，设大一点
F = [1, 0, dt, 0;
     0, 1, 0, dt;
     0, 0, 1, 0;
     0, 0, 0, 1]       // 状态转移矩阵
H = [1, 0, 0, 0;
     0, 1, 0, 0]       // 只测量位置
Q = eye(4) * 0.1       // 过程噪声
R = eye(2) * 1         // 测量噪声

// 主循环
for k = 1:N
    // 预测
    x_pred = F * x
    P_pred = F * P * F' + Q
    
    // 获取测量值 z
    z = radar_measurement()
    
    // 更新
    K = P_pred * H' * inv(H * P_pred * H' + R)
    x = x_pred + K * (z - H * x_pred)
    P = (eye(4) - K * H) * P_pred
    
    // 输出跟踪结果
    plot(x(1), x(2))
end

这段代码虽然简单，但已经能跑通一个基本的跟踪流程了。我建议初学者先把这个框架跑通，再慢慢加各种改进。

3.4 卡尔曼滤波的直观理解——用 SVG 图说话

说了这么多理论，咱们用一张图来总结卡尔曼滤波的核心逻辑。下面这张 SVG 图展示了预测-更新循环的完整流程：

卡尔曼滤波预测-更新循环 初始状态 x(k-1) 预测步骤 x_pred = F * x 预测状态 x_pred 测量值 z 更新步骤 x = x_pred + K*(z-H*x_pred) 更新状态 x(k) 下一时刻循环 图例： 初始状态 预测 测量 更新 输出状态

从这张图可以看得很清楚：卡尔曼滤波就是一个“预测→测量→更新→再预测”的闭环。每次循环，状态都会变得更准一些。

3.5 实际项目中的避坑指南

最后，我分享几个实际项目中踩过的坑，希望能帮大家少走弯路：

  • 初始化要谨慎：初始状态和协方差设不好，滤波器可能一直收敛不了。我习惯把初始协方差设大一些，让滤波器先相信测量值，慢慢收敛。
  • 野值处理不能少：雷达偶尔会丢点或出野值。我一般加一个卡方检验，如果测量值和预测值的差太大，就跳过这次更新。
  • 参数调优靠经验：Q 和 R 矩阵没有标准答案。我一般先根据物理模型估算，然后拿实际数据跑一遍，看残差是否满足白噪声假设。
  • 数值稳定性要注意：协方差矩阵 P 可能会因为数值误差变成非正定。我习惯用 Joseph 形式的协方差更新公式，或者定期做对称化处理。
一句话总结：卡尔曼滤波就是“用模型预测，用测量修正”。把这个循环跑通了，目标跟踪的入门就算完成了。

好了，这一章的内容就到这里。卡尔曼滤波是目标跟踪的基石，后面的扩展卡尔曼滤波、无迹卡尔曼滤波都是在这个框架上做的改进。大家先把基础打牢，后面学起来就轻松了。

可可精品资料库

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