CN109886305A - 一种基于gm-phd滤波的多传感器非顺序量测异步融合方法 - Google Patents

Abstract

本发明公开了一种基于GM‑PHD滤波的多传感器非顺序量测异步融合方法，可用于解决杂波环境下基于雷达、声呐等多传感器异步非顺序量测的多目标跟踪问题。本发明采取了集中式特征级融合策略，融合中心对实时接收到的量测进行判断，针对顺序量测和到达滞后量测两种异步量测分别设计了基于GM‑PHD滤波器的融合算法。特别的对于到达滞后量测，本发明合理改进了GM‑PHD滤波器，解决了随机集框架下的反向状态预测及负时间量测更新问题，实现了目标状态的二次估计。本发明借助随机集理论的优势，避免了异步融合问题中复杂的数据关联问题，方法结构简单，易于实现的迭代更新，对于解决实际的多传感器多目标目标跟踪问题具有重要的现实意义。

Description

一种基于GM-PHD滤波的多传感器非顺序量测异步融合方法

技术领域

本发明属于多目标跟踪及多传感器融合领域，涉及一种多传感器非顺序量测的异步融合方法，可用于解决杂波环境下基于雷达、声呐等多传感器异步量测的多目标跟踪问题。

背景技术

多传感器多目标跟踪技术在军事和民用方面有着广泛的应用，如空中预警，海洋监测，自动驾驶等。随着传感器种类的不断丰富和精度的有效提高，包含数据融合的多目标跟踪方法也在逐步向前发展。

针对多目标跟踪问题，经典解决方法集中在解决数据关联问题上，主要有最近邻算法(Nearest Neighbor，NN)、全局最近邻算法(Global Nearest Neighbor，GNN)、联合概率数据关联(Joint Probability Data Association，JPDA)，多假设跟踪(MultipleHypothesis Tracking，MHT)等。然而，最近邻算法受杂波影响较大，标准JPDA的计算复杂度为NP-hard，MHT也存在组合爆炸问题。近年来，基于随机有限集理论(Random Finite Set，RFS)的目标跟踪方法不断引起学术界的广泛关注，相对于传统的多目标跟踪方法，该方法不需要进行数据关联，多目标的状态和量测都以随机集的形式出现，目标的出现、消失、传感器量测受到环境噪声的影响统一用随机集的形式给出。R.Mahler先后提出概率假设密度(Probability Hypothesis Density，PHD)滤波器，带有势分布的PHD滤波器(CardinalizedPHD，CPHD)，多目标多伯努利(Multi-target Multi-Bernoulli，MeMBer)滤波器，逐步将随机有限集理论应用于目标跟踪领域。B.-N.Vo给出了PHD、CPHD、MeMBer等滤波器的两种收敛实现，即序贯蒙特卡洛(SequentialMonteCarlo，SMC)和高斯混合(Gaussian mixture，GM)方法。特别是GM-PHD滤波器，使得基于RFS的多目标跟踪方法在工程应用方面成为可能。已有研究表明，GM-CPHD的计算量相比JPDAF的多目标跟踪算法计算复杂度要小的多，并且能够得到更好的多目标跟踪结果。

在多传感器融合理论中，研究更多的是同步融合，即各传感器同步测量，且同步传输数据到融合中心。而实际中由于各传感器采样频率不同以及通信滞后等问题，到达融合中心的数据是异步的。异步量测主要分为顺序量测(in-sequence measurement,ISM)和非顺序量测(out-of-sequence measurement,OOSM)，两者的差别主要在于量测到达融合中心的顺序与其时间戳的顺序是否一致，如图1所示。针对顺序量测异步融合问题，只需进行序贯滤波即可，而非顺序量测的异步融合则较为复杂。针对一步滞后非顺序量测问题，有学者先后提出基于向后状态倒推的最优A1算法、次优的B1、C1算法，这些算法各有优劣，在不同场景下能达到有效估计。上述算法主要针对单目标下的异步信息融合，由于涉及数据关联问题，多目标异步信息融合时将更为复杂，目前关于多传感器多目标异步融合问题的研究较少，且尚未有在随机集框架下非顺序量测异步融合的应用研究。

发明内容

本发明针对多传感器非顺序量测(OOSM)异步融合问题，研究设计了一种基于高斯混合概率假设密度滤波器(GM-PHD)的多传感器融合方法。方法借助随机集理论的优势，避免了异步融合问题中较为棘手的数据关联问题。本发明的技术方案为：

