扩展目标PHD滤波的线性高斯混合实现：从点迹关联到随机有限集

做目标跟踪的人，多数都遇到过这个场景：激光雷达扫过行人，一个目标回七八个点；雷达分辨率高一点，一辆车也能出好几个散射中心。以前我用JPDA和MHT做数据关联，目标一多，联合事件和假设树疯长，纯粹是组合爆炸。后来换到标题里这套“扩展多目标跟踪概率假设密度滤波（PHD）线性高斯混合实现”，最大的感受就是：不用再显式维护“哪个量测对应哪个目标”的关联假设，而是把整个多目标状态当作随机有限集，只去递推一阶统计矩——强度函数，在Matlab里用一堆高斯分量把它表示出来。这个方案对做雷达/激光雷达多目标跟踪、写毕业设计、或者手里有一堆点云量测但不知道怎么建模的人来说，是一个很值得跑的起点。

先把这个名字拆开：“扩展”说的是扩展目标（extended target），也就是目标本身不是一个点，而是一个有尺寸、会在一帧里产生多个量测的对象，不是“把算法扩展一下”的意思。很多人第一眼看到标题会误会成“一个扩展版本的多目标跟踪算法”，实际上它描述的是目标模型从“点目标”变成“扩展目标”后，PHD滤波的一种线性高斯混合实现。理解了这一层，后面读代码会顺畅很多。

1. 为什么说这个滤波是“跳过数据关联”的多目标跟踪方法

1.1 点目标与扩展目标：最大的差别在于一个目标到底回多少量测

传统点目标跟踪，默认每个目标每帧最多产生一个量测，检测概率低了还可能漏掉。扩展目标模型不同，一个目标每帧可能产生好几个量测，量测数本身是随机的。雷达和激光雷达场景最明显：一辆车在点云里扫出来几十个点，一个行人也可能是五六个回波。还有视频检测里，一个目标对应多个检测框或者多个人体关键点，也可以理解成扩展量测。

这个区别看似简单，实际上是算法选型的核心分水岭。点目标那一套“一个量测只更新一个目标”的关联逻辑，在扩展目标场景下很难直接套用，因为同一个目标的多个量测之间也存在“组内关联”，你不知道哪些量测属于同一目标，也不知道这一组量测到底是哪个目标产生的。换句话说，点目标跟踪的难点是“量测-目标”匹配，扩展目标跟踪的难点是“量测集合-目标”匹配，困难程度完全不是一个量级。

1.2 传统数据关联方法为什么在扩展目标场景下很难受

JPDA和MHT的复杂度来源是关联假设的数量。点目标场景下，目标数和量测数的笛卡尔积还可以靠门限和剪枝硬扛；扩展目标场景里每个目标可能对应多个量测，这个组合数瞬间上来了。而且目标数量本身还在变化——新目标进入、旧目标离开，漏检、虚警一大堆。

JPDA计算联合概率时要枚举所有可行关联事件，MHT要把关联假设树一直往下挂，扩展到“一群量测对一群目标”的时候，工程实现基本不可控。我自己试过用JPDA处理一个目标产生三四个量测的情况，关联矩阵的维度已经不是N×M的问题，而是N个目标和一堆量测子集之间的排列组合，跑几帧就卡住。

PHD滤波绕开这个问题的方式很直接：不关心“具体哪个量测来自哪个目标”，而是描述“某个区域里大概有多少目标、目标的大致状态分布是怎样的”，用积分把目标个数期望算出来。这是一种思想上的切换，从离散关联问题变成连续密度递推问题。

1.3 为什么选择线性高斯混合形式

PHD滤波在一般情况下没有闭式解，因为强度函数的递推涉及多维积分。如果运动模型和量测模型都是线性高斯的，目标新生、存活、衍生的强度都用高斯分量线性组合来表示，那么预测和更新都能解析计算，这就是高斯混合PHD（GM-PHD）。对扩展目标也有对应版本，叫扩展目标高斯混合PHD，不少论文里简写为ET-GM-PHD。标题里“线性高斯混合实现”说的就是这个路线。

它最大的优势是工程友好。你不需要在Matlab里做粒子采样那套重采样、权值退化处理，而是维护一组均值和协方差，跑起来像一堆卡尔曼滤波器在并行工作。缺点也很明显：模型假设必须符合线性高斯约束，目标机动、强非线性量测场景需要额外处理。不过对大多数雷达、激光雷达二维跟踪场景，这个假设在短时间内完全够用。

2. 强度函数与随机有限集：这套理论里两个必须理解的概念

2.1 随机有限集：目标集合不是固定数组

