感知与几何：状态估计与跟踪

检测器每一帧独立给出一堆框，彼此没有关系。规划器要的却是“前车在 12.75 m 处，每帧前进约 2 m，误差有多大”，以及“这个行人和上一帧那个是同一个人”。跟踪拆成两件事：用运动模型预测，再按两边的不确定度用观测修正（Kalman filter）；以及决定哪个观测属于哪条轨迹（数据关联）。前者在线性高斯假设下有闭式最优解，很多 ID 跳变出在后者。

待审核草稿。 这一篇还没有经过站主审阅，事实、取舍和措辞都可能修改；引用前请回到文中链接的一手资料核对。

先把“相信什么”写成一个高斯

前置知识是条件概率和高斯分布，以及第 1 篇的投影。

跟踪不是记住一个数，而是记住一个分布：均值是最可能的位置，方差（variance）是有多不确定。在线性、高斯噪声的假设下，这个分布一直是高斯，只需要更新两个量。取 Barfoot《State Estimation for Robotics》第 2 版式 (3.1) 的一维情形（书里把已知输入记作 $v_k$，这里写成 $u_k$）：

$$ x_k=x_{k-1}+u_k+w_k,\quad w_k\sim\mathcal N(0,Q);\qquad y_k=x_k+n_k,\quad n_k\sim\mathcal N(0,R). $$

$x_k$ 是第 $k$ 帧的真实位置，$u_k$ 是已知的位移（例如运动模型给出的速度乘帧间隔），$w_k$ 是运动模型没算到的部分，方差 $Q$；$y_k$ 是检测器给的位置，噪声方差 $R$。

一维手算：预测一次，修正一次

Kalman filter 每帧做两步（Barfoot 式 3.131 的一维情形；$\check{}$ 表示预测，$\hat{}$ 表示修正后）：

$$ \check x_k=\hat x_{k-1}+u_k,\qquad \check P_k=\hat P_{k-1}+Q $$

$$ K_k=\frac{\check P_k}{\check P_k+R},\qquad \hat x_k=\check x_k+K_k\,(y_k-\check x_k),\qquad \hat P_k=(1-K_k)\,\check P_k $$

教学设定：沿车道跟踪前车，上一帧估计 $\hat x=10$ m、$\hat P=1$ m²；前车每帧前进 $u=2$ m（20 m/s、10 Hz），$Q=0.5$；这一帧检测器给 $y=13$ m，$R=0.5$。

1. 预测：$\check x=12$，$\check P=1+0.5=1.5$。预测让不确定度变大。
2. 增益：$K=1.5/(1.5+0.5)=0.75$。预测比观测更不确定，所以更听观测。
3. 修正：$\hat x=12+0.75\times(13-12)=12.75$，$\hat P=0.25\times1.5=0.375$。

$y_k-\check x_k$ 叫创新（innovation），是“看到的”减“预想的”。换个角度验算：修正结果就是按精度（方差的倒数）加权平均预测和观测，

$$ \hat x=\frac{12/1.5+13/0.5}{1/1.5+1/0.5}=12.75,\qquad \hat P=\Big(\frac{1}{1.5}+\frac{1}{0.5}\Big)^{-1}=0.375. $$

修正后的方差比预测和观测都小：两个独立来源合起来，比任何一个都确定。每次只改一个条件再算：

最后一行的 $S=\check P+R=2$ 是创新的方差。$e^2/S$ 叫 normalized innovation squared（NIS）；模型正确时它服从自由度为 1 的 $\chi^2$ 分布，95% 分位数约 3.84。Barfoot 5.1.2 节用 NIS 检验滤波器是否自洽；拿它逐个筛观测，就是门限（gating）：18 远大于 3.84，拒绝这次观测。

从一维到 SORT：一个框也有状态

SORT（2016）把每个目标的状态写成 $[u,v,s,r,\dot u,\dot v,\dot s]$：框中心、面积、长宽比，以及前三个的变化速度；长宽比当作常数。运动模型是匀速线性模型，不考虑其他目标和相机运动。公式和一维完全一样，只是数换成了矩阵（Barfoot 式 3.131）：