首先，建立多传感器多目标跟踪场景及基础模型，包括目标运动模型，量测模型、杂波模型等，并初始化。

其次，建立多传感器非顺序量测异步融合框架。该框架对异步量测进行集中式特征级融合。假设有s个同质传感器对目标进行探测，并将量测传给融合中心。融合中心每接到一个传感器的量测，便将量测时刻与当前最新估计结果时刻进行对比，当量测为顺序量测时，直接应用高斯混合PHD滤波器进行剪枝合并、预测、更新，得到新的估计结果；当量测为到达滞后量测时，应用基于直接更新法的高斯混合PHD滤波器，先进行反向状态预测，然后借助滞后量测对当前估计结果二次滤波。

最后，在需要获得目标状态估计的时刻，融合中心对当前最新估计结果进行目标个数及状态提取并输出。

本发明的创新点在于：

提出了基于高斯混合概率假设密度滤波器(GM-PHD)的多传感器异步融合方法，能对顺序量测和到达滞后量测两种异步数据进行有效处理，得到最新时刻多目标状态的估计值；对GM-PHD进行合理改进，以用于反向状态预测及非同步量测的状态直接更新问题，实现了非顺序量测下的目标状态估计；借助随机集理论的优势，避免了异步融合问题中复杂的数据关联问题。方法结构简单，易于实现的迭代更新，并保证了目标跟踪误差的收敛性。

附图说明

图1为异步非顺序量测示意图。

图2为本发明所提出的基于GM-PHD滤波的多传感器非顺序量测异步融合方法的流程图。

图3为仿真实验中三种不同测量情形。

图4为一次蒙特卡洛仿真中目标的真实轨迹及测量值。

图5为三种场景下100次蒙特卡洛仿真的跟踪估计结果。

图6为三种情况下100次蒙特卡洛仿真跟踪结果的平均Hausdoff距离和平均Wasserstein距离对比图。

图7为三种情况下100次蒙特卡洛仿真目标数目估计平均值。

具体实施方式

下面结合附图对本发明的实施进行详细的说明，并给出具体的操作方式以及实施步骤。

步骤一：建立多传感器多目标跟踪模型，并初始化

在随机有限集框架下，多目标的状态和量测均可视为随机有限集，假设t_k时刻有N(k)个状态和M(k)个量测，多目标的状态和量测可以表示为有限集形式：

X_k＝{x_k，1，x_k，2，...，x_k，N(k)} (1)

Z_k＝{z_k，1，z_k，2，...，z_k，N(k)} (2)

其中，x_k，1，x_k，2，...，x_k，N(k)分别为第1、2、...、N(k)个目标状态，z_k，1，z_k，2，...，z_k，N(k)分别为第1、2、...、M(k)个量测。

对于单个目标来说，运动模型为：

x_k＝F(k，k-1)x_k-1+v(k，k-1) (3)

其中，为目标状态变量，其中的元素分别代表x方向的位置，x方向的速度，y方向的位置，y方向的速度。F(k，k-1)是目标从t_k-1时刻到k时刻的状态转移矩阵，v(k，k-1)是从k-1时刻到t_k-1时刻的状态误差累积过程，服从N(0，Q(k，k-1))的分布。

对于单个量测来说，量测模型为：

z_k＝H(k)x_k+w(k) (4)

其中，z_k为t_k-1时刻的一个量测，H(k)为t_k-1时刻量测函数，w(k)是分布为N(0，R(k))的量测噪声。

对于t_k-1时刻多目标状态X_k-1，每个目标的状态x_k-1∈X_k-1或者以p_s，k+1的概率继续存活，变成新的状态x_k，或者以1-p_s，k+1的概率消失。所以对于给定的X_k-1，t_k时刻的多目标状态X_k由继续生存的目标S_k|k-1(x)、和新生的目标Γ_k构成：

量测方面，对于给定目标或者被检测到，或者漏检，故每个目标x将产生一个随机有限集量测Θ_k(x)。除了源于目标的量测，传感器也可能会接收到杂波或虚警量测集K_k。所以多目标的量测Z_k由真实量测和杂波构成，即：

在高斯混合概率密度假设下，起始时刻目标表示为高斯混合的形式：

式中x为目标状态空间的任意值，v₀(x)为起始时刻目标状态x的概率假设密度函数(或强度函数)，J₀表示起始时刻目标的高斯分量个数，分别为第i个高斯分量的权重、均值和协方差，根据跟踪场景进行选取。

