1.一种基于道路网的机动目标跟踪算法,其特征在于,包括以下步骤:
A.构建道路信息库;
B.基于道路信息库的地面目标跟踪:
B-1.根据道路信息库确定总目标运动模型集其中,Nr为总模型集的模型个数,每一个运动模型Mp包括该模型的状态转移矩阵Fp和过程噪声协方差矩阵Qp的信息,确定各模型间的模型转移概率矩阵[πpq],其中p=1,2,..,Nr,q=1,2,..,Nr;
B-2.初始化,令观测时刻k=0,即为初始时刻;定义初始时刻的目标后验强度D0(x)为:
<mrow>
<msub>
<mi>D</mi>
<mn>0</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>j</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>J</mi>
<mn>0</mn>
</msub>
</munderover>
<msubsup>
<mi>w</mi>
<mn>0</mn>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>|</mo>
<msubsup>
<mi>m</mi>
<mn>0</mn>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>,</mo>
<msubsup>
<mi>P</mi>
<mn>0</mn>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
其中,j=1,2,...,J0,J0为初始时刻的目标后验强度的高斯分量数,表示均值为协方差矩阵为的高斯分布,和分别为高斯分量j的权重、均值和协方差矩阵,为初始时刻目标个数期望值;
B-3.对总目标运动模型集中的每一个运动模型分别定义一个GMPHD滤波器,将每个GMPHD滤波器初始时刻的目标后验强度函数D0,p(x)都取为D0(x),即:
<mrow>
<msub>
<mi>D</mi>
<mrow>
<mn>0</mn>
<mo>,</mo>
<mi>p</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>D</mi>
<mn>0</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>j</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>J</mi>
<mn>0</mn>
</msub>
</munderover>
<msubsup>
<mi>w</mi>
<mn>0</mn>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>|</mo>
<msubsup>
<mi>m</mi>
<mn>0</mn>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>,</mo>
<msubsup>
<mi>P</mi>
<mn>0</mn>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
其中,D0,p(x)为基于模型Mp(p=1,2,..,Nr)定义的GMPHD滤波器的初始目标后验强度函数;
B-4.对高斯分量标号j(j=1,2,..,J0),定义模型标记向量其中,(p=1,2,..,Nr,j=1,2,..,J0)定义为
<mrow>
<msubsup>
<mi>I</mi>
<mrow>
<mn>0</mn>
<mo>,</mo>
<mi>p</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mrow>
<mi>p</mi>
<mo>&Element;</mo>
<msubsup>
<mi>M</mi>
<mn>0</mn>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mi>p</mi>
<mo>&NotElement;</mo>
<msubsup>
<mi>M</mi>
<mn>0</mn>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
为k=0时刻标号为j的高斯分量的当前运动模型子集,作为初始时刻,各标号高斯分量的当前运动模型子集(j=1,2,..,J0)取为总目标运动模型集M,从而对j=1,2,..,J0,模型标记向量都为全1向量[1,1,...,1];
B-5.对每个高斯分量标号j(j=1,2,..,J0),定义模型权重向量其中,(p=1,2,..,Nr,j=1,2,..,J0)为模型p在当前运动模型子集中的模型概率(如果模型p不在模型集合中,其概率为0),作为初始时刻,各标号高斯分量的当前运动模型子集(j=1,2,..,J0)取为总目标运动模型集M且当前运动模型子集中各模型概率取相等,从而对j=1,2,..,J0,模型权重向量都为
C.令k=k+1,即为计算下一时刻;
D.假设k-1时刻,即k时刻的前一时刻,基于模型Mp定义的GMPHD滤波器(p=1,2,..,Nr)的目标后验强度函数Dk-1,p(x)如下:
<mrow>
<msub>
<mi>D</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>p</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<munderover>
<mi>&Sigma;</mi>
<mrow>
<mi>j</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>J</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</munderover>
<msubsup>
<mi>w</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>p</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>|</mo>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>p</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>,</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>p</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
其中,j=1,2,...,Jk-1,Jk-1为k-1时刻目标后验强度的高斯分量数,表示均值为协方差矩阵为的高斯分布,和分别为滤波器后验强度函数的第j个高斯分量的权重、均值和协方差矩阵;假设k-1时刻各高斯分量标号j=1~Jk-1对应的模型标记向量和模型权重向量分别为和其中,(p=1,2,..,Nr,j=1,2,..,Jk-1)定义为:
<mrow>
<msubsup>
<mi>I</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>p</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mrow>
<mi>p</mi>
<mo>&Element;</mo>
<msubsup>
<mi>M</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mi>p</mi>
<mo>&NotElement;</mo>
<msubsup>
<mi>M</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
为k-1时刻标号为j的高斯分量的当前运动模型子集,(p=1,2,..,Nr,j=1,2,..,Jk-1)为模型p在模型集合中的模型概率;k-1时刻Nr个滤波器的目标后验强度函数组成的集合记为k-1时刻的后验强度函数集合{Dk-1(x)},其中,{Dk-1(x)}表示集合具体步骤如下:
D-1.新生目标强度确定:新生目标强度为高斯和形式:
<mrow>
<msub>
<mi>D</mi>
<mrow>
<mi>&gamma;</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<munderover>
<mi>&Sigma;</mi>
<mrow>
<mi>j</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>J</mi>
<mrow>
<mi>&gamma;</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
</munderover>
<msubsup>
<mi>w</mi>
<mrow>
<mi>&gamma;</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>|</mo>
<msubsup>
<mi>m</mi>
<mrow>
<mi>&gamma;</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>,</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>&gamma;</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
其中,j=1,2,...,Jγ,k,Jγ,k为新生目标强度的高斯分量数,表示均值为协方差矩阵为的高斯分布,和分别为各高斯分量的权重、均值和协方差矩阵,新生目标强度的具体参数一般事先已经给定;定义新生目标各高斯分量j=1,2,..,Jγ,k的模型标记向量和模型权重向量分别为和其中,(p=1,2,..,Nr,j=1,2,..,Jγ,k)定义为:
<mrow>
<msubsup>
<mi>I</mi>
<mrow>
<mi>&gamma;</mi>
<mo>,</mo>
<mi>k</mi>
<mo>,</mo>
<mi>p</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mrow>
<mi>p</mi>
<mo>&Element;</mo>
<msubsup>
<mi>M</mi>
<mrow>
<mi>&gamma;</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mi>p</mi>
<mo>&NotElement;</mo>
<msubsup>
<mi>M</mi>
<mrow>
<mi>&gamma;</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
为新生目标强度标号为j的高斯分量的当前运动模型子集,(p=1,2,..,Nr,j=1,2,..,Jγ,k)为模型p在模型集合中的模型概率;对新生目标,各标号高斯分量的当前运动模型子集(j=1,2,..,Jγ,k)取为总目标运动模型集M且当前运动模型子集中各模型概率取相等,从而对j=1,2,..,Jγ,k,模型标记向量都为全1向量[1,1,...,1],模型权重向量都为
D-2.运动模型集确定:对j=1,2,..,Jk-1,根据k-1时刻标号j的模型权重向量得到最大模型权重对应的运动模型取运动模型的状态转移矩阵对基于运动模型的滤波器的目标后验强度中第j个高斯分量进行一步预测,判定预测值在步骤A-1所述数字地图中的坐标,并根据步骤A-2确定的“区域-区域运动模型集”关系得到预测坐标所在区域的预设目标运动模型集,将其作为k时刻标号j对应的当前运动模型子集,记为根据计算模型标记向量具体为将的项置1,其余清0,其中,j=1,2,...,Jk-1,p=1,2,..,Nr。对k时刻新生目标强度Dγ,k(x)的各高斯分量j=1~Jγ,k,判定其均值在步骤A-1所述数字地图中的坐标,并根据A-2确定的“区域-区域运动模型集”关系得到均值坐标所在区域的预设目标运动模型集,将其作为标号为j的新生目标高斯分量在时刻k对应的当前运动模型子集,记为根据对运动模型标记向量进行更新,具体为将的项置1,其余清0,其中,j=1,2,...,Jγ,k,p=1,2,..,Nr;
D-3.模型混合概率初始化:根据步骤D-2的模型标记向量(j=1,2,...,Jk-1)和k-1时刻的模型权重向量(j=1,2,...,Jk-1),计算各高斯分量在k时刻模型为q(q=1,2,..,Nr)的情况下k-1时刻模型为p(p=1,2,..,Nr)的概率规则为:如果则
<mrow>
<msubsup>
<mi>&mu;</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>k</mi>
<mo>,</mo>
<mi>p</mi>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<msubsup>
<mi>&mu;</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mfrac>
<msubsup>
<mi>&pi;</mi>
<mrow>
<mi>p</mi>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<msubsup>
<mi>&mu;</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>p</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mrow>
其中,为k时刻模型q的模型权重,计算公式为
<mrow>
<msubsup>
<mi>&mu;</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>p</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>N</mi>
<mi>r</mi>
</msub>
</munderover>
<msubsup>
<mi>&pi;</mi>
<mrow>
<mi>p</mi>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<msubsup>
<mi>&mu;</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>p</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mrow>
如果则其中,j=1,2,..,Jk-1,p=1,2,..,Nr,q=1,2,..,Nr。以上步骤后,对标号j的权重(q=1,2,..,Nr)进行归一化,令归一化结果构成模型权重向量其中,q=1,2,..,Nr,j=1,2,...,Jk-1。同理,对新生目标强度Dγ,k(x)的各高斯分量分别计算更新模型权重向量其中,j=1,2,...,Jγ,k,具体操作类似,不再细述;
D-4.混合条件PHD初始化:计算k时刻滤波器(q=1,2,..,Nr)的重初始化目标强度其中
<mrow>
<msub>
<mover>
<mi>D</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>j</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>J</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</munderover>
<msubsup>
<mover>
<mi>w</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>|</mo>
<msubsup>
<mover>
<mi>m</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>,</mo>
<msubsup>
<mover>
<mi>P</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
其中,表示均值为协方差矩阵为的高斯分布,和分别为重初始化目标强度高斯分量j的权重、均值和协方差矩阵,计算规则为,如果模型标记向量中,则
<mrow>
<msubsup>
<mover>
<mi>w</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<munderover>
<mi>&Sigma;</mi>
<mrow>
<mi>p</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>N</mi>
<mi>r</mi>
</msub>
</munderover>
<msubsup>
<mi>w</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>p</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<msubsup>
<mi>&mu;</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>p</mi>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mrow>
<mrow>
<msubsup>
<mover>
<mi>m</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>p</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>N</mi>
<mi>r</mi>
</msub>
</munderover>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>p</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<msubsup>
<mi>&mu;</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>p</mi>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mrow>
<mrow>
<msubsup>
<mover>
<mi>P</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<munderover>
<mi>&Sigma;</mi>
<mrow>
<mi>p</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>N</mi>
<mi>r</mi>
</msub>
</munderover>
<mo>&lsqb;</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>p</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>+</mo>
<mrow>
<mo>(</mo>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>p</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>-</mo>
<msubsup>
<mover>
<mi>m</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
<msup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>p</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>-</mo>
<msubsup>
<mover>
<mi>m</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
<mi>T</mi>
</msup>
<mo>&rsqb;</mo>
<msubsup>
<mi>&mu;</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>p</mi>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mrow>
如果则 和取全零矩阵,其中,q=1,2,..,Nr,j=1,2,...,Jk-1;
D-5.条件PHD预测:计算k时刻滤波器(q=1,2,..,Nr)的预测强度函数Dk|k-1,q(x)
Dk|k-1,q(x)=Ds,k|k-1,q(x)+Dγ,k(x)
其中,存活目标强度DS,k|k-1,q(x)为
<mrow>
<msub>
<mi>D</mi>
<mrow>
<mi>S</mi>
<mo>,</mo>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<munderover>
<mi>&Sigma;</mi>
<mrow>
<mi>j</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>J</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</munderover>
<msubsup>
<mi>w</mi>
<mrow>
<mi>S</mi>
<mo>,</mo>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>|</mo>
<msubsup>
<mi>m</mi>
<mrow>
<mi>S</mi>
<mo>,</mo>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>,</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>S</mi>
<mo>,</mo>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
其中,表示均值为协方差矩阵为的高斯分布, 和分别为存活目标强度DS,k|k-1,q(x)高斯分量j的权重、均值和协方差矩阵,计算规则为,如果模型标记向量中,则
<mrow>
<msubsup>
<mi>w</mi>
<mrow>
<mi>S</mi>
<mo>,</mo>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<msub>
<mi>p</mi>
<mrow>
<mi>S</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
<msubsup>
<mover>
<mi>w</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mrow>
<mrow>
<msubsup>
<mi>m</mi>
<mrow>
<mi>S</mi>
<mo>,</mo>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<msub>
<mi>F</mi>
<mi>q</mi>
</msub>
<msubsup>
<mover>
<mi>m</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mrow>
<mrow>
<msubsup>
<mi>P</mi>
<mrow>
<mi>S</mi>
<mo>,</mo>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<msub>
<mi>F</mi>
<mi>q</mi>
</msub>
<msubsup>
<mover>
<mi>P</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>F</mi>
<mi>q</mi>
</msub>
<mo>)</mo>
</mrow>
<mi>T</mi>
</msup>
<mo>+</mo>
<msub>
<mi>Q</mi>
<mi>q</mi>
</msub>
</mrow>
如果则 和取全零矩阵,其中,pS,k为目标存活概率,是已知参数,Fq为滤波器对应模型Mq的状态转移矩阵,Qq为模型Mq的过程噪声协方差矩阵,上述计算完毕后,将各模型预测强度改写为如下形式:
<mrow>
<msub>
<mi>D</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<munderover>
<mi>&Sigma;</mi>
<mrow>
<mi>j</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>J</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</munderover>
<msubsup>
<mi>w</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>|</mo>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>,</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
其中,Jk|k-1=Jk-1+Jγ,k,Dk|k-1,q(x)中标号为1~Jk-1的高斯分量对应DS,k|k-1,q(x)中的Jk-1个高斯分量,Dk|k-1,q(x)标号中标号为Jk-1+1~Jk|k-1的高斯分量对应Dγ,k(x)中标号为1~Jγ,k的Jγ,k个高斯分量。生成预测强度的模型标记向量(j=1~Jk|k-1),其中,标号为1~Jk-1的来自步骤D-2的标号为Jk-1+1~Jk|k-1的来自D-2的生成预测强度的模型权重向量(j=1,2,...,Jk|k-1),其中,标号为1~Jk-1的来自步骤D-3的标号为Jk-1+1~Jk|k-1的来自步骤D-3的
D-6.条件PHD更新:计算k时刻滤波器(q=1,2,..,Nr)的后验强度函数Dk,q(x)
<mrow>
<msub>
<mi>D</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<msub>
<mi>p</mi>
<mrow>
<mi>D</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<msub>
<mi>D</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>N</mi>
<mrow>
<mi>z</mi>
<mo>,</mo>
<mi>x</mi>
</mrow>
</msub>
</munderover>
<msub>
<mi>D</mi>
<mrow>
<mi>D</mi>
<mo>,</mo>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>;</mo>
<msub>
<mi>z</mi>
<mi>i</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
其中,为k时刻的量测值集合,Zk量测个数为Nz,k,pD,k为目标检测概率,是已知参数,且
<mrow>
<msub>
<mi>D</mi>
<mrow>
<mi>D</mi>
<mo>,</mo>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>;</mo>
<msub>
<mi>z</mi>
<mi>i</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>j</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>J</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</munderover>
<msubsup>
<mi>w</mi>
<mrow>
<mi>D</mi>
<mo>,</mo>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mrow>
<mo>(</mo>
<msub>
<mi>z</mi>
<mi>i</mi>
</msub>
<mo>)</mo>
</mrow>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>|</mo>
<msubsup>
<mi>m</mi>
<mrow>
<mi>D</mi>
<mo>,</mo>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>(</mo>
<msub>
<mi>z</mi>
<mi>i</mi>
</msub>
<mo>)</mo>
<mo>,</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>D</mi>
<mo>,</mo>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>(</mo>
<msub>
<mi>z</mi>
<mi>i</mi>
</msub>
<mo>)</mo>
<mo>)</mo>
</mrow>
</mrow>
其中,表示均值为协方差矩阵为的高斯分布,和分别为后验强度函数Dk,q(x)高斯分量j的权重、均值和协方差矩阵,计算规则为,如果模型标记向量中,则
<mrow>
<msubsup>
<mi>w</mi>
<mrow>
<mi>D</mi>
<mo>,</mo>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mrow>
<mo>(</mo>
<msub>
<mi>z</mi>
<mi>i</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfrac>
<mrow>
<msub>
<mi>p</mi>
<mrow>
<mi>D</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
<msubsup>
<mi>w</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mi>g</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>z</mi>
<mi>i</mi>
</msub>
<mo>|</mo>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mi>&kappa;</mi>
<mi>k</mi>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>z</mi>
<mi>i</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>p</mi>
<mrow>
<mi>D</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
<munderover>
<mi>&Sigma;</mi>
<mrow>
<mi>l</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>J</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</munderover>
<msubsup>
<mi>w</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>l</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mi>g</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>z</mi>
<mi>i</mi>
</msub>
<mo>|</mo>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>l</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
</mfrac>
</mrow>
<mrow>
<msubsup>
<mi>m</mi>
<mrow>
<mi>D</mi>
<mo>,</mo>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mrow>
<mo>(</mo>
<msub>
<mi>z</mi>
<mi>i</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>K</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mrow>
<mo>(</mo>
<msub>
<mi>z</mi>
<mi>i</mi>
</msub>
<mo>-</mo>
<msub>
<mi>H</mi>
<mi>q</mi>
</msub>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msubsup>
<mi>P</mi>
<mrow>
<mi>D</mi>
<mo>,</mo>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mrow>
<mo>(</mo>
<msub>
<mi>z</mi>
<mi>i</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mrow>
<mo>(</mo>
<mi>I</mi>
<mo>-</mo>
<msubsup>
<mi>K</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<msub>
<mi>H</mi>
<mi>q</mi>
</msub>
<mo>)</mo>
</mrow>
<msubsup>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mrow>
<mrow>
<msubsup>
<mi>K</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>H</mi>
<mi>q</mi>
</msub>
<mo>)</mo>
</mrow>
<mi>T</mi>
</msup>
<msup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>S</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
</mrow>
<mrow>
<msubsup>
<mi>S</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<msub>
<mi>H</mi>
<mi>q</mi>
</msub>
<msubsup>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>H</mi>
<mi>q</mi>
</msub>
<mo>)</mo>
</mrow>
<mi>T</mi>
</msup>
<mo>+</mo>
<msub>
<mi>R</mi>
<mi>q</mi>
</msub>
</mrow>
<mrow>
<mi>g</mi>
<mrow>
<mo>(</mo>
<mi>z</mi>
<mo>|</mo>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mi>z</mi>
<mo>|</mo>
<msub>
<mi>H</mi>
<mi>q</mi>
</msub>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>,</mo>
<msub>
<mi>H</mi>
<mi>q</mi>
</msub>
<msubsup>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>H</mi>
<mi>q</mi>
</msub>
<mo>)</mo>
</mrow>
<mi>T</mi>
</msup>
<mo>+</mo>
<msub>
<mi>R</mi>
<mi>q</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
如果则 和取全零矩阵,以上步骤以后,将k时刻模型q的后验强度改写为以下形式:
<mrow>
<msub>
<mi>D</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>j</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>J</mi>
<mi>k</mi>
</msub>
</munderover>
<msubsup>
<mi>w</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>|</mo>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>,</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
其中,Jk=Jk|k-1+Nz,k×Jk|k-1为k时刻滤波器后验强度函数Dk,q(x)的高斯分量数,改写后标号为1~Jk|k-1的高斯分量对应(1-pD,k)Dk|k-1,q(x)的Jk|k-1个高斯分量,标号为(i×Jk|k-1+1)~((i+1)×Jk|k-1)的高斯分量对应DD,k,q(x;zi)的Jk|k-1个高斯分量,其中,i=1,2,...,Nz,k;根据步骤D-5的预测强度模型标记向量(j=1~Jk|k-1)生成后验强度的模型标记向量(j=1~Jk),对应关系为其中,i=0,1,2,...,Nz,k,l=1,2,...,Jk|k-1;根据步骤D-5的预测强度模型权重向量(j=1,2,...,Jk|k-1)生成后验强度的模型标记向量(j=1~Jk),其中,
<mrow>
<msubsup>
<mi>&mu;</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>i</mi>
<mo>&times;</mo>
<msub>
<mi>J</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>+</mo>
<mi>l</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mi>&mu;</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>l</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mi>g</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>z</mi>
<mi>i</mi>
</msub>
<mo>|</mo>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>l</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
<msubsup>
<mi>c</mi>
<mi>k</mi>
<mrow>
<mo>(</mo>
<mi>l</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mfrac>
</mrow>
<mrow>
<msubsup>
<mi>c</mi>
<mi>k</mi>
<mrow>
<mo>(</mo>
<mi>l</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>q</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>N</mi>
<mi>r</mi>
</msub>
</munderover>
<msubsup>
<mi>&mu;</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>l</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mi>g</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>z</mi>
<mi>i</mi>
</msub>
<mo>|</mo>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>l</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
其中,i=0,1,2,...,Nz,k,l=1,2,...,Jk|k-1;
D-7.PHD混合估计:对k时刻全部滤波器的后验强度Dk,q(x)(q=1,2,..,Nr)进行混合估计,得到k时刻的多模型混合估计Dk,M(x):
<mrow>
<msub>
<mi>D</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>M</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>j</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>J</mi>
<mi>k</mi>
</msub>
</munderover>
<msubsup>
<mi>w</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>M</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>|</mo>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>M</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>,</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>M</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
其中,表示均值为协方差矩阵为的高斯分布,和分别为多模型混合估计Dk,M(x)高斯分量j的权重、均值和协方差矩阵,计算公式为:
<mrow>
<msubsup>
<mi>w</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>M</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>q</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>N</mi>
<mi>r</mi>
</msub>
</munderover>
<msubsup>
<mi>w</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<msubsup>
<mi>&mu;</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mrow>
<mrow>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>M</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>q</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>N</mi>
<mi>r</mi>
</msub>
</munderover>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<msubsup>
<mi>&mu;</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mrow>
<mrow>
<msubsup>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>M</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>=</mo>
<munderover>
<mi>&Sigma;</mi>
<mrow>
<mi>q</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>N</mi>
<mi>r</mi>
</msub>
</munderover>
<mo>&lsqb;</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>+</mo>
<mrow>
<mo>(</mo>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>M</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
<msup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>M</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>m</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
<mi>T</mi>
</msup>
<mo>&rsqb;</mo>
<msubsup>
<mi>&mu;</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>q</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mrow>
E.剪枝与合并:设置修剪阈值r,删除k时刻多模型混合估计Dk,M(x)中权值低于该门限的高斯分量,得到剪枝后的目标强度Dk,M(x),同时对Nr个滤波器的后验强度Dk,q(x)(q=1,2,..,Nr)分别进行与混合估计Dk,M(x)相同的剪枝操作,对后验强度的模型权重向量和模型标记向量的相应项也进行删除;设置合并阈值U,计算Dk,M(x)各高斯分量距离,把距离小于阈值U的高斯分量进行合并,得到合并后的目标强度Dk,M(x),同时对Nr个模型下的后验强度Dk,q(x)(q=1,2,..,Nr)分别进行与Dk,M(x)相同的合并操作,对后验强度的模型权重向量(j=1~Jk)和模型标记向量(j=1~Jk)的相应项也进行合并,其中,模型权重向量的合并操作为将被合并的高斯分量对应的模型权重进行求和,得到合并后高斯分量的权重,模型标记向量的合并操作为将被合并的高斯分量对应的模型标记进行求并集,得到合并后高斯分量的模型标记;对修改后的模型权重向量进行归一化,使对剪枝合并后的各高斯分量标号j,
F.数目与状态估计;
G.终止的判断条件。