手撕自动驾驶算法——多目标跟踪：数据关联

KF可以提供跟踪目标的的最优估计，但由于测量的不确定性，实际上无法保证被跟踪目标是相关联的。因此，需要对估计的跟踪目标进行分类和验证。

数据关联（DA）是将检测结果关联到跟踪滤波器的过程， DA 滤波器有两类：确定性滤波器和概率滤波器。

• 确定性 DA 滤波器：代表是最近邻域滤波器 (NNF) 算法，该算法使用相对状态的最近测量值更新每个对象。 NNF 根据测量和轨迹之间的最短欧几里德或马氏距离将对象与已知轨迹相关联。但多个测量值彼此靠近时，可能导致单个测量值被错误地更新到其他附近对象。

• DA 滤波器：常见概率数据关联滤波器（PDAF），PDAF 使用所有关联假设执行对象状态的加权更新，避免了使用 NNF 算法时经常遇到错误关联。

下表总结了 NN 和 PDA 之间的差异：

概率数据关联过滤器

为简单起见，使用线性的卡尔曼滤波 KF推导，来了解联合概率如何与 PDAF 结合的

PDAF 及其导数都使用传入测量值来近似每次更新后被跟踪对象的概率分布函数。 这是通过假设测量遵循高斯概率分布来完成的。

PDAF 基于以下假设：

1. 仅存在一个感兴趣的目标，其状态变量 $x\in R^{n_x}$ 假设根据方程随时间演变
   $${\mathbf{x}}_k=f_{k-1}({\mathbf{x}}_{k-1})+{\mathbf{v}}_{k-1}$$
   测试值$z_k\in R^{n_z}$可得：
   $${\mathbf{z}}_k=h_x{\mathbf{x}}_k+{\mathbf{w}}_k$$

   其中：${\mathbf{v}}_{k-1}$是均值为0，协方差为${\mathbf{Q}}_k$的高斯白噪声，${\mathbf{w}}_{k-1}$是均值为0，协方差为${\mathbf{R}}_k$的高斯白噪声

2. 跟踪初始化

3. k-1 时刻，高斯后验概率形式为：
   

4. 如果检测到目标并且相应的测量值落入验证区域，选取测量值的最大值

5. 假设所有非目标的测量都起源于空间上均匀分布、时间上泊松分布的杂波

PDAF算法主要分4个阶段：预测、测量验证、数据关联和状态估计。 预测和状态更新类似于传统的 KF，测量验证和数据关联是 JPDAF 的一个特定部分， PDAF 算法的一个周期执行如图 3-7 所示。

预测

PDAF 像 KF 一样提前一步预测状态和协方差矩阵：
$$\begin{gathered} {\hat{\mathbf{x}}}_{k|k-1}=f_{k-1}({\hat{\mathbf{x}}}_{k-1|k-1)} \\ {\hat{\mathbf{z}}}_{k|k-1}=h_k({\hat{\mathbf{x}}}_{k|k-1}) \\ {\mathbf{P}}_{k|k-1}=f_{k-1}{\mathbf{P}}_{k-1|k-1}{f}_{k-1}^{{}^{\prime }}+{\mathbf{Q}}_{k-1} \end{gathered}$$

更新的协方差矩阵：
$${\mathbf{x}}_k=h_k{\mathbf{P}}_{k|k-1}{h}_k^{{}^{\prime }}+{\mathbf{R}}_k$$

测量验证

在将测量传递给 DA 过滤器之前，排除了与最近track的距离超过阈值的检测，门限示意图如：

门限选择一个测量子集，在对象存在和被检测到的先验知识的情况下，该子集包含高概率的对象检测。这个概率被称为“门限概率”，常用方法是假设测量服从高斯分布，将超椭球门作为门限区域，计算获得的测量状态向量的马氏距离，并与预测状态向量的马氏距离进行比较。常采用基于逆值${\chi }^2$ 平方分布的阈值，该阈值称为门级。

测量验证由椭圆区域给出的门限区域完成

其中 $\gamma$ 是等于 inv-${\chi }^2(P_G)$ 的门阈值，$P_G$ 是门包含真实测量的概率，$S_k$ 是测量更新的协方差。 q 维测量的验证区域的体积表示为:

最后，一组经过验证的测量结果如下：

数据关联

使用非参数PDAF，我们假设一个扩散的先验杂波模型，适用于异质杂波环境。时刻 k 的测量 i 的关联概率 β 为：

$P_D$ 是检测概率，$P_G$ 是门限概率

状态估计

状态更新方程为：
$${\hat{\mathbf{x}}}_{k|k|}={\mathbf{x}}_{k|k-1}+{\mathbf{K}}_k{v}_k$$

$v_k$和卡尔曼增益${\mathbf{K}}_k$为：
更新状态的协方差矩阵：

PDAF算法在状态预测和更新方面类似于传统的KF，${\beta }_{0,k}$为测量的概率权重

JPDA

MOT 问题可以被视为多个跟踪器并行运行的单个对象跟踪问题。 但是，这仅在对象独立移动时才有效。 在城市地区，由于交通状况，道路使用者通常以类似编队的方式移动。 虽然被跟踪物体的位置不同； 速度和加速度可能几乎相同，这种情况在将多个测量与多个跟踪相关联时引入了歧义。 这种情况如图 3-9 所示。

测量2可能属于预测跟踪1或2，错误的关联会影响跟踪器状态估计性能，甚至会导致跟踪丢失，为了解决这个问题，提出了联合概率数据关联过滤器 (JPDAF) ，是 PDAF 的扩展，它在假设对象在杂波下被跟踪的情况下运行，当不同的跟踪共享相似的测量值时就会出现这种情况。

从概率上讲，由于测量对应于跟踪检测的事件在轨迹之间是互斥的，但不一定是相互独立的，因此最优滤波器更新操作必须考虑所有可能的tracker，全局考虑将测量分配给可能的tracker。 假设如下：

• 杂波中已建立目标（即轨道）的数量 N 是先验已知的。
• 来自一个目标的测量可能落在相邻目标的验证区域内，并持续干扰。
• 系统的过去由包含状态估计的近似总结，状态估计由近似均值和协方差给出
• 根据上述近似统计量，假设状态为均值和协方差的高斯分布
• 每个目标都有一个运动模型和测量模型

考虑到我们有一些测量值 j 和一些预测目标 t。 在联合事件 θ 中将 j 与 t 相关联的高斯似然 $A_{t,j}$计算为：

在这里，我们认为所有数量的杂波测量都是同样可能的，联合关联 $\gamma (\theta )$概率变为

其中 V 是监视区域的体积，其中假设与目标无关的测量值是均匀分布的，Φ 是事件 A 中错误测量的数量，τ 是二进制测量指标，δ 是二进制目标检测指标，以及 c 是归一化常数。

考虑图 3-9，例如，如果我们有可行的联合关联 Measurement 1 到 Track 1 (θ 11 )，Measurement 2 到杂波 (θ 02 ) 和 Measurement 3 到 Track 2 (θ 23 )，那么 δ 1 = 1, δ 2 = 1，τ 1 = 0，τ 2 = 0，τ 3 = 1，Φ = 1，则γ(θ)变为
这也适用于其他可行的联合事件（θ tj , θ tj , θ tj ），其中 j = (1, . . . , m k ) 和 j = (1, . . . , N )。

假设以过去观察为条件的状态是相互独立的，则JPDA 关联概率 β 只是所有可行关联概率 P {θ k |Z k } 的边缘化，分别由下式给出
状态估计方程将为每个目标解耦，并以与（3-38）中相同的方式执行。 最后，状态估计与PDA过程完全相同。