杂波模型是强度为κ_k(z)的泊松有限集，即

κ_k(z)＝λ_cVu(z) (8)

其中，u(z)表示监视区域的均匀概率分布，λ_c为杂波强度，V为传感器监视区域的面积。

步骤二：建立多传感器非顺序量测异步融合框架

如图2，本发明设计了一种多传感器非顺序量测异步融合框架，该框架对异步量测进行集中式特征级融合。假设有s个同质传感器对目标进行探测，所有传感器采样频率相同，融合中心以相同的频率输出目标状态的估计结果。导致传感器量测异步的原因主要有两个：各传感器探测时间未同步；由于信号传递时延不同，即便相同时间戳的量测到达融合中心的时间也不同。假设融合中心依次接收到来自不同传感器的量测，相应的测量时刻(时间戳)分别为：t₁，t₂，...，t_k-1，t_k，...，t_s(其中下标代表接收顺序)，融合中心当前最新估计值对应的时刻为t_k，将其作为下一帧量测的比较基准，用T_k表示(T_k＝t_k)，每接收到一个传感器的量测集合，融合中心依据其时间戳判断该量测集合属于顺序量测或到达滞后量测，然后采取相应的融合策略。即针对到达融合中心的量测的真实时刻t_k+1，通过与当前最新估计时刻t_k比较分为两种情况，顺序量测(t_k+1＞T_k)，进行下述的剪枝合并策略，并执行步骤三；到达滞后量测(t_k+1＜T_k)，不进行剪枝合并，直接执行步骤四。

(1)剪枝合并

假设t_k时刻目标的后验强度可以表示为高斯混合的形式：

在高斯混合PHD滤波过程中，随着时间的推移，高斯分量的不断增加，因此需要对权重较小的高斯分量进行减枝；另外，一些高斯分量非常接近，需要合并为一个高斯分量。通常剪枝合并步骤在滤波器预测更新之后，但是当下一帧量测为滞后量测时，需用到剪枝合并前的相关信息，故将剪枝合并放在第二步，即确定量测为顺序量测时进行，到达滞后量测时不进行。设T_w为减枝阈值，U为合并阈值，J_max为最大允许的高斯项数目，k表示剪枝合并的时刻。

令l＝0，

循环：

直到循环终止。

如果l＞J_max，则用J_max个权重最大的高斯分量代替

步骤三：顺序量测(t_k+1＞T_k)，依照GM-PHDF处理

(1)预测

假设t_k时刻目标的后验强度可以表示为高斯混合的形式：

式中x为目标状态空间的任意值，J_k表示t_k时刻目标后验强度的高斯分量个数，分别为第i个高斯分量的权重、均值和协方差。t_k+1时刻目标状态集合可表示为源于t_k时刻的存活目标S_k+1|k(x)和当前时刻的新生目标Γ_k+1，

即：

所以，t_k+1时刻目标的先验强度可以表示为：

v_k+1|k(x)＝v_s，k+1|k(x)+γ_k+1(x) (12)

其中，γ_k+1(x)为新生目标的强度，此处假设为高斯混合形式，即：

v_s，k+1|k(x)为存活部分的先验高斯分布，可表示为：

其中，p_s，k+1为目标存活概率，分别为j个高斯分量的权重、均值和协方差，类似卡尔曼滤波中的预测步骤，有

(2)更新

如(1)中所述，t_k+1时刻目标的先验强度可以统一表示为高斯混合形式：

其中，J_k+1|k＝J_k+J_γ，k+1表示t_k+1时刻目标先验强度的高斯分量个数，分别为i个高斯分量的先验权重、均值和协方差。

得到t_k+1时刻多目标PHD的预测后，由下式对PHD进行更新：

式中等号右面第一项为未被检测到的目标，直接用先验高斯分量代替，P_D，k+1表示目标的检测概率，第二项代表对于每一个量测z∈Z_k+1，都有对应的更新高斯分量，即：

式中对单个高斯分量使用了卡尔曼滤波的更新方程。具体步骤如下：

其中，分别为j个高斯分量的权重、均值和协方差，x_k+1(z)表示t_k+1时刻的杂波密度。

(3).最后，由于获得了更新时刻目标的状态估计，故T_k需要赋予新值，即T_k＝t_k+1

