简介多假设跟踪MHT算法的Matlab实现程序面向雷达、视频监控等复杂场景下的多目标跟踪需求专为解决目标诞生、消失、分割、合并带来的关联不确定性而设计适合研究多假设跟踪思想及工程落地的开发者使用。资源共33个文件以21个m源文件为核心覆盖初始化、预测、更新、关联、分支合并与后处理等完整流程并附带C语言源码、Windows与Linux下的MEX编译文件、辅助数据文件及说明文档压缩包仅45KB轻量而完整。已有2033人学习下载适合具备Matlab基础的概率数据关联领域读者对照研读。代码中包含匈牙利算法、Murty算法、卡尔曼滤波更新与预测、假设生成等关键模块并提供了示例数据与可视化辅助函数可帮助读者理清MHT的实现细节据此调整门限与关联策略将算法迁移到自己的多目标跟踪项目中。 前阵子整理旧项目翻到两年前写的一个多目标跟踪程序。当时为了把编队飞行的几个目标分开试遍了最近邻、全局最近邻和JPDA最后在Matlab里手写了一个基于假设树的多假设跟踪器MHT才算把航迹ID交换的问题压下去。这篇博文就把这个程序的原理、模块划分、核心代码和调参经验逐层拆开讲清楚。适合刚接触多目标跟踪、想在Matlab里动手实现MHT的读者也适合已经调过GNN但觉得数据关联不够稳的同学参考。我尽量不按教科书的方式讲而是按“我当时怎么想的、代码怎么写的、踩了哪些坑”来组织内容。看完之后你应该能照着自己搭一套能跑的MHT程序出来。1. MHT到底是什么为什么数据关联不能拍脑袋决定1.1 一个让我转投MHT的实战场景先说一个很典型的场景两个目标靠近、交叉、再分开。如果用最近邻关联在交叉点附近很容易把量测分配给错误的轨迹一旦错误发生后边的滤波结果就会持续被带偏。最直观的表现就是两条航迹在交叉前后“换了个ID”看起来像两条轨迹互相穿透但目标的真实身份对不上。我第一次遇到这个问题时第一反应是把关联门限调小结果目标漏关联反而变多。后来又试JPDA它的思路是把所有“量测-轨迹”的关联概率加权平均相当于让轨迹看所有可能量测的加权平均。这个方法在目标密度不算高时表现还不错但一旦两个目标离得很近JPDA的合并效应会把两条轨迹拉成一条。因为JPDA本质上只保留了上一次的关联结果没有把“历史选择”一起纳入决策。MHT的办法是完全换了一个思路不到最后时刻不做硬决策。它把“这次扫描中哪个量测属于哪条轨迹”的每一种可能组合都保留下来形成一颗假设树然后用贝叶斯后验概率给每个分支打分。等新数据来了再做全局最优的剪枝把概率低的分支砍掉。这样即使某一次关联错了只要对应分支概率不是太高后续仍然有机会被纠正回来。1.2 三条技术路径的对比把三种方法放在一起看差异就很清楚最近邻/全局最近邻一次扫描内做一次最优分配决策后不再回头。优点是计算快缺点是对交叉、机动、密集杂波非常敏感。JPDA对当前扫描的所有可行关联做概率加权相当于“模糊关联”。优点是抗杂波强缺点是轨迹身份融合目标间距小的时候容易“吞并”。MHT维护所有可行关联的历史假设延迟决策用全局概率评分。优点是抗交叉、抗杂波、能保持目标身份代价是计算复杂度高工程实现繁琐。MHT并不适合所有场景。如果目标少、杂波低、只需要简单跟踪用全局最近邻就够了没必要把系统搞复杂。但如果目标密集、交叉频繁、杂波严重或者任务要求长时间稳定保持航迹IDMHT几乎是唯一靠谱的选择。1.3 假设树、轨迹得分和剪枝的基本逻辑MHT里的核心数据结构是“假设树”。每条轨迹对应一棵树树的根节点是轨迹起始时刻每个节点代表一次扫描中该轨迹关联了哪个量测。沿着树根走到某个叶子代表这条轨迹走到当前时刻的一种完整关联历史。每个关联历史对应一条候选轨迹也对应一个概率得分。这个得分不是简单的一次关联似然而是累积的贝叶斯后验概率。如果某条轨迹在当前时刻关联到一个量测得分会增加如果这条轨迹没有量测关联漏检得分会乘以漏检概率如果某个量测没有匹配任何已有轨迹它会触发一条新轨迹的初始化并乘以新生目标概率。有了概率得分之后就可以做剪枝。最常用的是N-scan剪枝保留最优分支在N次扫描前的节点把该节点下的其他分支全部砍掉。因为前N次的关联概率已经累积了足够多的证据早于那个时间窗口的决策基本可以认定为定局。剪枝是MHT工程化的关键没有剪枝假设树会指数增长任何机器都扛不住。2. Matlab实现前要想清楚的模块划分2.1 程序文件怎么拆写MHT程序最容易犯的错误是一上来就堆代码所有逻辑全挤在一个脚本里。我建议按功能把程序拆成独立函数每个函数只做一件事这样调试的时候能单步验证每个环节。我当时的文件结构大致是这样mht_main.m主流程负责读取量测数据、初始化滤波器、按时间步循环调用各模块。init_tracker.m初始化系统参数生成轨迹对象和假设对象。mht_predict.m所有轨迹的状态预测统一调用卡尔曼滤波预测方程。mht_gate.m量测和轨迹之间的门控判断输出每个轨迹的候选量测集合。mht_cluster.m把轨迹和量测划分成互不影响的簇降低假设组合规模。mht_generate_hypotheses.m在簇内枚举所有可行关联假设。mht_score_track.m计算每条轨迹的累积得分更新假设概率。mht_prune.m执行N-scan剪枝和全局分支删除。mht_update_track.m确认、保持、删除轨迹输出最终航迹。mht_plot.m绘制若干帧量测、航迹和ID标签。拆完之后主循环会变得非常简洁预测、门控、聚类、生成假设、得分、剪枝、更新轨迹每个环节调用一个函数。这样也方便你单独测试某个模块。比如门控写得对不对可以只跑mht_gate和绘图函数看候选量测。2.2 轨迹、假设、簇的数据结构定义Matlab里没有C那种class的开发习惯时我建议先用结构体数组把字段定义清楚。等到逻辑跑通了再考虑改写成classdef。MHT涉及的几个核心对象一定要在初始阶段想明白。轨迹对象我这样定义id轨迹编号state状态向量我统一用[x, y, vx, vy]cov状态协方差矩阵score累积对数似然得分age轨迹存活时间步数confirmed是否已经确认parentBranch父分支标识用于N-scan回溯lastUpdateStep最后一次更新的时刻假设对象的核心是记录“哪条轨迹关联了哪个量测”包含以下字段tracks假设包含的轨迹ID列表measurements轨迹对应的量测ID列表0表示漏检score该假设的总概率parentH父假设标识在实际程序里轨迹和假设的对应关系通常用矩阵表示行代表轨迹列代表量测元素1代表“该轨迹关联了该量测”。矩阵的每一列最多只能有一个1因为一个量测最多来自一个目标每一行可以全是0代表该轨迹当前时刻漏检。簇的概念稍微绕一点。如果两个轨迹共享同一个候选量测或者两个量测能通过轨迹形成关联链它们就被分到同一个簇里。簇与簇之间没有公共量测因此假设概率可以独立计算最后相乘即可。这是MHT避免全局组合爆炸的关键一步。2.3 主循环的整体流程MHT的主循环不复杂但每个环节都依赖上一步的结果顺序不能乱第一步对每条轨迹做状态预测。这里就是标准的卡尔曼滤波预测得到预测状态和预测协方差。第二步计算量测和轨迹之间的隶属关系做椭圆门控。门控的作用是只保留“有可能属于这条轨迹”的量测门外的量测直接忽略。第三步聚类。根据门控结果把轨迹和量测分成若干簇。第四步在每个簇内枚举所有可行关联假设。这一步是MHT最核心也最容易出问题的地方。第五步计算每个假设的概率得分归一化后传到下一步。第六步剪枝。保留得分最高的若干分支删除低分分支执行N-scan剪枝。第七步更新轨迹状态。对每个保留分支用对应的量测去更新卡尔曼滤波器的状态和协方差。第八步轨迹管理。判断哪些轨迹可以确认、哪些需要删除、哪些可以输出。我习惯把主循环写成20行以内的代码所有细节都丢给函数。这样可读性非常高出bug时也能快速定位到具体环节。3. 核心代码实现假设生成、得分计算与剪枝3.1 门控先用马氏距离把无关量测挡在门外门控不是MHT独有的但它是控制假设规模的第一道防线。门控阈值太小会漏掉真实关联太大则会把大量杂波量测放进来。我常用的做法是用马氏距离的平方做门限和马氏距离相比欧氏距离马氏距离考虑了状态估计不确定性和量测噪声更合理。对二维位置量测残差的马氏距离平方服从自由度为2的卡方分布取95%置信度时门限值大约是5.99。Matlab里直接用chi2inv函数就能算function gate computeGating(z, track, R) S track.cov(1:2,1:2) R; % 新息协方差 y z - track.state(1:2); % 残差 d2 y / S * y; % 马氏距离平方 gate d2 chi2inv(0.95, 2); end这里有个很容易踩的坑我把S算成预测协方差矩阵加上量测噪声R有些教程会直接用track.cov(1:2,1:2)做分母忽略了量测噪声。这样门控会偏紧真实关联被挡在门外的概率会显著上升。因为卡尔曼滤波里的新息协方差本来就是预测协方差加量测噪声门控也必须用同一个协方差。3.2 假设生成深度优先枚举可行关联假设生成是整个MHT里最让人头疼的部分。如果直接用nchoosek做排列组合再逐个判断是否满足条件很快就会爆炸。我建议用递归的方式做深度优先搜索一边枚举一边剪掉非法组合。一个簇内如果有M条轨迹和N个量测理论上可能的分配方案是每个量测最多分配给一条轨迹每条轨迹最多关联一个量测或漏检。写成递归函数大概是这个样子function allAssignments generateAssignments(gatedMeas, M) allAssignments {}; partial zeros(1, M); % 初始所有轨迹都不关联量测 dfs(1, partial); function dfs(trackIdx, partial) if trackIdx M allAssignments{end1} partial; return; end % 候选量测0表示漏检其他为通过门控的量测编号 candidates [0, gatedMeas{trackIdx}]; used partial(1:trackIdx-1); for c candidates if c 0 || ~ismember(c, used) next partial; next(trackIdx) c; dfs(trackIdx 1, next); end end end end这段代码有三点值得注意一是candidates里包含了0表示轨迹当前时刻漏检。漏检分支必须保留否则真实目标一旦被遮挡航迹直接断裂。二是量测编号不能重复。同一时刻一个量测只能属于一条轨迹所以递归到下一层时要检查当前量测是否已经被前面的轨迹占用。三是这种深度优先搜索天然只生成合法假设不需要事后过滤。复杂度和门控后的候选量测数量直接相关。如果某条轨迹的候选量测有5个那么单条轨迹的分支就是6M条轨迹的组合数仍然是指数级的。所以递归搜索还不够必须配合后续的聚类和剪枝才能把实际运行规模控制住。3.3 轨迹得分用对数似然累积判断优劣每个假设的概率得分是MHT决策的核心依据。我一开始直接用乘积形式计算概率结果迭代十几步以后数值直接下溢变成0。后来改成对数似然累加问题就解决了。对数得分的更新可以写成function score updateScore(score, z, zPred, S, pD, betaFA, betaNT) if ~isempty(z) % 检测到目标并关联量测 likelihood mvnpdf(z(:), zPred(:), S); score score log(pD * likelihood / (betaFA betaNT)); else % 轨迹漏检 score score log(1 - pD); end end其中pD是检测概率betaFA是虚警密度单位面积每秒出现的杂波数量betaNT是新生目标密度。这三个参数对得分影响很大。仔细看这个公式就能明白pD越高检测到量测时得分贡献越接近log(likelihood)说明这个关联越可信pD越低漏检分支的得分惩罚越小轨迹越能扛得住短暂遮挡。betaFA越高新量测越不可能来自虚警所以关联到一个量测的增益会被压低。这里有一个被很多资料忽略的细节mvnpdf计算的是连续量测下的概率密度值。这个值可能大于1取对数以后甚至可能为正数这很正常因为我们最终比较的是相对得分不是绝对概率。不要在得分上做无谓的归一化只要在剪枝时比较大小就够了。只有跨聚类合并为全局假设概率时才需要把各簇内得分转换为概率后相乘。3.4 N-scan剪枝和簇内合并得分算完假设树上的分支数量已经爆炸过了。N-scan剪枝的思想很简单在当前时刻取最高得分分支回溯到N次扫描之前的节点只保留这个节点下的所有后代分支其他分支全部删除。因为决策越早证据越充分N次扫描后依旧低分的分支基本不可能翻盘。Matlab里实现N-scan剪枝关键在于每个轨迹对象里记录每次扫描时的父节点索引。剪枝时从叶子节点反向找父节点定位到N层前的某个节点然后把该节点的其他子分支全部丢弃。这个操作不复杂但要小心索引错位。我建议用parentBranch字段记录“这个分支在上一扫描时的全局分支编号”剪枝时一层层往上找。聚类和剪枝可以配合使用。每个簇独立做假设生成和概率计算那么剪枝也可以按簇进行。簇与簇之间没有关联一个簇的分支剪切不影响另一个簇的得分。这样内存占用会明显减小。我实测过没有聚类时10个目标、30个杂波量测的组合假设数轻松破几十万做好聚类后大多数场景下每个簇的假设数只有几百。4. 参数调优与工程化心得4.1 影响MHT效果的五个关键参数MHT里没有“万能参数”但以下五个参数对最终效果影响最大调参时优先动它们参数含义推荐范围调大影响调小影响门控阈值马氏距离平方上限2D取5.993D取7.81漏关联少但候选组合多计算轻但易漏真关联N-scan窗口剪枝延迟深度3~5次扫描决策更全局计算开销大收敛快但早期错误难纠正最大假设数每个簇保留分支上限500~2000精度高内存暴涨跑得快但可能砍掉正确分支P_D检测概率0.7~0.99漏检分支惩罚小抗遮挡强更信任检测量测缺失时航迹易断杂波密度betaFA单位面积虚警数量按场景实测估计新量测更难建立新轨迹虚警容易形成假轨迹这几个参数之间的耦合关系要特别注意。门控阈值和最大假设数是一对矛和盾门控宽了候选量测多假设数必然上涨如果你不想改门控就只能把最大假设数压小但这样可能把正确分支剪掉。我习惯先固定门控阈值再根据实际运行时假设数量去调最大假设数。4.2 如何把demo程序改成真正能用的跟踪器从demo到工程可用中间隔着几步看不到但很关键的改造。第一个改造是量测噪声矩阵R要按真实传感器标定。demo里通常写死一个对角阵但实际场景中雷达测距和测角的误差是随距离变化的。我踩过一次很惨的坑直接用固定R结果远距离目标门控门限算出来异常大把几十个杂波点全放进来了假设组合直接爆炸。后来改成距离相关噪声模型门控效果立刻正常。第二个改造是时间步长不均匀的问题。demo通常假设每帧时间间隔相同但真实数据流经常有丢帧。你可以把卡尔曼预测里的时间参数dt作为一个输入变量每次预测前根据当前帧和上一帧的时间戳更新dt而不是写死常量。第三个改造是轨迹起始策略。demo经常用“单个量测即起始新轨迹”这在低虚警场景能跑但杂波高的时候会产生大量假轨迹。我习惯用滑窗确认连续3次扫描中至少2次有关联量测才把轨迹标为确认。注意这里的“2次”不要求连续容忍漏检的同时排除了孤立杂波点。4.3 我踩过的三个坑第一个坑是全穷举带来的内存崩溃。我最早用nchoosek生成所有候选组合再逐条判断合法性程序跑了几帧就内存耗尽。改用深度优先递归之后的运行内存降低了两个数量级。这个坑几乎每个MHT初学者都会踩建议直接用递归写法。第二个坑是轨迹得分归一化时机错误。我一开始在每个簇内算完得分就归一化导致簇和簇之间再乘的时候全局概率被算错了。MHT里各簇是条件独立的概率相乘应该在最后进行。正确的做法是保存每个簇的对数得分最后相加得到全局假设的对数得分。第三个坑是Matlab矩阵运算和cellfun的性能差异。同样一个“遍历所有轨迹做门控”的操作用循环写200行代码改成cellfun后速度提升可能不到20%但代码可读性差很多。我最终选择的是把“门控”这个操作彻底向量化一次性算出所有量测和所有轨迹之间的马氏距离矩阵而不是逐条轨迹循环。在Matlab里矩阵化思维比任何语法技巧都重要。5. 常见问题与排查技巧实录5.1 假设组合总是爆炸这是MHT程序最常被问到的问题。遇到这种情况先不急着重写代码按顺序排查第一门控阈值是不是太宽检查一下每帧平均每个轨迹的候选量测数量如果超过5个就得缩小门限第二聚类是不是生效了如果所有轨迹被分到同一个簇说明目标之间靠得太近或者门控太宽第三最大假设数是不是设得太高可以先用100压一压看跟踪效果损失多大。我见过另一个不常见但致命的原因量测噪声R矩阵设置过小导致马氏距离偏大门控被迫放宽。这时候调门控阈值治标不治本必须把R改回符合实际的值。5.2 航迹频繁断裂航迹断裂最常见的原因是检测概率pD设得太低。pD低意味着漏检分支的得分惩罚小轨迹即使长时间没有量测也能存活但代价是大量虚假分支会长期占用资源。如果pD已经很高比如0.95航迹还在断那要检查轨迹删除条件。我在程序里对“未确认轨迹”和“已确认轨迹”分别设了不同的删除阈值未确认轨迹连续2帧无量测就删已确认轨迹允许连续5帧无量测。这样短时遮挡不会破坏确认航迹同时假轨迹快速消亡。5.3 目标ID切换严重ID切换的本质是“正确分支的得分”在交叉过程中被错误分支超过了。解决思路有三个加大N-scan窗口让交叉前的关联证据更多地参与决策提高得分门槛轨迹的确认阈值调高避免低置信分支被输出在轨迹管理里做身份平滑输出ID前检查“当前最优分支”和前几帧的ID是否有连续继承关系如果发生突跳需要用得分对比来确认。5.4 运行速度太慢如果程序逻辑没问题纯粹是慢优先优化三个地方门控部分的距离矩阵计算改成向量化假设生成部分的递归搜索增量式更新而不是每帧从零开始枚举剪枝部分的矩阵操作避免用cellfun套函数句柄。做完这三步大部分场景的耗时能降到原来的三分之一以内。如果还想更快可以考虑把最深层的假设生成函数用Matlab Coder转成mex。我自己是在一个1000帧目标密集仿真场景里做的实测mex化之后单帧耗时从800ms降到120ms左右。这个收益很明显但前提是函数要写得足够规范变量类型一致循环边界清晰否则Coder报错能报到你怀疑人生。我个人在写这个MHT程序时最有感触的一点是MHT的难点从来不是数学公式而是把公式转化成能真正处理长序列数据的工程代码。假设生成、得分累积、N-scan剪枝、轨迹管理每一个环节单独拿出来都不复杂但合在一起就会产生大量边界情况。建议你先从两目标交叉、低杂波的仿真数据开始跑通再逐渐增加目标数量和杂波密度。每一步只改一个参数观察它对假设数量、航迹完整性和ID保持的影响这样调参才不会盲目。如果你已经跑通了一个基础版本可以试着往里面加航迹起始确认、幅度信息辅助门控或者多传感器量测融合这些都是MHT后续扩展的常见方向。本文还有配套的精品资源点击获取