GM-PHD滤波器详解：多目标跟踪、扩展目标与MATLAB源码实践

简介这是一份基于高斯混合概率假设密度GM-PHD滤波器的扩展目标多目标跟踪MATLAB实现面向雷达、计算机视觉等领域的研究者与工程师尤其适合需要处理非点状、有体积目标的跟踪场景。资源以GM-PHD核心滤波流程为主线配套扩展卡尔曼滤波EKF的雅可比矩阵计算、匈牙利数据关联、目标新生与更新、OSPA度量评估等模块构成一套较完整的算法研究框架。压缩包共含32个文件以30个.m脚本为主、2个txt说明为辅整体仅32KB代码轻量易读便于按函数模块逐段调试与二次开发。目前已有351人学习下载。通过该源码包读者可掌握GM-PHD滤波器的初始化、预测、更新、剪枝及性能评估全流程理解扩展目标跟踪中状态估计与数据关联的衔接方式也可将其作为算法对比或课程设计的起点。1. GM-PHD滤波器是什么多目标跟踪里“不需要关联”的那条路做过多目标跟踪的人第一反应都是“量测-航迹关联”。JPDA算边缘关联概率MHT维护全局假设到了密集杂波场景组合数会迅速压垮算力。GM-PHD滤波器换了一条路它根本不回答“这个量测属于哪个目标”而是直接估计目标状态集合的概率假设密度PHD也就是一阶统计矩。PHD在状态空间上的积分就是目标数的期望峰值位置就是目标状态的估计。加上“高斯混合”这个词意味着这个密度用一组带权重的高斯分量来近似更新方程因此有闭式解不需要粒子采样。这套思路特别适合两个场景一是目标数随时间变化、有目标出现也有目标消失二是杂波密度不低但还没到极端程度比如雷达近程监视、声呐被动跟踪、视觉多行人跟踪。MATLAB源码形式的GM-PHD实现通常还带着扩展目标extended target的量测划分模块这比标准GM-PHD又进一步——一个目标一个时刻会产生多个量测量测不再是一对一而是一簇对一。本文就按“PHD原理→GM实现→扩展目标建模→源码参数→排错调参”的顺序把这个滤波器讲透。2. PHD滤波到GM-PHD随机有限集、量测不确定性与高斯混合的落地前提2.1 目标数和量测数都不确定为什么还要算“密度”传统贝叶斯滤波里状态是一个随机向量后验是它的概率密度函数。多目标场景下目标集合本身是随机有限集RFS目标数随机每个目标的状态随机。完整的多目标后验分布在理论上要定义在“集合的集合”上直接递推在数学上可行工程上不可计算。PHD滤波的做法是降维不去求完整分布只求其一阶矩。这个矩的定义是对状态空间中任意区域 S∫_S D(x)dx 表示区域内目标数的期望。这个 D(x) 就是PHD也叫强度函数。这个降维会损失信息换来的是计算可行性。推导得到的PHD递推式在形式上很像单目标卡尔曼滤波的预测-更新但其中多了“目标数期望”的表达预测时存活目标强度乘以存活概率加上新生目标强度更新时每个量测对强度函数产生一个类似似然比的修正项。关键点在于PHD更新公式里存在对“所有可能关联假设”的隐式求和它被折叠进了“每个量测独立修正强度”的结构里。这使得PHD滤波器不需要显式枚举关联也就避开了组合爆炸。从实现角度看D(x) 没有解析形式必须近似。GM-PHD假设每个目标都满足线性高斯动态模型且量测模型也是线性高斯那么强度函数可以写成高斯混合形式——每个分量有自己的权重、均值和协方差。于是PHD递推就变成了对一组高斯分量的预测、更新、剪枝和合并。这也是库名里“GM”的来源。如果模型非线性要么用无迹变换扩展成UK-GMPHD要么退到粒子滤波但那些都超出标题里GM-PHD源码的范畴。2.2 GM-PHD的5步循环以MATLAB代码对照2.2.1 预测步新生与存活GM-PHD的一个循环包含预测和更新两步但工程上通常还要加剪枝合并和状态提取总共是4到5个阶段。预测时要处理两类分量存活的旧目标和新生目标。新生目标强度一般建模为若干个高斯分量的混合位置分布在可能出现目标的区域。常见的做法是在观测区域边缘或固定热点放置固定数量的高斯分量。MATLAB源码里预测过程通常是这样一段结构% 预测存活分量状态转移矩阵 F过程噪声协方差 Q for i 1:length(w_prev) m_pred{i} F * m_prev{i}; P_pred{i} F * P_prev{i} * F Q; w_pred(i) pS * w_prev(i); % pS为存活概率 end % 合并新生分量位置、协方差和权重来自场景先验 w_pred [w_pred, w_birth]; m_pred [m_pred, m_birth]; P_pred [P_pred, P_birth];这段代码的逻辑是先把每个旧分量的均值做一次状态转移协方差加上过程噪声权重乘上存活概率然后把新生分量直接拼接在数组后面。需要注意w_birth的总权重决定了每帧最多出现的新目标期望数这个和实际场景的新目标出现率要对齐否则滤波器会系统性低估或高估目标数。GM-PHD对“新目标从哪来”非常敏感位置放错会导致目标出现在错误区域。2.2.2 更新步漏检、量测似然与权重修正更新步是GM-PHD区别于普通高斯滤波器的核心。漏检情形对应“没有量测来源于该目标”的假设其强度修正仅需乘上漏检概率1 - pD。而每个量测 z 都会对所有预测分量产生一个“修正副本”副本的权重正比于 pD、量测似然和杂波密度的比值。也就是说一个量测来了每个高斯分量都试图“认领”它但最终的权重会自然调节——距离近、似然高的分量权重被放大远的分量权重趋近于0。% 对每个量测 z_j计算所有预测分量的新副本 for j 1:num_measurements for i 1:num_pred_components q gauss_likelihood(z_j, m_pred{i}, P_pred{i}); % 量测似然 w_upd(i,j) pD * q / (lambda_c * c(z_j)) * w_pred(i); m_upd{i,j} m_pred{i} K * (z_j - H * m_pred{i}); P_upd{i,j} (eye(size(P_pred{i})) - K * H) * P_pred{i}; end end这里的核心参数是杂波率lambda_c和杂波空间分布c(z)。杂波率越高分母越大量测对权重的贡献被稀释得越厉害。实际调参时杂波率偏低会导致虚假目标增多偏高则会降低检测灵敏度。卡尔曼增益K的计算不再单独给出因为它和单目标卡尔曼滤波没有区别。每条量测都要为每个预测分量生成一个副本所以这一阶段的计算量是“预测分量数 × 量测数”是源码里最吃CPU的部分。2.2.3 剪枝、合并与状态提取更新后的分量数量等于预测分量数与量测数的乘积如果不加控制几帧之后就会爆炸。这里就需要两条规则剪枝是扔掉权重低于阈值的分量合并是把距离很近的分量合成一个。权重的绝对值在GM-PHD里的意义是“该分量对应的目标数期望”小于T_prune的分量丢掉后整体目标数期望只损失很小。% 剪枝 keep w_upd_all T_prune; w_keep w_upd_all(keep); m_keep m_upd_all(keep); P_keep P_upd_all(keep); % 合并按权重降序贪心合并 while ~isempty(idx_remain) [~, i_star] max(w_keep(idx_remain)); merge_set find(Mahalanobis(m_keep(idx_remain), m_keep(i_star)) U_merge); w_new(end1) sum(w_keep(merge_set)); m_new(end1) (1/w_new(end)) * sum(w_keep(merge_set) .* m_keep(merge_set)); end合并门限U_merge决定多少个分量合并成一个它的设置直接影响两个相近目标能否被区分。马氏距离门槛越小分量越不容易合并目标分辨率高但计算量也大门槛越大相邻目标容易被并成一个表现为目标数估计偏低。状态提取则更简单把所有权重之和作为目标数估计 N取权重最大的 N 个分量均值作为目标位置。注意这里提取的是“位置”而不是“航迹”GM-PHD本身不包含航迹管理若要输出连续航迹还需要在提取后进行数据关联或标签传递。3. 扩展目标场景下的ET-GM-PHD量测是“一簇”而非“一个”3.1 点目标模型为什么不适用量测簇、泊松率与量测划分标准GM-PHD的前提是“每个目标每个时刻至多产生一个量测且 pD 1”。但雷达对大型目标、激光雷达对行人或车辆、声呐对艇体这样的场景中一个目标会反射多个量测。这些量测在空间上形成一簇且簇内量测数量本身是随机的通常建模为均值为 γ 的泊松分布。如果仍然用点目标的量测似然会出现一个严重问题一个目标产生的多个量测会被当成多个目标滤波器会把一个簇当成一群目标来估计目标数瞬间膨胀。因此扩展目标PHDET-PHD的更新公式和点目标有本质区别。点目标更新是“每个量测独立修正强度”扩展目标更新是“先对量测集做划分再对每个划分单元计算乘积似然”。量测划分是一次性的前置步骤将所有量测划分成若干个子集cell每个cell要么认为来自某个目标要么认为是杂波。划分方式直接影响后续所有计算划分多了候选假设多但算力爆炸划分少了会漏掉量测簇之间的区分。常见划分方法包括距离阈值划分、K-means聚类、基于期望最大化EM的划分源码里最常出现的是距离阈值因为它最快也最容易理解。3.2 扩展目标的更新似然与量测划分算法ET-GM-PHD的更新步中量测集 Z 的似然是对所有可能划分 p 求和。每个划分 p 中每个 cell W 都有两种来源杂波或者某个目标。若来源于目标则该cell内的量测是泊松簇簇的似然等于目标和量测集之间的乘积形式。这个求和项在工程上会截断只保留有限数量的划分最常见的是只保留量测数较小时的穷举划分量测数大时使用聚类近似。function [partition] partition_measurements(Z, d_threshold) N size(Z, 2); partition {}; visited false(1, N); for i 1:N if visited(i), continue; end cell [i]; visited(i) true; changed true; while changed changed false; for j 1:N if visited(j), continue; end % 如果量测j与cell中任一量测距离小于阈值则归入同一个cell if min(vecnorm(Z(:,j) - Z(:,cell), 2, 1)) d_threshold cell [cell, j]; visited(j) true; changed true; end end end partition{end1} cell; end end这个贪心划分的复杂度是O(N²)量测数超过几十个后仍然很快但它的质量对阈值d_threshold很敏感。阈值太小同一目标的量测被拆成多个cell目标数会被高估阈值太大不同目标的量测被并进同一个cell目标数被低估。实际操作中这个阈值要结合目标尺寸和传感器分辨率来设定——激光雷达场景可以粗略取目标半径的1.5倍雷达场景取距离分辨率的2倍然后做一次离线仿真扫描确定。3.3 MATLAB源码中两处关键判断分簇与权重收敛在MATLAB源码中扩展目标模块通常要回答两个判断。第一个是“当前量测是否属于同一个扩展目标”——这个逻辑已经体现在划分函数里。第二个是“更新后权重收敛到什么程度”——因为扩展目标一个目标产生γ个量测更新后该目标的强度峰值权重大约是pD * γ / (lambda_c * c(z))如果γ固定这个值可以反推pD或者杂波率是否设置合理。还有一个工程细节扩展目标更新中漏检项的处理与点目标不同。点目标的漏检贡献是(1-pD) * D_pred(x)扩展目标的漏检项虽然形式相同但pD表示“目标至少产生一个量测且被检测到”的概率。如果目标的反射点数量有时为0需要额外建模检测概率与“产生量测簇”这一事件的耦合关系。这部分在MATLAB源码里通常体现为一个参数pD_ext它一般比点目标pD略低因为这还隐含了“产生非空量测簇”的概率。调参时不区分这两个pD的含义是源码跑不出稳定目标数的最常见原因。4. 打开GM-PHD编写的MATLAB源码目录、参数表与第一次运行4.1 源码目录如何读demo、滤波器主体与绘图一份典型的GM-PHD MATLAB源码目录结构通常分成三层。第一层是init或demo脚本负责参数初始化和场景搭建第二层是滤波器主体即gmphd_filter.m、et_gmphd_filter.m这类核心函数第三层是辅助函数包括量测划分、高斯似然计算、剪枝合并、绘图脚本。读源码不要从头到尾读顺序应该是先跑demo再读demo里调用到的滤波器函数最后看辅助函数。% 伪代码从demo脚本中抽取的调用关系 % 初始化参数 params initialize_parameters(); % 循环仿真 for t 1:params.T_total Z{t} generate_measurements(params.X_true{t}, params); [X_est, N_est] gmphd_filter(Z{t}, params, X_prev); plot_result(X_est, N_est); end常见误用是直接改滤波器主函数里的内部变量却不动初始化脚本。GM-PHD的参数几乎全部集中在初始化脚本中主函数的变量只是参数的搬运工。找到initialize_parameters.m就找到了这个源码的“控制面板”。如果你的源码没有独立的初始化脚本那么在直接调用滤波器的脚本里一定有一大段画着% 参数设置分隔线的代码这就是要对齐的部分。4.2 一组够用的参数初始化从pD到合并门限下面是经多个场景验证过的一组起点参数适合“二维雷达监视 扩展目标”的默认场景。实际使用以这组为基准调整不要一次改多个参数。参数名典型值含义与调整策略pS0.99存活概率。目标频繁消失时降到0.95pD0.9检测概率。影响漏检项的权重大小lambda_c5e-6相对量测空间杂波率。按杂波强度先粗调数量级gamma10扩展目标量测泊松率。均值即每个目标每帧的量测数T_prune1e-5剪枝阈值。过低会导致分量爆炸U_merge4合并门限马氏距离。目标间距小时调小到2MAX_N100分量数量上限。防爆保护一般够用w_birth_sum0.01每帧新生目标强度总权重。目标出现频繁时调大初始化代码常见做法是params.pS 0.99; params.pD 0.9; params.lambda_c 5e-6; params.gamma 10; params.T_prune 1e-5; params.U_merge 4; params.MAX_N 100; params.w_birth [0.01, 0.01]; % 两个新生热点 params.m_birth {[x0; y0; 0; 0], [x1; y1; 0; 0]};泊松率gamma和杂波率lambda_c这两项的比值决定一个量测簇能被当作目标还是被当作杂波丢弃。如果gamma太小比如等于2和杂波簇几乎无法区分此时扩展目标模型退化为点目标模型还不如直接用标准GM-PHD。工程上gamma小于量测空间维度1时扩展目标的优势基本消失。所以要么传感器确实能产生足够的每目标量测数要么就不该用扩展目标模型。4.3 运行后看什么权重、目标数和位置的日志验证第一次运行源码后不要只看最终轨迹图。轨迹图会掩盖很多中间过程的问题最典型的错误是“跟踪结果看起来不错但目标数估计忽高忽低”。正确的验证手段是把每帧的目标数估计、权重总和、最大权重分量、量测数量记录下来逐帧对齐来看。% 记录每帧日志 log.N_estimated(t) round(sum(weights_after_prune)); log.W_total(t) sum(weights_after_prune); log.N_measurements(t) size(Z{t}, 2); log.max_weight(t) max(weights_after_prune);log.W_total是PHD积分它是对“全场景目标数期望”的估计可以和log.N_measurements对比看合理性。如果量测数30个而W_total只有1.5说明杂波率设高了或者大量分量的权重被过度压低如果W_total超过量测数的一半需要检查是不是一个目标被拆成了多个分量。另外要注意绘制估计位置时代码直接取权重最大的前N个分量但如果两个分量都来自同一个目标这个方法会画出重复轨迹。要发现这种问题看轨迹图是没用的必须结合log.W_total和log.N_estimated一起判断。5. 三个容易翻车的点与对应的调参验证技巧5.1 目标数过估杂波率与漏检项的平衡最常碰到的现象是目标数估计持续偏高每隔几帧就冒出虚假目标。通常的归因是“杂波太多”但改小lambda_c往往没有用反而更糟。真正的原因是漏检项权重过高。GM-PHD更新时漏检项继承了预测强度的大部分权重如果pD设置得太低比如0.7那么每个量测对权重的修正力度都不够目标分量在漏检帧后权重大幅下降而杂波分量只要有一次量测接近就会获得相对很高的权重虚假峰值随之出现。验证方法是查看log.max_weight与阈值T_prune的比值——如果大量分量权重刚刚高于剪枝阈值但从未真正收敛就说明漏检项主导了权重分布。此时应当提高pD同时略微降低w_birth_sum严格控制新生分量的“影响力”。5.2 目标靠近时的“合并不当”先调门限还是先调协方差两个目标相距较近时滤波器的估计常常变成一个目标这不是合并阈值的问题而是预测协方差传递的误差导致两个分量在马氏距离上不可分。先看P_pred中的位置协方差对角线如果它远大于两目标间距的平方那么任何合并阈值都会把它们并在一起。这时正确的做法是先降低过程噪声Q中位置项让预测协方差不要膨胀过快再调U_merge。相反如果协方差正常但目标仍然被并掉才考虑把U_merge从4降到2或者更小。调参要有这样的顺序观念先确认“几何上可分”再确认“算法上没合并”。5.3 用OSPA距离替代RMSE验证整体滤波性能单目标跟踪用RMSE没问题多目标跟踪再用RMSE会误导因为目标数估计错误在RMSE中没有体现。一个常用的替代是OSPA距离最优子模式分配它把位置误差和目标数误差融合成一个标量。MATLAB里不依赖额外工具箱的话实现OSPA并不复杂function [ospa_dist] ospa_metric(X_est, X_true, c, p) % c为截断距离p为范数阶数 m size(X_est,2); n size(X_true,2); if m 0 n 0, ospa_dist 0; return; end if m 0 || n 0 ospa_dist c; return; % 目标数完全不对时按最大惩罚计 end D pdist2(X_est(1:2,:), X_true(1:2,:)); % 仅用位置维计算 D min(D, c); % 超过c的距离截断 [assignment, cost] munkres(D); % 匈牙利算法求解最优匹配 cardinality_error (c^p * abs(m - n)) / max(m, n); ospa_dist (1/max(m,n) * (sum(cost.^p) cardinality_error))^(1/p); endOSPA的调参逻辑和RMSE完全不同c设为目标间距的2倍左右较为合理p取1或2即可。当OSPA下降但目标数误差依然偏大时问题大概率在新生强度位置或pD设置上当OSPA平稳但有固定偏置时问题大概率在量测划分的d_threshold上。把OSPA曲线和每一帧的目标数估计画在同一张图里你会发现大部分调参都能从曲线形态上直接判断——这是纯看轨迹图替代不了的验证手段。本文还有配套的精品资源点击获取