步骤四：一步滞后量测(t_k+1＜T_k)处理

同步骤三，t_k时刻目标的后验强度可以表示为高斯混合的形式：

当融合中心接到的下一个传感器在t_k+1(t_k+1＜T_k)时刻的量测集合Z_k+1时，新的滤波时刻仍为t_k(为强调量测的滞后性，这里用Z_d代替滞后量测Z_k+1，用t_d代替滞后量测的真实时刻t_k+1，滞后的相关量均带有下标d)。本发明主要基于直接更新法中的次优方法(B1法)，利用Z_d对t_k时刻目标状态直接进行二次更新，得到新的高斯分量

由步骤三可知，t_k时刻的目标主要分为两类：

第一项为未被检测到的目标，直接用先验高斯分量代替，并利用目标未被检测到的概率(1-P_D，k)对其权重进行修正，第二项代表被检测到的目标，且每一个目标都用所有量测z∈Z_k对其先验高斯分量进行了更新。本发明对于两种目标分别进行了反向状态预测及二次更新。

1.t_k时刻未被检测到的目标(先验目标)

(1).反向状态预测

未被检测到的目标，用先验高斯分量作为后验高斯分量，假设量测一步滞后，t_d、t_k两时刻相距不远，t_d时刻目标状态集合可简化表示为t_k时刻的所有目标：

所以，t_d时刻目标的先验强度可以为：

其中，p_s，d为目标存活概率，分别为j个高斯分量的权重、均值和协方差，可推得

其中，为从t_d到t_k的过程噪声v的协方差，为状态变量x和过程噪声v的互协方差。

(2).滞后量测对最新目标状态的二次更新：

如(1)中所述，t_d时刻目标的先验强度可以统一表示为高斯混合形式：

其中，分别为反向状态预测得到的第i个高斯分量的权重、均值和协方差。

得到t_d时刻多目标PHD的预测后，依照卡尔曼滤波算法，对于每一个由下式直接对PHD进行二次更新：

式中等号右面第一项代表t_d时刻未被检测到的目标，直接用原有后验高斯分量代替，P_D，d表示t_d时刻目标的检测概率，第二项代表对于每一个量测z∈Z_d，都有对应的更新高斯分量，即：

式中对单个高斯分量使用了卡尔曼滤波的更新方程。具体步骤如下：

依据线性估计基本方程，由反向预测结果可计算出t_d时刻量测的协方差t_k时刻状态x_k和t_d时刻量测z_d的协方差更新增益

因此，更新后的高斯分量的均值和协方差为：

2.t_k时刻被检测到的目标(后验目标)

(1).反向状态预测

与1中先验目标相似，t_d时刻后验目标状态集合可简化表示为t_k时刻的所有目标：

所以，t_d时刻目标的先验强度可以为：

其中，p_S，d为目标存活概率，分别为j个高斯分量的权重、均值和协方差，类似卡尔曼滤波中的预测步骤，有

其中，为从t_d到t_k的过程噪声v的协方差，为状态变量x和过程噪声v的互协方差，具体表达式如下：

(2).滞后量测对最新目标状态的二次更新：

如(1)中所述，t_d时刻目标的先验强度可以统一表示为高斯混合形式：

其中，分别为反向状态预测得到的第i个高斯分量的权重、均值和协方差。

得到t_d时刻多目标PHD的预测后，依照卡尔曼滤波算法，对于每一个由下式直接对PHD进行二次更新：

式中等号右面第一项代表t_d时刻未被检测到的目标，直接用原有后验高斯分量代替，P_D，d表示t_d时刻目标的检测概率，第二项代表对于每一个量测z∈Z_d，都有对应的更新高斯分量，即：

式中对单个高斯分量使用了卡尔曼滤波的更新方程。具体步骤如下：

依据线性估计基本方程，由反向预测结果可计算出t_d时刻量测的协方差t_k时刻状态x_k和t_d时刻量测z_d的协方差更新增益

因此，更新后的高斯分量的均值和协方差为：

步骤五：目标个数及状态提取

在需要获得目标状态估计的时刻，融合中心对当前最新估计结果进行目标个数及状态提取，并输出结果。也可以由融合中心依据一定的输出频率进行该步骤。这里选择权重大于0.5的高斯分量进行输出，具体方法如下：

对于高斯分量集合设输出状态集合为

输出作为多目标的状态估计结果。

仿真实验对比了三种不同测量情形下的融合方法，如图3所示。