$$ \check P_k=A\hat P_{k-1}A^{\top}+Q,\qquad K_k=\check P_kC^{\top}\big(C\check P_kC^{\top}+R\big)^{-1},\qquad \hat P_k=(I-K_kC)\,\check P_k $$

$A$ 把“新位置 = 旧位置 + 速度 × 一帧”写进矩阵，$C$ 只取出能观测的 $[u,v,s,r]$。新轨迹的速度没观测过，SORT 把它初始化为 0、方差设得很大。论文也直说匀速模型对真实运动预测得不好，实验里目标丢失 1 帧就删除（$T_{\rm Lost}=1$），重新出现时换一个新 ID。

在三维里跟踪、却用像素做观测时，观测模型就是第 1 篇的投影，是非线性的。扩展 Kalman 滤波（EKF）在当前估计处线性化一次；Barfoot 4.3.4 节指出，它的主要短板正是只线性化一次、不迭代到收敛。

数据关联：3 条轨迹、3 个检测

教学设定：两个并排向右走的行人 T1、T2，远处还有一个 T3。为了能手算，所有框高 100 px、上下边对齐、宽 100 px，IoU 就只看水平方向：重叠 $o$ px 时，$\mathrm{IoU}=o/(200-o)$。

• Kalman 预测的框（x 范围）：T1 [100, 200]，T2 [30, 130]，T3 [400, 500]。
• 这一帧的检测：D1 [80, 180]，D2 [140, 240]，D3 [410, 510]。

例如 T1 和 D1 重叠 [100, 180]，$o=80$，IoU $=80/120=0.667$。全部算完：

贪心每次拿最大的 IoU：先 T3–D3（0.818），再 T1–D1（0.667）。T2 只剩 D2，IoU 为 0，配不上；D2 被当成新目标。如果这两个人其实比预测走得更快，D1 是 T2、D2 是 T1，贪心就把 T2 的检测抢给了 T1：T2 断掉，T1 的框跳到别人身上。

Hungarian 算法（Kuhn 1955）求的是整体最优：一对一配对，让总代价 $1-\mathrm{IoU}$ 最小。3×3 可以手算，先每行减去本行最小值，再每列减去本列最小值：

代价 1 − IoU              每行减最小值              每列减最小值（只有 D2 列减 0.238）
     D1    D2    D3          D1    D2    D3          D1    D2    D3
T1  0.333 0.571 1.000       0     0.238 0.667       0     0     0.667
T2  0.667 1.000 1.000       0     0.333 0.333       0     0.095 0.333
T3  1.000 1.000 0.182       0.818 0.818 0           0.818 0.580 0

最后每行每列各挑一个 0：T2 只有 D1 一个 0，于是 T2–D1、T1–D2、T3–D3，总代价 1.420，比贪心那组的 1.515 小。我用 SciPy 的 linear_sum_assignment 复算，得到同样的配对。3×3 时也可以直接枚举 6 种排列核对；Hungarian 的意义在于目标多时不用枚举 $n!$ 种。

配完还要过门限：IoU 太低的配对直接拒绝。SORT 设一个调过的 $\mathrm{IoU}_{\min}$，ByteTrack 的实现细节里是 IoU 小于 0.2 就拒绝。T2–D1 的 0.333 能过。

Hungarian 只保证总 IoU 最大，不保证配对正确。如果 T2 其实被挡住了，而 D1 确实是 T1，它就会配错。运动方向、外观特征（Re-ID）能补上一部分；DanceTrack（2021）专门收集穿着相似、动作复杂的群舞视频，让外观几乎区分不了人，逼着方法依赖运动。

ByteTrack：低分框先别扔

接着上面的例子。下一帧 T2 走到柱子后面，检测器在它的预测位置给出 D4，但分数只有 0.35；另外在一张海报上给出 D5，分数 0.25。常见做法只保留分数过阈值的框（ByteTrack 的示意图用 0.5），D4 被扔掉，T2 配不上；按 SORT 的设置，它会被删掉，人走出来后换一个新 ID。

ByteTrack 的关联方法 BYTE 几乎保留所有框，按分数阈值 $\tau$ 分两轮关联（论文算法 1）：