我们习惯的状态向量是一个固定长度的数组，但多目标跟踪里目标数都不知道，你没法把所有目标状态拼成一个大向量去滤波，因为向量维度在变。随机有限集就是指“以集合为值的随机变量”，集合的元素个数和目标状态都是随机的。

你可以把它理解成一筐苹果：苹果数量不确定，每个苹果的重量也不确定，我们要估计的是这筐苹果有多少个、这些苹果大致多重、分布在哪些位置。目标集合就是一个随机有限集，相邻时刻的集合之间也有演化规律：有的苹果被拿走（目标消失），有的新苹果放进来（新生目标），还可能出现一筐变两筐（衍生目标）。

在公式里，一个多目标状态就是一堆单目标状态的集合，比如X = {x1, x2, x3}，元素个数可以是0、1、2……任何非负整数。这个集合里元素顺序是不重要的，所以{x1, x2}和{x2, x1}是同一个集合。

2.2 PHD强度函数：多目标后验的一阶矩

直接递推多目标后验密度在理论上是最优的，但计算量是组合级的，工程上几乎不可能。Mahler提出用强度函数（也叫概率假设密度，PHD）来近似，它是一阶统计矩。

强度函数最直观的性质是：在任意空间区域上的积分，等于该区域内目标数量的期望。如果一个区域强度值高，说明那里更可能有目标。PHD滤波器每一帧递推这个强度函数，预测步处理目标的运动、新生、消亡，更新步利用量测对强度函数做修正。

这样多目标状态估计就退化成两个问题：对强度函数积分近似得到目标个数，找强度函数的局部峰值得到目标状态。因为期望值不一定是整数，比如权重总和是2.3，说明这个区域大概有2到3个目标，通常四舍五入取整就作为当前目标数估计。

2.3 高斯混合表示：用一堆“小高斯”拼出强度函数

强度函数本身是任意正函数，直接存成网格在二维三维场景里不现实。线性高斯条件下，可以用一组加权高斯分量来近似：每个分量就是高斯分布的均值m、协方差P、权重w。多个分量线性叠加后，比如两个目标靠近时，它们各自的波形会重叠，重叠区域强度更高，这正是多个目标靠近时我们希望看到的效应。

这和用砖块拼墙很像，每块砖是一个高斯分量，砖多到一定程度，墙的形状就出来了。更新之后，每个量测单元都可能产生新分量，所以分量数量会快速增长，必须有修剪和合并机制。这就是后面Matlab代码里最核心的循环逻辑，理解了“分量是砖、墙是强度函数”这个比喻，看代码时就不会迷路。

3. 扩展目标GM-PHD的递推拆解：预测、更新、修剪合并

3.1 扩展目标量测模型先搞清楚

扩展目标与点目标最大的模型差异在量测生成过程。点目标模型：目标以检测概率pD产生一个量测，量测噪声叠加。扩展目标模型：目标每一个采样周期产生一个泊松随机数目的量测，平均量测数记为gamma(x)，可能随目标状态变化，量测位置在目标真实位置附近按量测噪声分布散开，并叠加传感器噪声。

另外还要考虑杂波，也就是虚警，杂波强度通常用kappa(z)表示，一般假设服从泊松分布且均匀分布在测量空间。在Matlab里写模拟数据时，我习惯先随机生成目标轨迹，再对每个目标每帧生成N个量测，N从泊松分布采样，最后再叠加均匀分布的杂波点。这样做出的仿真数据才能验证滤波器是不是真的能分离目标和杂波。

3.2 预测步：存活、新生、衍生

预测步相比点目标GM-PHD变化不大。存活分量按运动模型线性高斯转移：均值乘状态转移矩阵F，协方差变成F * P * F' + Q，权重乘存活概率pS。新生目标强度由设计者给定，通常是在场景边缘或者已知入口处布置几组高斯分量，权重很小，比如0.03左右。

衍生目标（一个目标分裂成两个）在大部分场景用不上，比如车辆跟踪、行人跟踪里极少出现一个目标在物理上分裂成两个的情况，直接省略能省不少事。预测完成后的强度就是存活分量、新生分量（和可选衍生分量）的线性叠加。

需要提醒的是，预测后权重总和并不是不变的。存活概率乘以权重会让总权重减少，新生分量会增加总权重。如果设置的存活概率太低、新生分量又太多，滤波器会每帧“凭空”产生大量目标，目标数估计很容易偏大。

3.3 更新步：量测划分是核心

更新步是扩展目标PHD和点目标PHD最大的分水岭。点目标GM-PHD的更新，是每个量测独立去更新每一个分量，因为一个量测最多只能来自一个目标，量测之间不需要分组。扩展目标PHD需要先把当前帧全部量测划分成一个一个的子集W，每个子集代表“可能由一个目标产生的量测集合”，杂波点就是单点单元。