(a)顺序量测序贯PHD滤波器(ISM-PHD)：两个传感器的量测到达融合中心顺序与采样顺序一致，融合中心基于PHD滤波器对异步测量数据进行序贯融合。

(b)非顺序量测PHD滤波器(OOSM-PHD)：两个传感器的量测到达融合中心顺序与采样顺序相反，融合中心采取基于PHD滤波器的非顺序量测融合方法。

(c)丢帧情况下PHD滤波器：与(b)相同，两个传感器的量测到达融合中心顺序与采样顺序相反，但融合中心丢掉滞后量测，只利用顺序量测进行目标状态估计。

采取100次蒙特卡洛仿真实验，每次仿真目标真实轨迹保持不变，量测重新生成。图4为一次蒙特卡洛仿真中目标的真实轨迹及测量值，其余图皆为100次蒙特卡洛实验结果。图5为三种场景下跟踪估计结果，可以看出非顺序量测PHD滤波器的跟踪结果与基于顺序量测效果相当，而(c)中丢掉非顺序量测使得估计结果散乱，精度较差。图6为三种情况下跟踪结果的Hausdoff距离和Wasserstein距离对比图，图7为三种情况下目标数目估计值，分别从不同角度进一步说明所提出的非顺序量测PHD滤波器较为稳定，达到了和基于顺序量测的算法相近的效果，而直接将滞后量测丢掉的做法由于量测数目缺失，造成跟踪精度较低，结果易发散。

Claims (5)

1.一种基于GM-PHD滤波的多传感器非顺序量测异步融合方法，其特征在于包括如下步骤：

1)建立多传感器多目标跟踪模型，包括目标运动模型、量测模型、杂波模型，并初始化；

2)建立多传感器非顺序量测异步融合框架，融合中心当前最新的目标估计值对应的时刻为t_k，将其作为下一帧量测的比较基准，用T_k表示(T_k＝t_k)，当接收到一帧传感器的量测，融合中心将量测时间戳t_k+1与T_k比较，判断该量测属于顺序量测或到达滞后量测，然后采取如下融合策略：对于顺序量测，即t_k+1>T_k，应用高斯混合PHD滤波器进行剪枝合并、预测、更新，得到新的估计结果；对于到达滞后量测，即t_k+1<T_k，应用基于直接更新法的高斯混合PHD滤波器，先进行反向状态预测，然后借助滞后量测对当前估计结果二次滤波；

3)在需要获得目标状态估计的时刻，融合中心对当前最新估计结果进行目标个数及状态提取并输出。

2.如权利要求1所述的基于GM-PHD滤波的多传感器非顺序量测异步融合方法，其特征在于所述的步骤1)具体为：

1.1)建立多传感器多目标跟踪模型，并初始化；

用X_k＝{x_k,1,x_k,2,…,x_k,M(k)}表示t_k时刻多目标状态构成的随机有限集，用Z_k＝{z_k,1,z_k,2,…,z_k,N(k)}表示t_k时刻量测构成的随机有限集，其中，x_k,1,x_k,2,…,x_k,M(k)分别为第1、2、…、M(k)个目标状态，z_k,1,z_k,2,…,z_k,N(k)分别为第1、2、…、N(k)个量测；

对于单个目标来说，运动模型为：

x_k＝F(k,k-1)x_k-1+v(k,k-1)

其中，为目标状态变量，其中的元素分别代表x方向的位置，x方向的速度，y方向的位置，y方向的速度；F(k,k-1)是目标从t_k-1时刻到t_k时刻的状态转移矩阵，v(k,k-1)是从t_k-1时刻到t_k时刻的状态误差累积过程，服从N(0,Q(k,k-1))的分布；

对于单个量测来说，量测模型为：

z_k＝H(k)x_k+w(k)

其中，z_k为t_k时刻的一个量测，H(k)为t_k时刻量测函数，w(k)是分布为N(0,R(k))的量测噪声；

给定t_k-1时刻多目标状态X_k-1，t_k时刻的多目标状态X_k由继续生存的目标S_k|k-1(x)、和新生的目标Γ_k构成：

量测方面，多目标的量测Z_k由真实量测Θ_k(x)和杂波K_k构成，即：

起始时刻，目标表示为高斯混合的形式：

式中x为目标状态空间的任意值，v₀(x)为起始时刻目标状态x的概率假设密度函数(或强度函数)，J₀表示起始时刻目标的高斯分量个数，分别为第i个高斯分量的权重、均值和协方差，根据跟踪场景进行选取；

