Kalman Filter (KF)

卡尔曼滤波器（KF）是一种递归算法，它根据一系列含噪声的测量值，估计变化系统的隐藏状态，例如物体的位置和速度。每个时间步中，它先根据系统运动模型预测下一状态，再根据新测量值修正预测，并依据各自的不确定性为两者加权。Rudolf E. Kálmán 于 1960 年发表了这种方法；对于带有高斯噪声的线性系统，它能给出均方误差最小的估计。卡尔曼滤波器应用于 GPS 接收器、飞机和航天器导航、机器人定位，以及计算机视觉中几乎所有的多目标跟踪器，用于平滑抖动的检测结果，并预测每个物体下一次出现的位置。

卡尔曼滤波器的工作原理#

滤波器会维护两个量：一个状态估计 x，例如位置和速度；以及一个协方差矩阵 P，用于描述该估计的不确定性。每个周期分为两个步骤：

1. 预测：运动模型 F 会将状态向前投影，x = F x，而过程噪声 Q 会增大不确定性。过程噪声涵盖模型未能捕捉的运动：P = F P Fᵀ + Q。
2. 更新：收到新的测量值 z，它与状态之间的关系由测量模型 H 描述。滤波器会计算残差 z − H x 和卡尔曼增益 K = P Hᵀ (H P Hᵀ + R)⁻¹，其中 R 表示测量噪声。修正后的状态为 x + K (z − H x)，不确定性则缩小至 (I − K H) P。

卡尔曼增益决定了模型和传感器之间的权衡。当测量值相对于预测值很精确时，增益趋近于 1，估计值会跟随测量值；当测量值噪声较大时，增益趋近于 0，估计值会跟随预测值。由于状态可以包含从未直接测量的量，滤波器也能推断这些量：即使跟踪器只观测位置，仍能估计速度。缺少测量值时，滤波器会跳过更新，继续依据预测值运行：

目标跟踪中的卡尔曼滤波器#

在目标跟踪中，每条轨迹都有自己的卡尔曼滤波器。状态包含边界框的中心和尺寸，以及它们各自的速度；匀速模型则预测边界框在下一帧中的位置。系统会根据交并比（IoU）将这些预测与新的检测结果匹配，每个匹配的检测结果都会作为测量值来更新对应轨迹。当物体被遮挡几帧时，轨迹仍会沿着预测路径继续移动，因此物体重新出现时，其检测结果会落在足够接近预测位置的地方，从而保留原有 ID。

Ultralytics YOLO的跟踪模式中的每个跟踪器都采用这种方法：ByteTrack的 KalmanFilterXYAH会跟踪一个包含边界框中心、长宽比、高度及其速度的 8 维状态；BoT-SORT 和 TrackTrack 使用宽度和高度变体；两者都会根据边界框大小缩放噪声，因此尺寸较大、距离较近的物体可以容忍更大的像素跳变。对于边界框中心的一个坐标，NumPy 中只需几行代码就能实现相同的预测和更新步骤：

import numpy as np

F = np.array([[1.0, 1.0], [0.0, 1.0]])  # constant-velocity model, state = [position, velocity]
H = np.array([[1.0, 0.0]])  # the detector measures position only
Q = 0.001 * np.eye(2)  # process noise: how far real motion may stray from the model
R = np.array([[25.0]])  # measurement noise: detections jitter by about 5 px

rng = np.random.default_rng(0)
truth = 100 + 3.0 * np.arange(60)  # box center moving 3 px per frame
z = truth + rng.normal(0, 5.0, truth.size)  # noisy detections

x, P = np.array([z[0], 0.0]), np.diag([25.0, 100.0])  # start from the first detection
estimates = [x[0]]
for measurement in z[1:]:
    x, P = F @ x, F @ P @ F.T + Q  # predict
    K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)  # Kalman gain
    x = x + K @ (measurement - H @ x)  # update with the weighted residual
    P = (np.eye(2) - K @ H) @ P
    estimates.append(x[0])

print(f"Detection error: {np.sqrt(np.mean((z - truth) ** 2)):.2f} px")
print(f"Kalman error: {np.sqrt(np.mean((np.array(estimates) - truth) ** 2)):.2f} px")
print(f"Estimated velocity: {x[1]:.2f} px/frame")

原始检测结果的均方根误差为 4.50 px，滤波后的估计值为 2.48 px；估计速度为每帧 2.98 px，接近真实的 3 px，尽管速度从未被直接测量。

相关滤波器#

标准卡尔曼滤波器假设运动和测量模型是线性的，并且噪声服从高斯分布。它的变体放宽了这些假设：

• 扩展卡尔曼滤波器（EKF）：每一步都使用雅可比矩阵在当前估计值附近将非线性模型线性化，是 GPS 和惯性导航的标准选择。
• 无迹卡尔曼滤波器（UKF）：将一组经过精心选择的少量采样点输入非线性模型，通常比线性化更准确，而且不需要计算导数。
• 粒子滤波器：使用大量带权重的随机样本表示状态分布，因此能够处理非高斯和多峰信念，例如机器人不确定自己位于两条走廊中的哪一条；但计算成本要高得多。
• 隐马尔可夫模型（HMM）：对离散状态使用相同的预测与更新递归过程；卡尔曼滤波器则是其用于连续线性高斯状态的对应方法。

跟踪之外的应用#

• 机器人技术与传感器融合：机器人会将轮式里程计、IMU、激光雷达和摄像头的数据融合为一个位姿估计；基于 EKF 的方法是同时定位与建图的经典方案。
• 增强现实：头戴设备会滤波头部位姿测量值，以低延迟保持虚拟物体稳定。
• 经济学与信号处理：分析人员使用卡尔曼平滑，从含噪声的时间序列中估计未观测到的趋势。
• 导航：飞机、船舶、无人机和手机会将 GPS 与惯性传感器融合，使位置在卫星定位之间保持平滑。导航也是这种滤波器最早的重要应用：20 世纪 60 年代初，NASA Ames 的 Stanley F. Schmidt 将其用于阿波罗导航计算机的非线性轨迹估计。