目标跟踪
目标跟踪的本质是，基于前序状态与当前含噪的测量数据，估计并测量目标在连续时间内的真实运动轨迹，并维持目标的身份一致性。雷达点云跟踪遵循以下四个步骤：
- 目标检测与量测提取。从点云中提取出当前帧的目标候选位置（距离+速度联合聚类），量测数据通常存在虚警和漏检。
- 状态估计与滤波。基于运动学模型预测目标当前位置，并用新量测对状态进行更新修正。常用算法，卡尔曼滤波（线性/弱非线性），扩展卡尔曼滤波或无迹卡尔曼滤波（强非线性），粒子滤波（非高斯/复杂非线性）
- 数据关联。解决哪条航迹对应哪条新量测的问题。单目标/简单场景：最近邻算法、匈牙利算法。高密度/杂波场景：联合概率数据关联、多假设跟踪。
- 航迹管理。维护航迹的生命周期。连续M帧关联成功则确认为新航迹。利用卡尔曼预测和更新不断刷新航迹新位置。连续N帧未关联上量测，则删除该航迹。
一、扩展卡尔曼滤波（Extended Kalman Filter，EKF）
标准卡尔曼滤波（KF）有一个非常严苛的前提假设：系统状态转移矩阵和观测矩阵必须是线性 的，且噪声服从高斯分布。在理想线性系统中，一个高斯分布经过线性变换后，依然是高斯分布（只需维护均值 μ 和协方差 P）。但在实际雷达或导航系统中，非线性 无处不在，如运动模型非线性，观测模型也非线性（极坐标转笛卡尔坐标）。
EKF 的核心思想 非常直接："既然非线性函数不好处理，那就在当前估计值附近，把它局部线性化 ！"EKF 利用一阶泰勒级数展开（First-order Taylor Expansion），将非线性函数在当前状态估计点附近切线化，从而用线性矩阵来近似非线性变化。对多维向量函数求解一阶偏导数，得到的就是雅可比矩阵（Jacobian Matrix）：F=∂f/∂x,H=∂h/∂xF=∂f/∂x,H=∂h/∂xF=∂f/∂x,H=∂h/∂x 假设系统状态方程和观测方程如下：
xk=f(xk−1,uk)+wkx_k=f(x_{k-1},u_k )+w_k xk=f(xk−1,uk)+wkzk=h(xk)+vkz_k=h(x_k )+v_k zk=h(xk)+vk其中uku_kuk代表系统的控制输入向量，是人为或外部控制系统"主动施加给被控对象的指令/驱动信号"，如无人机飞控系统发给电机的转速控制信号或舵机偏转角，汽车驾驶员踩下油门的踏板开度或转动的方向盘角度，纯被动雷达目标跟踪雷达无法得知驾驶员操作控制情况，方程简化不写uku_kuk。wk∼N(0,Q)w_k∼N(0,Q)wk∼N(0,Q)表示过程噪声，vk∼N(0,R)v_k∼N(0,R)vk∼N(0,R)表示测量噪声。
预测阶段 。状态预测（直接带入非线性状态转移函数 fff）：x^k−=f(x^k−1,uk)\hat{x}k^-=f(\hat{x}{k-1},u_k )x^k−=f(x^k−1,uk) 协方差预测（使用状态雅可比矩阵 FkF_kFk 进行线性投影）：Pk−=FkPk−1FkT+QP_k^-=F_k P_{k-1} F_k^T+QPk−=FkPk−1FkT+QFk=∂f/∂x∣x^k−1F_k= ∂f/∂x| \hat{x}{k-1}Fk=∂f/∂x∣x^k−1是状态转移函数fff 在上一步估计值 x^k−1\hat{x}{k-1}x^k−1 处的雅可比矩阵
计算卡尔曼增益KkK_kKk 。Kk=Pk−HkT(HkPk−HkT+R)−1K_k=P_k^- H_k^T (H_k P_k^- H_k^T+R)^{-1}Kk=Pk−HkT(HkPk−HkT+R)−1
状态更新 ，残差由实际测量 zkz_kzk 减去非线性观测预测 h(x^k−)h(\hat{x}_k^- )h(x^k−) 得到x^k=x^k−+Kk(zk−h(x^k−))\hat{x}_k=\hat{x}_k^-+K_k (z_k-h(\hat{x}_k^- ))x^k=x^k−+Kk(zk−h(x^k−)) 协方差更新 ，Pk=(I−KkHk)Pk−P_k=(I-K_k H_k ) P_k^-Pk=(I−KkHk)Pk−
EKF的局限性：EKF 仅保留了一阶泰勒展开，忽略了高阶项。如果系统非线性程度极高（如高剧烈机动或极度弯曲的坐标转换），局部线性化近似误差巨大，会导致滤波发散。在复杂高维状态下（如 15 维惯导 SLAM），手动求导计算雅可比矩阵极其繁琐且极易出错。
二、无迹卡尔曼滤波（UKF）
EKF用雅可比矩阵拟合非线性函数，但对于强非线性系统，切线和曲线相差很远，导致误差巨大，滤波发散。UKF的逻辑是，近似概率分布比近似任意非线性函数更容易 。UKF直接在当前状态高斯分布的中心和四周挑选几个最关键的代表性采样点（称为sigma点），把这几个sigma点直接代入原始的非线性函数f(x)f(x)f(x) 或 h(x)h(x)h(x) 中算完，再通过加权平均，重新统计出变换后的新高斯分布的均值和协方差（假设系统状态服从高斯分布）。
假设状态维度为 LLL（例如 x,y,vx,vyx,y,v_x,v_yx,y,vx,vy，则 L=4L=4L=4），UKF的运行步骤如下：
- Sigma 点采聚（Sigma Points Sampling）
根据当前的均值 x^\hat{x}x^ 和协方差矩阵 PPP，依据特定的对称分布规则挑选出 2L+12L+12L+1 个 Sigma 点：1 个中心点（即当前估计的均值x^\hat{x}x^ ，2L2L2L 个分布在中心点四周对称位置的点，距离由协方差 PPP 的平方根（通常通过 Cholesky 分解算出）和缩放因子确定。 - 非线性传播
把这 2L+12L+12L+1 个 Sigma 点逐个直接代入非线性状态转移方程 f(χ)f(χ)f(χ) 或观测方程 h(χ)h(χ)h(χ)中，得到映射后的新 Sigma 点集。不需要求导/计算雅可比矩阵。 - 加权重构
利用预先设定的权重系数 WmW_mWm（均值权重）和 WcW_cWc（协方差权重），把经过非线性映射后的那些 Sigma 点重新合成出：预测的均值 x^−\hat{x}^-x^−，预测的协方差 P−P^-P−。
三、粒子滤波（Particle Filter, PF）
KF适用于纯线性+高斯噪声，EKF/UKF适用于非线性/弱非线性+高斯噪声，PF适用于强非线性+非高斯噪声 （突发脉冲噪声、多模态分布、双峰分布）。KF/EKF/UKF都假设状态是一个高斯分布，高斯分布用一个均值和协方差就能描述。但很多实际场景，概率分布不是漂亮的高斯曲线。
粒子滤波在状态空间中随机撒下大量带权重的粒子，粒子密集的地方，概率高，稀疏的地方，概率就低。即，用粒子的分布和权重直接模拟任意形状的概率密度函数。PF标准步骤：
- 初始采样
在整个可能的区域内随机生成 NNN 个粒子 x0(i)x_0^{(i) }x0(i)，初始时每个粒子的权重相同，w0(i)=1/Nw_0^{(i)}=1/Nw0(i)=1/N。 - 状态预测
将每一个粒子直接代入非线性的状态转移方程，再加上随机的过程噪声。
xk(i)=f(xk−1(i),uk)+wk(i)x_k^{(i)}=f(x_{k-1}^{(i)},u_k )+w_k^{(i)}xk(i)=f(xk−1(i),uk)+wk(i)让这1000个粒子根据运动模型各自向前跑一步，粒子群会自然扩散。 - 权重更新
传感器收到了一次真实观测数据 zkz_kzk。根据观测方程与观测噪声分布，计算每一个粒子与真实观测值的"吻合程度"。 如果某个粒子预测的位置和雷达测得的位置非常近，说明这个粒子"猜得准"，赋予它极高权重。如果某个粒子离真实测得位置十万八千里，赋予它极低权重。最后对所有粒子的权重进行归一化（使权重之和等于 1）。
这里每个粒子是如何匹配上雷达观测值的？核心概念-似然函数 。计算预测粒子x(i),y(i)x\^{(i)},y\^{(i)}x(i),y(i)的观测值r^(i)=sqrt((x(i))2+(y(i))2)\hat{r}^{(i)}=sqrt((x^{(i)} )^2+(y^{(i)} )^2) r^(i)=sqrt((x(i))2+(y(i))2)θ^((i))=arctan(y(i)/x(i))\hat{θ}^((i) )=arctan(y^{(i)}/x^{(i)})θ^((i))=arctan(y(i)/x(i))与雷达实际测量值zk=robs,θobsz_k=r_{obs},θ_{obs} zk=robs,θobs之间的残差，Δr(i)=robs−r^(i)Δr^{(i)}=r_{obs}-\hat{r}^{(i)}Δr(i)=robs−r^(i)Δθ(i)=θobs−θ^(i)Δθ^{(i)}=θ_{obs}-\hat{θ}^{(i)}Δθ(i)=θobs−θ^(i)代入高斯似然函数计算权重，假设雷达的距离误差服从标准差为 σrσ_rσr 的高斯分布，角度误差服从 σθσ_θσθ 的高斯分布（协方差矩阵为 RRR）。这个粒子匹配上真实观测值的概率密度（似然值）为wk(i)∝p(zk∣xk(i))=1/(2πσrσθ)exp(−1/2(Δr(i))2/(σr2)+(Δθ(i))2/(σθ2))w_k^{(i)}∝p(z_k∣x_k^{(i)} )=1/(2πσ_r σ_θ ) exp(-1/2 (Δr\^{(i)} )\^2/(σ_r\^2 )+(Δθ\^{(i)} )\^2/(σ_θ\^2 ))wk(i)∝p(zk∣xk(i))=1/(2πσrσθ)exp(−1/2(Δr(i))2/(σr2)+(Δθ(i))2/(σθ2))如果残差 Δr,ΔθΔr,ΔθΔr,Δθ 非常接近 0（粒子位置与雷达测得的位置几乎重合），指数项接近 exp(0)=1exp(0)=1exp(0)=1，计算出的似然概率极高 → 该粒子获得高权重。如果残差很大（粒子跑偏了），指数项是负很大数（如 exp(−50)）exp(-50)）exp(−50)），计算出的似然概率趋近于 0 → 该粒子获得极低权重。将所有NNN 个粒子的权重相加，除以总和，使所有新权重的和为 1wˉk(i)=wk(i)∑j=1Nwk(j)\bar{w}k^{(i)} = \frac{w_k^{(i)}}{\sum{j=1}^{N} w_k^{(j)}}wˉk(i)=∑j=1Nwk(j)wk(i)权重最高的那个粒子，代表的就是当前最符合雷达实际观测数据、可能性最大的状态估算。虽然权重最高的粒子匹配度最好，但在实际应用（比如输出目标的坐标 x,yx,yx,y 和速度 vx,vyv_x,v_yvx,vy）时，我们一般不直接单选这个"冠军粒子"，而是计算所有粒子的加权平均值（Weighted Average）获得最稳健的估计结果。x^k=∑i=1Nwˉk(i)⋅xk(i)\hat{x}k = \sum{i=1}^{N} \bar{w}_k^{(i)} \cdot x_k^{(i)}x^k=i=1∑Nwˉk(i)⋅xk(i) - 重采样
只做123，跑几步后，绝大多数粒子的权重会接近0（粒子退化问题），只有极少数粒子占有巨大权重，造成算力的浪费。所以"克隆强者，淘汰弱者"，重采样：权重大的粒子被复制多个，权重小的粒子直接被丢弃。重采样后，所有新粒子的权重重新恢复为相同的 1/N1/N1/N，但粒子群已经自适应地集中到了高概率区域。 - 状态输出与下一轮准备
重采样后的粒子群集中在目标周围，此时可以通过这群粒子计算出当前时刻目标的最优估计状态（位置、速度等）。最常用的是粒子算术平均（重采样后所有粒子权重恢复相等）x^k=1/N∑i=1Nxk(i)\hat{x}k = 1/N\sum{i=1}^{N} x_k^{(i)}x^k=1/Ni=1∑Nxk(i) 如果重采样后的粒子聚集成了 2~3 个簇（比如目标可能分叉或被短时遮挡），可以先做一次简单的 DBSCAN 聚类，然后分别计算最大粒子簇的中心作为目标位置。此时，算法输出当前帧的目标位置及速度，完成本帧跟踪任务。
很多新粒子是由同一个高权重的"母粒子"完全复制过来的，它们的位置一模一样，失去了样本的多样性。如果直接带着这些完全一样的粒子进入下一帧，粒子群会迅速"僵化"，失去应对目标突然变向或加速的能力。在重采样后的每个粒子位置上，人工加入一点微小的高斯高频噪声（Jitter/MCMC 步）：xk(i)=xk(i)+ϵ(i),ϵ(i)∼N(0,σjitter2)x_k^{(i)}=x_k^{(i)}+ϵ^{(i)},ϵ^{(i)}∼N(0,σ_{jitter}^2 )xk(i)=xk(i)+ϵ(i),ϵ(i)∼N(0,σjitter2)处理完当前帧后，这群"高质量且具备一定多样性"的粒子就成为了新的基准粒子集。当雷达接收到下一时刻（k+1k+1k+1 帧）的数据时，系统回到算法起点，开启下一次循环，状态预测->权重更新->重采样->状态输出与下一轮准备。
PF应用场景，机器人SLAM自定位（机器人刚开机时完全不知道自己在地图的哪个房间，此时概率是多峰分布，PF 能很好地处理这种"全局定位"问题），高动态遮挡下的视觉目标跟踪，当目标被树木或建筑物频繁长久遮挡时，后验概率变成多峰分布，PF 能维持多个潜在可能的位置。PF局限性，计算复杂度高，在车规级或DSP芯片上实时运行压力巨大，无法满足毫秒实时帧率要求。雷达跟踪场景中，大多数系统的过程噪声和观测噪声经过处理后基本可以近似为高斯噪声，使用UKF或IMM-UKF就能以极低的计算量达到足够的跟踪精度。
四、数据关联---匈牙利算法
核心任务，拿上一帧跟踪到的目标（航迹 Track），去和这一帧雷达/传感器检测到的点（量测 Measurement）进行配对。打破GNN最近邻算法的贪心局限，求解全局总代价最小。把所有可能的配对组合组合起来，看哪一种组合算出来的总距离加起来最小。适用于目标密集、互相靠近交叉的复杂场景。
五、数据关联---联合概率数据关联（Joint Probabilistic Data Association, JPDA）
在密集多目标或强杂波场景下，传统的"确定性匹配"（如 GNN、匈牙利算法）会面临巨大的挑战。当两辆车距离极近时，雷达测量点会混在一起。硬性指定"点 1 归车 A，点 2 归车 B"极易选错。一旦选错，卡尔曼滤波就会带入错误的测量值，导致航迹发散或 ID 互换（ID Switch）。如果某个干扰点刚好离目标很近，硬匹配可能会误把杂波当目标，拉偏轨迹。
JPDA 的思路：不强行决定这个点到底属于谁，而是计算所有合理的分配组合（联合假设），求出每个测量点属于各个目标的概率，然后把所有相关点的贡献加权混合起来更新目标状态。假设场景中有 MMM 个已知目标（Tracks）和 NNN 个雷达测量点（Measurements）。
-
构建波门与确认矩阵
不是每个点都可能属于每个目标。首先根据卡尔曼滤波的预测位置和协方差，为每个目标画一个马氏距离波门（超椭球区域）。只有落在波门内的测量点，才会被纳入该目标的候选列表，以此大幅削减计算量。 -
生成联合事件/联合假设
把所有落在波门内的目标和测量点组合起来，列出所有物理上可行的分配方案（事件 θkθ_kθk）。物理可行性约束1：一个测量点最多只能来自于一个源（要么来自于目标 A，要么来自于目标 B，要么是杂波）。一个目标最多只能产生一个测量点（目标可能被检测到产生 1 个点，也可能漏检产生 0 个点）。
例如，眼前有两个目标（小王、小李），雷达测到了两个点（点 A、点 B）：
• 猜想 1：点 A 是小王，点 B 是小李。
• 猜想 2：点 A 是小王，点 B 是杂波（小李没被测到/漏检）。
• 猜想 3：点 B 是小王，点 A 是小李。
• ... -
计算联合事件概率
在上一步生成联合事件中，我们列出了物理上可能成立的猜想（联合事件θ_k），这里计算联合事件概率的目标是，计算每一个猜想成立的概率（可能性）有多大。要评估某一个猜想（比如"猜想 1：点 A 给小王，点 B 给小李"）合不合理，需要同时考核 3 个指标。
该猜想的总概率∝(匹配相似度)×(目标检测概率)×(杂波出现概率)该猜想的总概率∝(匹配相似度 )×(目标检测概率 )×(杂波出现概率 )该猜想的总概率∝(匹配相似度)×(目标检测概率)×(杂波出现概率)① 匹配相似度（测量似然 gijg_{ij}gij）。如果猜想里说"点 A 是小王"，我们就看点 A 离小王预测的位置近不近。越近（符合高斯分布中心），说明越像，这个似然值 ggg 就越大。越远，似然值 ggg 就会呈指数级暴跌（趋近于 0）。
假设目标 TiT_iTi 当前预测的测量值为 Z^i\hat{Z}iZ^i（比如预测雷达应该在 x=10m, y=20m 测到它），实际雷达测到的点为 ZjZ_jZj（比如 x=10.2m, y=19.8m ）。
计算测量残差yij=Zj−Z^iy{ij}=Z_j-\hat{Z}iyij=Zj−Z^i代入高斯似然函数，测量似然值的数学公式为
gij=p(Zj∣Ti)=1(2π)d/2∣Si∣1/2exp(−12yijTSi−1yij)g{ij} = p(Z_j \mid T_i) = \frac{1}{(2\pi)^{d/2} \vert{}S_i\vert{}^{1/2}} \exp \left( -\frac{1}{2} \mathbf{y}{ij}^T S_i^{-1} \mathbf{y}{ij} \right)gij=p(Zj∣Ti)=(2π)d/2∣Si∣1/21exp(−21yijTSi−1yij)其中，d为测量数据的维度（例如只有距离和角度则 d=2；如果有 3D 坐标 x,y,z 则 d=3）。SiS_iSi为残差协方差矩阵（Innovation Covariance），即预测不确定性与雷达测量噪声 RRR 的叠加（Si=HPi−HT+RS_i=HP_i^- H^T+RSi=HPi−HT+R）,∣Si∣|S_i |∣Si∣为矩阵 SiS_iSi的行列式。yijTSi−1yij\mathbf{y}{ij}^T S_i^{-1} \mathbf{y}{ij}yijTSi−1yij是著名的马氏距离 （Mahalanobis Distance）的平方 。
为什么使用马氏距离而不是欧式距离？因为传感器在不同方向上的误差/不确定性是不一样的（比如雷达测距很准、测角较差，误差范围是个椭圆而非正圆）。残差为 0（偏离为 0）：指数项为 exp(0)=1，似然值 gijg_{ij}gij 达到最大峰值 → 匹配度极高。偏离增大：马氏距离变大，指数项呈 exp(-大数) 暴跌 → 似然值急速衰减趋近于 0（比如偏离 3 倍标准差外，概率几乎为 0）。② 目标检测概率（PDP_DPD 与 1−PD1-P_D1−PD）。雷达不是 100% 完美无缺的，它有一定概率漏检（比如 PD=0.9P_D=0.9PD=0.9，代表 90% 概率能测到目标，10% 概率会漏检 1−PD=0.11-P_D=0.11−PD=0.1）。如果这个猜想里小王分到了某个点，说明小王被测到了，概率因子乘以 PD（0.9）P_D（0.9）PD（0.9）。如果这个猜想里小王什么点都没分到（猜想他漏检了），概率因子乘以 1−PD（0.1）1-P_D（0.1）1−PD（0.1）。
③ 杂波概率（γγγ）。假设雷达在空旷的地方，背景杂波出现的概率密度是 γγγ。如果这个猜想里把点 B 归为了杂波，那么乘以该区域出现杂波的概率 γγγ。
举个例子 ：假设场景，目标 T1T_1T1（检测概率 PD=0.9P_D=0.9PD=0.9），测量点 Z1Z_1Z1，背景杂波密度 γ=0.1γ=0.1γ=0.1。评估两个竞争猜想。
猜想A：Z1Z_1Z1 是目标 T1T_1T1 的回波。计算公式："匹配相似度" g11g_{11}g11×"检测到目标的概率" PDP_DPD。假设点 Z1Z_1Z1 离 T1T_1T1 很近，g11=2.0g_{11}=2.0g11=2.0。猜想 A 的未归一化得分 = 2.0×0.9=1.8
猜想B：Z1Z_1Z1 是虚假的野杂波（目标 T1T_1T1 漏检了）。计算公式：杂波概率γγγ×目标漏检的概率 (1−PD1-P_D1−PD )。猜想 B 的未归一化得分 = 0.1×(1-0.9)=0.01
算完所有猜想的得分后，把所有猜想的得分加起来作为分母，算各自的百分比（归一化）。所有猜想总分 = 1.8+0.01=1.81
猜想 A 成立的真实概率 = 1.8/1.81≈99.45%
猜想 B 成立的真实概率 = 0.01/1.81≈0.55%
通过这三步乘积，算法以 99.45% 的绝对优势认定：点 Z1Z_1Z1 就是目标 T1T_1T1，而不是杂波！ -
计算联合边缘概率并加权更新
把所有包含"点 ZjZ_jZj 分配给目标 TiT_iTi"的联合事件概率加和，得到最终的联合边缘关联概率 βijβ_{ij}βij：βij=P(Zj来自于Ti∣所有观测数据)β_{ij}=P(Z_j 来自于 T_i∣ 所有观测数据 )βij=P(Zj来自于Ti∣所有观测数据)最后，目标 T_i 在更新卡尔曼滤波时，不是用某一个点，而是把波门内所有点产生的创新残差（Innovation）按 βijβ_{ij}βij 进行加权求和：vi=∑j=1Nβij⋅(Zj−Z^i)v_i = \sum_{j=1}^{N} β_{ij} \cdot (Z_j-\hat Z_i)vi=j=1∑Nβij⋅(Zj−Z^i)用这个组合残差 viv_ivi 去更新卡尔曼滤波的状态和协方差。
举个例子 。设雷达看到了 2 个点（Z1,Z2Z_1,Z_2Z1,Z2），我们跟踪着 2 个目标（小王 T1T_1T1, 小李 T2T_2T2）。设在第三步我们计算出了所有互相排斥的联合事件（猜想）成立的概率。
猜想 1，点 Z1Z_1Z1 是小王，点 Z2Z_2Z2 是小李，60%
猜想 2，点 Z1Z_1Z1 是小王，点 Z2Z_2Z2 是杂波（小李漏检了）20%
猜想 3，点 Z2Z_2Z2 是小王，点 Z1Z_1Z1 是小李15%
猜想 4，点 Z1Z_1Z1 是小李，点 Z2Z_2Z2 是杂波（小王漏检了）5%
把所有包含"点 ZjZ_jZj 分配给目标 TiT_iTi"的联合事件概率加和的意思是，把所有支持"点 Z1Z_1Z1 是小王"的猜想概率加在一起：
点 Z2Z_2Z2 属于小王的概率= 15%
小王完全被漏检（没拿到任何点）的概率= 5%
残差就是 "传感器测到的点 ZjZ_jZj" 减去 "卡尔曼预测的小王位置 ZZZ ˆ" 的向量差距。小王不能只用点 Z1Z_1Z1（因为有 20% 的可能选错，导致轨迹被野杂波拉偏），也不能直接用点 Z2Z_2Z2。把波门内所有点产生的创新残差（Innovation）按 βijβ_{ij}βij 进行加权求和的意思是小王把每个点产生的拉力（残差 νjν_jνj），乘以这个点属于小王的概率（β1jβ_{1j}β1j），全部加在一起，算出一个综合的拉力 νtotalν_{total}νtotal：vtotal=80%×v1⏟点 Z1 提供的拉力+15%×v2⏟点 Z2 提供的拉力v_{total} = \underbrace{80\% \times v_1}{\text{点 } Z_1 \text{ 提供的拉力}} + \underbrace{15\% \times v_2}{\text{点 } Z_2 \text{ 提供的拉力}}vtotal=点 Z1 提供的拉力 80%×v1+点 Z2 提供的拉力 15%×v2算出了这个综合拉力 vtotalv_{total}vtotal之后，卡尔曼滤波的更新方程就和普通卡尔曼一模一样了X^k=X^k−+K⋅vtotal\hat{X}_k=\hat{X}k^-+K⋅v{total}X^k=X^k−+K⋅vtotal在实际编写JPDA或粒子滤波时，通常直接调用现成的多维高斯分布函数，如python代码：
python
import numpy as np
from scipy.stats import multivariate_normal
def calculate_likelihood(z_meas, z_pred, S_cov):
"""计算测量点 z_meas 与目标预测 z_pred 之间的似然度 g_ij
:param z_meas: 实际测量点向量 [x, y]
:param z_pred: 卡尔曼滤波预测的测量向量 [x_hat, y_hat]
:param S_cov: 创新协方差矩阵 (2x2)
:return: 似然值 (标量)
"""
# 1. 构建以 z_pred 为中心、S_cov 为协方差的高斯概率分布
gaussian_dist = multivariate_normal(mean=z_pred, cov=S_cov)
# 2. 直接计算测量点 z_meas 在该分布下的概率密度 (PDF)
g_ij = gaussian_dist.pdf(z_meas)
return g_ij六、数据关联---多假设跟踪（Multiple Hypothesis Tracking, MHT）
最近邻算法GNN只顾眼前，立刻做决定，容易错配。匈牙利算法/JDPA是顾及当下的协调者，只看当前帧的概率/分配，MHT是一个拥有多平行宇宙观察能力的终极延迟决策者。特点是，不在当前帧急于做出硬配对决定，而是将所有合理的配对假设画成一颗树，保留所有的可能性，多看几帧后续的测量数据，再回头裁决哪一条轨迹假设是真实航迹。核心逻辑：延时决策。
假设在第 k 帧，雷达在一个已知目标周围测到了 2 个点（点 A 和 点 B）。系统在内存中分裂出两个分支。分支1：假设目标去了点 A，继续用卡尔曼滤波向前预测。分支2：假设目标去了点 B，也继续用卡尔曼滤波向前预测。到了第k+1 帧、第 k+2 帧......如果后续点 A 的方向连续出现了新测量点，而点 B 的方向再也没有出现过点。分支1的累积似然概率越来越高；分支2因为连续漏检，累积似然概率暴跌。MHT此时斩断分支2，确定分支1才是真实航迹。
MHT 通常有两种实现范式：面向量测的 MHT（Measurement-Oriented MHT, HO-MHT） 和 面向航迹的 MHT（Track-Oriented MHT, TO-MHT）。"面向量测"把所有可能性列成巨型表格（容易内存爆炸），而"面向航迹"是为每个目标单独维护一棵"航迹树"，只在最后一步求解哪些航迹互不冲突。工业界常用的是后者（TO-MHT，更省内存）。其标准循环流程如下：
① 波门筛选与新候选航迹生成。
当第 k 帧收到一系列雷达点 Z1,Z2Z_1,Z_2Z1,Z2,...（量测数据）时：对于已经存在的每一条单树分支（候选航迹），根据卡尔曼滤波预测当前位置，以预测位置为中心画一个波门，看当前帧哪些雷达点落在了波门内。如果点 ZjZ_jZj 落在波门内，建立一个新分支（关联，假设该目标更新到了ZjZ_jZj）；如果点没落入波门，建立"目标漏检"分支，生成一条"本帧漏检"分支，目标仅靠卡尔曼滤波进行盲推预估；如果点远离任何已有的航迹波门，则为这些点独立建立"新目标"的树根。
② 计算航迹得分。
每条从树根到叶子节点的路径，都代表一条"单航迹假设"，每一个假设分支（航迹）从诞生到当前时刻，都会维护一个综合累积得分（Log-Likelihood Ratio, LLR）：L(k)=L(k−1)+ΔL(k)L(k)=L(k-1)+ΔL(k)L(k)=L(k−1)+ΔL(k)增量 ΔL(k)ΔL(k)ΔL(k) 综合考虑了：测量匹配度：高斯似然值（马氏距离越小得分越高）；检测概率 PDP_DPD：成功检测到加分，漏检扣分；杂波与虚警密度：如果假设该点是杂波，扣除杂波惩罚分；新目标诞生概率：建立新目标的先验得分。
③ 构建全局假设。
因为"一个雷达点只能分配给一个航迹"，算法将所有互斥的候选航迹组合成若干个全局假设（Global Hypotheses）。每个全局假设的得分是其包含的所有单个航迹得分之和。
④ 剪枝与树的控制。
如果听任假设树无限分裂，航迹树会发生严重的指数爆炸！算法必须在每帧结尾进行剪枝操作。低分剪枝（Score Pruning）：把累积得分掉到门槛以下的"垃圾分支"直接砍掉。K-Best 限制：每一树层最多只保留得分最高的前 K 个全局假设（如使用 Murty 算法求前 K 个最优解）。N-Scan 深度剪枝：设定一个延迟回溯窗口 N（例如 N=5）。如果到了当前第 k 帧，回溯到第 k-N 帧时，所有存活的高分假设都一致认为"第 k-N 帧的点 Z 属于目标 A"，那么第 k-N 帧的决策被彻底固化（Hard Assignment），第 k-N 帧之前的所有分叉树全部砍掉并释放内存，将树根向前推进到第k-N帧。
举个例子：假设跟踪 1 个目标，回溯窗口 N=2。第 1 帧：遇到 2 个模糊点 A,B→ 树分裂为两支（分支 A，分支 B）。此时不急着做决定。第 2 帧：分支 A 附近测到了点 A_2；分支 B 附近什么都没测到 → 分支 A 得分暴涨，分支 B 得分暴跌。第 3 帧：分支 A 附近又测到了点 A_3 → 此时所有得分高的假设都指向"第 1 帧选 A 是对的"。触发 N-Scan 裁决，砍掉分支 B，确定第 1 帧的轨迹为点 A！