杂波模型是强度为κ_k(z)的泊松有限集，即：

κ_k(z)＝λ_cVu(z)

其中，u(z)表示监视区域的均匀概率分布，λ_c为杂波强度，V为传感器监视区域的面积。

3.如权利要求1所述的基于GM-PHD滤波的多传感器非顺序量测异步融合方法，其特征在于所述的步骤2中的，剪枝合并步骤在预测、更新之前，且剪枝合并步骤仅在顺序量测时进行，到达滞后量测时不进行。

4.如权利要求1所述的基于GM-PHD滤波的多传感器非顺序量测异步融合方法，其特征在于所述的步骤2中，所述的应用基于直接更新法的高斯混合PHD滤波器，先进行反向状态预测，然后借助滞后量测对当前估计结果二次滤波，具体为：

t_k时刻的目标主要分为两类：

第一项为未被检测到的目标，直接用先验高斯分量代替，并利用目标未被检测到的概率(1-P_D,k)对其权重进行修正，这里称为先验目标；第二项代表被检测到的目标，且每一个目标都用所有量测z∈Z_k对其先验高斯分量进行了更新，这里称为后验目标，两种目标分别进行反向状态预测及二次更新。

5.如权利要求1所述的基于GM-PHD滤波的多传感器非顺序量测异步融合方法，其特征在于，所述的两种目标分别进行反向状态预测及二次更新，具体为：

5.1t_k时刻未被检测到的目标，即先验目标

(1).反向状态预测

未被检测到的目标，用先验值作为后验高斯分量，t_d时刻先验目标状态集合可简化表示为t_k时刻的所有目标：

所以，t_d时刻目标的先验强度可以为：

其中，p_s,d为目标存活概率，分别为j个高斯分量的权重、均值和协方差，可推得

其中，为从t_d到t_k的过程噪声v的协方差，为状态变量x和过程噪声v的互协方差；

(2).滞后量测对最新目标状态的二次更新：

如(1)中所述，t_d时刻目标的先验强度可以统一表示为高斯混合形式：

其中，分别为反向状态预测得到的第i个高斯分量的权重、均值和协方差；

得到t_d时刻多目标PHD的预测后，依照卡尔曼滤波算法，对于每一个由下式直接对PHD进行二次更新：

式中等号右面第一项代表t_d时刻未被检测到的目标，直接用原有后验高斯分量代替，P_D,d表示t_d时刻目标的检测概率，第二项代表对于每一个量测z∈Z_d，都有对应的更新高斯分量，即：

式中对单个高斯分量使用了卡尔曼滤波的更新方程，具体步骤如下：

依据线性估计基本方程，由反向预测结果可计算出t_d时刻量测的协方差t_k时刻状态x_k和t_d时刻量测z_d的协方差更新增益

因此，更新后的高斯分量的均值和协方差为：

5.2t_k时刻被检测到的目标，即后验目标

(1).反向状态预测

与1中先验目标相似，t_d时刻后验目标状态集合可简化表示为t_k时刻的所有目标：

所以，t_d时刻目标的先验强度可以为：

其中，p_s,d为目标存活概率，分别为j个高斯分量的权重、

均值和协方差，类似卡尔曼滤波中的预测步骤，有

其中，为从t_d到t_k的过程噪声v的协方差，为状态变量x和过程噪声v的互协方差，具体表达式如下：

(2).滞后量测对最新目标状态的二次更新：

如(1)中所述，t_d时刻目标的先验强度可以统一表示为高斯混合形式：

其中，分别为反向状态预测得到的第i个高斯分量的权重、均值和协方差；

得到t_d时刻多目标PHD的预测后，依照卡尔曼滤波算法，对于每一个由下式直接对PHD进行二次更新：

式中等号右面第一项代表t_d时刻未被检测到的目标，直接用原有后验高斯分量代替，P_D,d表示t_d时刻目标的检测概率，第二项代表对于每一个量测z∈Z_d，都有对应的更新高斯分量，即：

式中对单个高斯分量使用了卡尔曼滤波的更新方程，具体步骤如下：

依据线性估计基本方程，由反向预测结果计算出t_d时刻量测的协方差t_k时刻状态x_k和t_d时刻量测z_d的协方差更新增益

更新后的高斯分量的均值和协方差为：