每一帧：
  检测 → 按分数阈值 τ 分成 D_high 和 D_low
  所有轨迹（包括暂时丢失的）用 Kalman 预测新位置
  第一轮：轨迹 ↔ D_high，相似度用 IoU 或 Re-ID，Hungarian 配对
  第二轮：第一轮剩下的轨迹 ↔ D_low，只用 IoU
  没配上的 D_low：当背景删掉
  没配上的轨迹：标为丢失，保留 30 帧，期间还能找回
  没配上的 D_high：新建轨迹

论文默认 $\tau=0.6$，丢失的轨迹保留 30 帧。第二轮只用 IoU，因为低分框往往是遮挡或运动模糊造成的，外观特征不可靠。在我们的例子里，T2 的预测框和 D4 的 IoU 很高，第二轮把它接上；D5 附近没有任何轨迹，被当作背景删掉。

作者在 MOT17 验证集（训练视频后半段）上用同一个检测器对比：SORT 换成 BYTE 后，MOTA 从 74.6 到 76.6，IDF1 从 76.9 到 79.3，ID 切换从 291 次降到 159 次（论文表 2，作者报告）。论文的说法是：MOTA 由误检、漏检和 ID 切换算出，更偏检测质量；IDF1 看身份保持；HOTA 同时平衡检测、关联和定位。

常用的行人跟踪 benchmark：MOT17 沿用 MOT16 的全部序列、换了更准的标注，训练和测试各 7 段，附带 DPM、Faster R-CNN、SDP 三种公开检测；MOT20 是 8 段非常拥挤的场景；DanceTrack 见上一节。

滤波还是优化：SLAM 里的同一个问题

把状态换成自车的位姿（和地图），就是视觉里程计和 SLAM。还是“预测，再用观测修正”，但有两种做法。

滤波只保留当前状态和协方差，每个观测用一次，然后被压缩进协方差。MSCKF（Mourikis & Roumeliotis 2007）是基于 EKF 的视觉惯性导航：状态里留几帧过去的相机位姿，不放三维特征点，用“同一个特征被多个位姿看到”构造约束，计算量随特征数线性增长（论文摘要）。

优化保留一个窗口内的所有状态和原始观测，反复迭代求解非线性最小二乘。VINS-Mono（2017）用紧耦合的非线性优化融合预积分的 IMU 和特征观测，在滑动窗口上求解，把旧状态边缘化成先验以保持耗时恒定，再用回环和 4 自由度位姿图保持全局一致。ORB-SLAM3（2020）基于特征点，连 IMU 初始化阶段都完全用最大后验估计，能把很久以前、甚至上一次建图的关键帧放进 bundle adjustment；作者报告双目加惯性在 EuRoC 上平均精度 3.5 cm。DSO（2016）是另一路：不提特征点，直接最小化光度误差。

在线性高斯的情况下，两者对最新状态给出同一个答案：Barfoot 第 3.3 节从批量最大后验解推出 Kalman filter，它就是 Rauch–Tung–Striebel 平滑器的前向一遍。差别来自非线性：

界线并不绝对。VINS-Mono 的相关工作一节提到，MSCKF 的一个扩展用逆滤波器做迭代重线性化，效果上等价于基于优化的方法。

截至 2026-10

• 视频分割模型也开始跟踪。 SAM 3（2025-11）对一个名词短语在视频里检测并跟踪所有实例，给出 ID；SAM 3.1（2026-03-27）加入多目标联合跟踪，Meta 自报在一张 H100 上、目标数中等时从 16 FPS 提到 32 FPS（公司自报，Meta 博客）。
• 点跟踪单独成线。 TAPIR（2023）是 Apache-2.0；CoTracker3（2024-10）是 CC BY-NC 4.0（模型卡，许可证 2026-10-15 读取），不能直接商用。VGGT 也输出三维点轨迹。
• 本篇没有展开 MOTA、IDF1、HOTA 的计算细节，也没有覆盖 CoTracker3 之后的点跟踪模型。

下一篇：2026 年的检测与分割。在自动驾驶里，这些轨迹接着进入预测和规划，见自动驾驶系列。