常见做法是距离阈值聚类：两个量测之间距离小于阈值就认为是同一个目标的量测，然后找出所有连通分量。这个划分是影响滤波质量最关键的一步，划分半径太小会把同一个目标的量测拆成多个目标，太大又会让不同目标合并。

我见过很多第一次跑ET-GM-PHD的人，把绝大部分精力花在研究更新公式上，结果滤波效果不好其实是量测划分这一步出了问题。建议先把划分结果画出来，一个颜色一个子集，肉眼看一眼就知道划分是否合理。

3.4 更新步的公式本质：漏检加每个单元都要更新

更新完成后，强度函数由两部分组成：一个是漏检分量，权重乘1-pD，目标状态不变；另一部分对每个量测单元W，每个预测分量j都会生成一个新分量。

权重的更新比例可以理解为：单元W在分量j假设下的似然，除以所有预测分量和杂波对这个单元的似然总和，再乘预测权重。单元似然的计算是量测单元W的联合似然：如果单元W里有n个量测，这些量测都被目标j产生的概率包括两部分，一是目标产生n个量测的泊松概率（平均量测数gamma的n次幂乘e^{-gamma}），二是每个量测相对目标预测量测的高斯密度乘积。

分母还要加上杂波对这个单元的强度。这个分母就是归一化因子，保证各种假设的概率加起来是1。均值协方差更新类似卡尔曼滤波的“批量量测更新”，用这个单元里的所有量测一起去修正高斯分量的均值和协方差。写代码时，每个单元和每个分量的组合都需要计算一遍，所以复杂度是“划分单元数乘以分量数”。

写代码时特别要注意数值下溢。多个高斯密度的乘积会非常小，连乘几次就可能变成0，更新后所有分量权重全部变成0，滤波直接崩掉。我的做法是在log域里算完再exp，或者对最终权重做一次归一化再参与下一步递推。

3.5 修剪合并与状态提取

更新之后分量数量爆炸式增长，必须做修剪和合并。修剪就是去掉权重低于阈值的分量，这是最暴力的降维手段。合并是把距离很近的分量融合成一个，距离一般用马氏距离或者欧氏距离，阈值太大会把不同目标合并成一个，太小则分量数降不下来。

状态提取时，把权重大于提取阈值（比如0.5）的分量拿出来，均值里的位置部分就是目标估计位置，权重的求和取整就是目标数。这里有个扩展目标特有的trick：因为一个目标产生多个量测，同一个目标的权重会被多个单元贡献，理论上一个目标只有一个分量，如果出现多个分量重叠在同一位置，说明更新和合并没调好。

4. Matlab代码的组织结构与核心函数写法

4.1 用struct表示高斯分量的集合

在Matlab里，最高效直观的做法是用一个struct数组保存所有高斯分量。我习惯这样定义：

matlab复制components = struct('w', {}, 'm', {}, 'P', {});

每个元素有三个字段：w是标量权重，m是状态向量（比如4x1），P是协方差矩阵（4x4）。分量遍历可以直接for i = 1:length(components)，插入新分量用components(end+1) = struct('w', wNew, 'm', mNew, 'P', PNew);。

虽然Matlab循环慢，但架不住直观，先把逻辑跑通再说性能。实测下来，几十上百个分量时，这个结构跑起来完全没问题，每秒几百帧轻轻松松；到上千个分量才需要考虑向量化整批计算。对应到场景里就是十几个目标，每帧更新后即使每个目标产生好几个分量，总体也远达不到这个量级。

4.2 预测函数的写法

预测分两部分：存活分量更新和新生分量添加。存活分量逐个乘F矩阵，协方差用F * P * F' + Q更新。新生分量可以直接在预测阶段末尾追加，也可以在初始化时指定。核心代码大概这样：

matlab复制function comp = predict_components(comp, F, Q, pS)
    for i = 1:length(comp)
        comp(i).w = comp(i).w * pS;
        comp(i).m = F * comp(i).m;
        comp(i).P = F * comp(i).P * F' + Q;
    end
    % 这里追加新生分量，一般是场景边缘的若干高斯
    % comp(end+1) = struct('w', 0.03, 'm', [...], 'P', diag([...]));
end

这里要注意：如果存活概率pS小于1，预测后所有分量的权重都要缩水；新生分量权重如果设得和存活分量一个量级，可能导致目标数估计偏高。一般新生权重0.02到0.05比较常见。

4.3 量测划分：用连通分量做分组

量测划分的思路是：对每个量