MHT代码阅读(1)

最新推荐文章于 2025-02-13 16:28:00 发布

原创

最新推荐文章于 2025-02-13 16:28:00 发布 · 1.8k 阅读

19 ·

CC 4.0 BY-SA版权

文章标签：

#自动驾驶 #MHT

本文详细解读MHT（Multiple Hypothesis Tracking）算法中的FormTrackFamily部分，包括整体思路和代码实现。核心内容涉及轨道形成与维护、轨迹分数LLR的确认与删除策略、门控技术，以及轨迹修剪逻辑。通过实例解析，阐述如何用门控减少计算，并介绍MHT中轨迹的确认、删除和修剪条件。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

MHT代码阅读(1)

1. FormTrackFamily

1.1 整体思路

轨道形成和维护：表示维护所有轨道的中央轨道文件以及在这些轨道上执行的操作

接收新的观测值
用轨迹分数（LLR）确认和删除轨迹、用门控技术减少后期计算
1. 轨迹分数LLR
  - 利用轨迹分数来确认和删除是经典的SPRT方法的应用，SPRT中将LLR当作轨迹分数。如图6-1，LLR（或者分数L），在跟阈值T1、T2相比较。
  - 当L大于T2：宣布追踪确认
  - 当L在T1、T2之间：继续测试
  - 当L小于T1：删除轨迹
2. 门控
  - 一种计算效率高的方法是首先使用粗略形式的门控，例如分箱。使用分箱方法，测量空间被划分为一个单元格（或分箱）网格。然后，一条轨迹只与它的分箱和相邻分箱中的观测值进行比较。可以根据轨道质量和可用计算资源自适应地选择箱大小
  - 使用门内的所有观察更新现有轨迹，并形成外推轨迹（未使用任何当前观察更新）。此外，基本上所有观测都用于形成新轨迹的第一个点。因此，可能形成大量轨迹并且许多轨迹是不一致的，因为相同的观测被用于多个轨迹。
  - 当轨道没有共同的观察时，轨道被定义为兼容。通过在每次扫描之间保持先前不兼容的结果，可以实现效率的显着提高。因此，实际上对于每个轨道都有一个与该轨道不兼容的轨道列表。这种不兼容性会传递给从父轨道产生的后代轨道。
  - 在MHT 中，门控的目的只是为了从进一步处理中消除极不可能的替代方案。
  - 在 MHT中，通常选择更大的门，并使用 MHT 轨道修剪逻辑来消除不太可能的潜在轨道分支。

1.2 代码实现

初始化卡尔曼滤波矩阵
形成一个新的轨迹
- 设置ct存放当前观测数据
- 进入循环
  1. 初始化轨迹分数LLR
  2. 初始化状态向量
  3. 构建stateTreeSet结构体，用于存放tree，每个tree中包含Node和parent，Node是个元胞数组，存放的第一列是状态向量，后四列是协方差矩阵。
  4. 初始化scoreTreeSet（树的分数集合）、idTreeSet（树的编号集合）、activeTreeSet（活跃的树的集合）、obsMembership{i}（观察成员之间的关系）、familyID（集群编号）、trackID（轨迹编号）
用新的观察或者虚拟观察来更新轨迹
- 初始化treeDel=[]（要删除的树）、treeConfirmed=[]（保留的树）
- 进入外循环
  1. ct1存储已确认轨迹中较差的将要被修剪的轨迹的数量
    
    ct2存储已确认轨迹中好的无需修剪的轨迹数量
  2. 进入内循环
    - 确认轨迹是否是活跃的，不活动的树枝保存在tabuList，这些树枝将不参与繁殖
    - 用虚拟观察更新轨迹
    - 当某轨迹没有被确认时
      1. 与other_param.dummyNumberTH比较，是在main中就设置好的阈值，意思是当我们遇到一个点观测不清楚，我们最多可以再继续观测次数不能超过这个阈值，如果连续观察次数超过阈值都无法准确观察，那么就将这个树废除。
    - 当观察的数量不为0时
      1. 如果轨迹没有确认，那么继续用一个新的观察更新它（这一步相当于重复上一步）
        
        保存观察成员的关系以加速 updateICL 函数
      2. 如果轨迹确认了，但是轨迹不够好，那么ct1+1
        
        confscTH：确认轨道修剪（MOT）。平均检测置信度分数低于此阈值的已确认轨迹将被忽略。
        
        dummyRatioTH：基于虚拟观察数量与总观察数量的比率确认轨迹修剪，大于这个大于这个比率将会被忽略
      3. 如果轨迹确认了，并且足够好，那么ct2+1
  3. 退出内循环
- 最终判断树是被删除、确认还是修剪
  1. 轨迹删除
    - 对于刚确认的轨迹,如果这棵树的所有track的置信度都小于阈值,这棵树死亡
  2. 轨迹确认
    - 确认轨迹的条件：轨迹所在的树只有这一条轨迹、这条轨迹置信度大于阈值、连续TH次miss
  3. 轨迹修剪
    - 根据其分数修剪轨道分支

1.3 附录代码

function [obsTreeSetNew,stateTreeSetNew,scoreTreeSetNew,idTreeSetNew,...
    activeTreeSetNew,obsTreeSetConfirmed,stateTreeSetConfirmed,...
    scoreTreeSetConfirmed,activeTreeSetConfirmed,familyID,trackID,treeDel,...
    treeConfirmed,obsMembership]=formTrackFamily(obsTreeSetPrev,...
    stateTreeSetPrev,scoreTreeSetPrev,idTreeSetPrev,activeTreeSetPrev, ...
    obsTreeSetConfirmed,stateTreeSetConfirmed,scoreTreeSetConfirmed,activeTreeSetConfirmed,...
    selectedTrackIDs,cur_observation,kalman_param,other_param,familyID,trackID,cur_time,dt,Mea)
---------------------------------------------------------------------------------------------------------------------------
    observationNo=length(cur_observation.x);
    familyNo=length(obsTreeSetPrev);
    fai=[1 dt 0 0;  %dt采样周期
         0  1 0 0;
         0  0 1 dt;
         0  0 0 1];
    qx=kalman_param.qx;
    qy=kalman_param.qy;
    %qf=kalman_param.qf;
    kalman_Q_adjusted=[qx*dt^3/3 qx*dt^2/2   0         0; 
                       qx*dt^2/2 qx*dt       0         0;
                       0         0         qy*dt^3/3   qy*dt^2/2;
                       0         0         qy*dt^2/2   qy*dt];        %这是kalman的过程噪声矩阵
    %sen=cur_observation.sen;
    if observationNo ~= 0  % ~=是不等于
        obsTreeSet(observationNo,1)=tree;
        stateTreeSet(observationNo,1)=tree;
        scoreTreeSet(observationNo,1)=tree;
        idTreeSet(observationNo,1)=tree;
        activeTreeSet(observationNo,1