CN103729637B - 基于容积卡尔曼滤波的扩展目标概率假设密度滤波方法 - Google Patents

Abstract

本发明公开的基于容积卡尔曼滤波的扩展目标概率假设密度滤波方法，具体按照以下步骤实施：步骤1、预先设定k‑1时刻后验强度的高斯混合形式，得到第i个高斯项的均值和协方差；步骤2、对步骤1得到的第i个高斯项的权值、均值和协方差进行一步预测：步骤3、根据步骤2得到的预测结果进行量测更新，得到k时刻各高斯分量的估计值。本发明的基于容积卡尔曼滤波的扩展目标概率假设密度滤波方法，解决非线性系统下的扩展目标跟踪问题和非线性函数的雅克比矩阵不存在或难以求解时的扩展目标跟踪问题，为解决非线性条件下的扩展目标跟踪提出了一种新的实现途径。

Description

基于容积卡尔曼滤波的扩展目标概率假设密度滤波方法

技术领域

本发明属于目标跟踪方法技术领域，涉及一种扩展目标概率假设密度滤波方法，具体涉及一种基于容积卡尔曼滤波的扩展目标概率假设密度滤波方法。

背景技术

目标跟踪就是结合传感器接收到的量测，对目标当前状态进行估计的过程。传统的多目标跟踪方法，如：联合数据关联、多假设跟踪等都是基于数据关联的，随着目标数目的增加，这些跟踪方法会存在“组合爆炸”、计算量呈指数级增长的问题；而近年来提出的基于随机有限集的概率假设密度(Probability Hypothesis Density，PHD)滤波方法，较为有效的避免了数据关联这个难题，能够直接对复杂环境中目标数未知且随时间变化的多目标的状态和个数进行联合估计，逐渐成为多目标跟踪领域的研究热点。但由于PHD滤波的递推公式中含有多个积分，所以它在一般情况下难以获得解析解。

随后，Mahler和Vo等学者分别给出了适用于非线性非高斯条件的序贯蒙特卡罗PHD(Sequential Monte Carlo PHD，SMC-PHD，又称为particle-PHD)滤波和适用于线性高斯条件的高斯混合PHD(Gaussian Mixture PHD，GM-PHD)滤波“Ba-Ngu Vo，Wing-KinMa.The Gaussian mixture probability hypothesis density filter,IEEETransactions on Signal Processing,2006，54(11)：4091-4104.”。在此基础上，Daniel C和Clark D等别对SMC-PHD滤波方法和GM-PHD滤波方法的收敛性进行了证明。Mahler通过将PHD滤波进行扩展，得到了势概率假设密度滤波(cardinalized PHD，CPHD)滤波，该滤波方法可以获得更多的关于多目标状态集合的势分布的信息，能够得到比PHD滤波更加精确的估计，但其时间复杂度较大。类似于PHD滤波，在线性高斯系统下的近似实现方法称为GM-CPHD，在非线性非高斯系统下CPHD的近似实现方法称为SMC-CPHD。

在多目标跟踪系统中，一般都假设目标为点目标，即在每一时刻每个目标至多产生一个量测。但是，当传感器与目标之间的距离较小或传感器分辨率较高时，单个目标(例如一架飞机)的不同发射点(例如机头、机翼和机尾等不同部位)可同时产生多个观测，此时称该目标为一个扩展目标。在假设扩展目标数目服从泊松分布的条件下，Mahler于2009年在文献“Granstrom K,Lundquist C,Orguner U.A Gaussian mixture PHD filter forextended target tracking.Proceedings of the 13^th International Conference onInfermation Fusion,2010,1-8.”中讨论了扩展目标的量测模型，并给出了扩展目标PHD(Extended-target PHD,EPHD)形式解法。紧接着，Granstrom等人给出了EPHD的高斯混合实现方法，称之为高斯混合扩展目标概率假设密度滤波，简称GM-EPHD滤波方法。该方法能够较好地解决线性高斯条件下多个扩展目标的跟踪问题。此外，Granstrom等还提出了一种简单的划分量测集的方法。随后，连峰等在“连峰，韩崇昭，刘伟峰，元向辉。高斯混合扩展目标概率假设密度滤波器的收敛性分析.自动化学报.2012(08):1343-1352.”一文中对扩展目标的GM-PHD滤波方法的收敛性进行了分析。在放松了量测率服从泊松分布条件的假设下，Orguner U和Lundquist C等给出了扩展目标的GM-CPHD滤波方法。与CPHD方法一样，该方法虽然能提高扩展目标的跟踪精度，但其过程较为复杂，且时间复杂度增大。Feldman等学者采用随机矩阵的方法为扩展目标建立椭圆形轮廓，然后将其看作状态变量的一部分进行估计和跟踪。随后，Wieneke等将上述方法推广到多个扩展目标的跟踪情形，使用期望最大化方法对扩展目标的椭圆形状和运动情况进行递推估计。

已有的扩展目标GM-PHD滤波方法只适用于线性高斯系统，扩展目标的扩展卡尔曼概率假设密度(Extended Kalman EPHD，EK-EPHD)滤波方法也只在系统为弱非线性时，能取得较好的滤波效果，而对于强非线性系统，其误差性能退化。针对该问题，本发明方法在扩展目标GM-PHD滤波算法的基础上，结合容积卡尔曼滤波(Cubature Kalman Filter,CKF)算法，给出了一种适用于非线性系统下的扩展目标跟踪方法，该方法能够解决滤波系统中非线性函数雅可比矩阵不存在或难以求解的跟踪问题，为非线性系统下扩展目标的跟踪提供了一种新的实现途径。

发明内容

本发明的目的在于提供一种基于容积卡尔曼滤波的扩展目标概率假设密度滤波方法，解决EK-EPHD处理不了的非线性函数雅克比矩阵不存在或难以求解的跟踪问题。

本发明所采用的技术方案是，基于容积卡尔曼滤波的扩展目标概率假设密度滤波方法，具体按照以下步骤实施：

步骤1、预先设定k-1时刻后验强度的高斯混合形式，得到第i个高斯项的均值和协方差；

步骤2、对步骤1得到的高斯混合形式中第i个高斯项的权值、均值和协方差进行预测；

步骤3、根据步骤2得到的预测结果进行量测更新，得到各高斯分量的估计值，完成滤波。

本发明的特点还在于，

步骤1中的高斯混合形式为：

${\mathrm{\υ}}_{k-1}\left(x_{k-1}\right)=\underset{i=1}{\overset{J_{k-1}}{\Σ}}{w}_{k-1}^{\left(i\right)}N\left(x_{k-1}\right|m_{k-1}^{\left(i\right)},P_{k-1}^{\left(i\right)});$

其中：J_k-1表示k-1时刻的高斯项数，分别表示第i个高斯项的权值、均值和协方差，N(:|m,P)表示一个具有均值m、协方差P的高斯密度。

步骤2具体按照以下步骤实施：

步骤2.1、对步骤1中得到的k-1时刻的高斯项方差做Cholesky分解，经Cholesky分解后得到：

$P_{k-1}^{\left(i\right)}=S_{k-1}^{\left(i\right)}{\left(S_{k-1}^{\left(i\right)}\right)}^T---\left(3\right);$

步骤2.2、计算出步骤1中高斯分量的容积点，具体按照以下算法实施：

$x_{l,k-1}^{\left(i\right)}=S_{k-1}^{\left(i\right)}{\mathrm{\ξ}}_l+m_{k-1}^{\left(i\right)}---\left(4\right);$

式中，ξ_l为第l个标准容积点，[1]_l表示第l列，[1]与状态向量的维数有关，当状态向量维数为2时，[1]表示下列点集：标准容积点的标号l＝1,2,…,m，而m等于状态向量维数的2倍；

步骤2.3、根据状态方程计算第i个高斯项的第l个预测容积点(l＝1,2,...,m)，具体按照以下算法实施：

$x_{l,k|k-1}^{\left(i\right)}=f\left(x_{l,k-1}^{\left(i\right)}\right)---\left(5\right);$

式中，表示状态转移函数；

步骤2.4、计算得到存活目标的进一步预测均值和方差，具体按照以下算法实施：

$m_{k|k-1}^{\left(i\right)}=\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{x}_{l,k|k-1}^{\left(i\right)}---\left(6\right);$

$P_{k|k-1}^{\left(i\right)}=\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{x}_{l,k|k-1}^{\left(i\right)}{\left(x_{l,k|k-1}^{\left(i\right)}\right)}^T-m_{k|k-1}^{\left(i\right)}{\left(m_{k|k-1}^{\left(i\right)}\right)}^T+Q_{k-1}---\left(7\right);$

式(7)中，Q_k-1为过程噪声方差；

步骤2.5、对各高斯项的权值进行进一步预测，具体按照以下算法实施：

对于新生目标的高斯项，其权值预测为：

$w_{k|k-1}^{\left(i\right)}=w_{\mathrm{\γ},k}^{\left(j\right)}---\left(8\right);$

式(8)中，表示k时刻新生目标第j个高斯项的权值；

对于衍生目标的高斯项，其权值预测为：

$w_{k|k-1}^{\left(i\right)}=w_{k-1}^{\left(l\right)}{w}_{\mathrm{\β},k}^{\left(j\right)}---\left(9\right);$

式(9)中，表示k-1时刻的第l个高斯项的权值，表示k时刻由对应的目标衍生出的第j个高斯项的权值；

对于存活目标的高斯项，其权值预测为：

$w_{k|k-1}^{\left(i\right)}=p_S{w}_{k-1}^{\left(j\right)}---\left(10\right);$

式(10)中，p_S表示目标存活概率；

根据上述的算法，进一步预测强度的高斯混合形式为：

${\mathrm{\υ}}_{k|k-1}\left(x_k\right)=\underset{i=1}{\overset{J_{k|k-1}}{\Σ}}{w}_{k|k-1}^{\left(i\right)}N\left(x_k\right|m_{k|k-1}^{\left(i\right)},P_{k|k-1}^{\left(i\right)})---\left(11\right);$

式(11)中，J_k|k-1表示进一步预测的高斯项数。

步骤3具体按照以下步骤实施：

步骤3.1、量测更新分为两部分：一部分为未检测到的目标强度，另一部分为检测到的目标强度；

若更新未检测到目标强度，则按照以下算法实施：

$w_{k|k}^{\left(i\right)}=(1-(1-e^{-\mathrm{\γ}\left(m_{k|k-1}^{\left(i\right)}\right)}\left)p_D\right)w_{k|k-1}^{\left(i\right)}---\left(12\right);$

式(12)中，表示由目标状态所产生的量测个数的期望，表示至少产生一个量测的概率，p_D为检测率，为有效检测率；

$m_{k|k}^{\left(i\right)}=m_{k|k-1}^{\left(i\right)}---\left(13\right);$

$P_{k|k}^{\left(i\right)}=P_{k|k-1}^{\left(i\right)}---\left(14\right);$

若未检测到的目标后验强度可表示为：

${\mathrm{\υ}}_{k|k}^{ND}\left(x_k\right)=\underset{i=1}{\overset{J_{k|k-1}}{\Σ}}{w}_{k|k}^{\left(i\right)}N\left(x_k\right|m_{k|k}^{\left(i\right)},P_{k|k}^{\left(i\right)});$

若检测到的目标强度，具体按照以下算法实施：

对做Cholesky分解，满足以下关系：

$P_{k|k-1}^{\left(i\right)}=S_{k|k-1}^{\left(i\right)}{\left(S_{k|k-1}^{\left(i\right)}\right)}^T;$

则新的容积点为：

$x_{l,k|k-1}^{\left(i\right)}=S_{k|k-1}^{\left(i\right)}{\mathrm{\ξ}}_l+m_{k|k-1}^{\left(i\right)}---\left(15\right);$

式(15)中，标准容积点ξ_l标号l＝1,2,…,m，而m等于状态向量维数的2倍；

步骤3.2、利用量测函数计算出传递的容积点，具体按照以下算法实施：

$z_{l,k|k-1}^{\left(i\right)}=h\left(x_{l,k|k-1}^{\left(i\right)}\right)---\left(16\right);$

式(16)中，h(·)为目标的量测函数；

步骤3.3、对步骤3.2得到的进行扩维，具体按照以下算法实施：

${\stackrel{\‾}{zz}}_{l,k|k-1}^{\left(i\right)}=repeat(z_{l,k|k-1}^{\left(i\right)},absW,1)---\left(17\right);$

式(17)中，absW表示当前时刻的每个量测划分里的元胞W所包含的量测的个数，其中，当前时刻的每个量测划分里的元胞W为已知量，repeat(A,m,n)函数表示创建一个m×n矩阵，矩阵中的每个值都为A；

步骤3.4、估计量测预测，具体按照以下算法实施：

${\hat{z}}_{k|k-1}^{\left(i\right)}=\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{z}_{l,k|k-1}^{\left(i\right)}---\left(18\right);$

${\stackrel{\‾}{zz}}_{k|k-1}^{\left(i\right)}=\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{\stackrel{\‾}{zz}}_{l,k|k-1}^{\left(i\right)}---\left(19\right);$

步骤3.5、更新积分点的状态和协方差，具体按照以下算法实施：

$m_{k|k}^{\left(i\right)}=m_{k|k-1}^{\left(i\right)}+K_k^{\left(i\right)}({\[z_1,...,z_{\left|W\right|}\]}^T-{\stackrel{\‾}{zz}}_{k|k-1}^{\left(i\right)})---\left(20\right);$

$P_{k|k}^{\left(i\right)}=P_{k|k-1}^{\left(i\right)}-K_k^{\left(i\right)}{P}_{zz}^{\left(i\right)}{\left(K_k^{\left(i\right)}\right)}^T---\left(21\right);$

式(20)中，[z₁,…,z_|W|]^T为相应元胞W中的真实量测值，为滤波增益，具体按照以下算法实施：

$K_k^{\left(i\right)}=P_{xz}^{\left(i\right)}{\left(P_{zz}^{\left(i\right)}\right)}^{-1}---\left(22\right);$

$P_{zz}^{\left(i\right)}={\stackrel{\‾}{R}}_k+\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{\stackrel{\‾}{zz}}_{l,k|k-1}^{\left(i\right)}{\left({\stackrel{\‾}{zz}}_{l,k|k-1}^{\left(i\right)}\right)}^T-{\stackrel{\‾}{zz}}_{k|k-1}^{\left(i\right)}{\left({\stackrel{\‾}{zz}}_{k|k-1}^{\left(i\right)}\right)}^T---\left(23\right);$

$P_{xz}^{\left(i\right)}=\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{x}_{l,k|k-1}^{\left(i\right)}{\left({\stackrel{\‾}{zz}}_{l,k|k-1}^{\left(i\right)}\right)}^T-m_{k|k-1}^{\left(i\right)}{\left({\stackrel{\‾}{zz}}_{k|k-1}^{\left(i\right)}\right)}^T---\left(24\right);$

式(23)中，表示扩维后的量测噪声协方差矩阵：

式(25)中，blkdiag(.)表示块对角矩阵，|W_k|表示当前元胞W_k的所包含的量测值的个数；

步骤3.6、更新相应的权值：

$w_{k|k}^{\left(i\right)}=w_p\frac{\mathrm{\Γ}\left(i\right)p_D}{dW}{\mathrm{\Φ}}_W^{\left(i\right)}{w}_{k|k-1}^{\left(i\right)}---\left(26\right);$

式(26)中，w_p是划分的权重，且满足：

$w_p=\frac{{\Π}_{W\∈p}dW}{{\Σ}_{p^{\′}\∠Z^{\′}}{\Π}_{W^{\′}\∈p^{\′}}{\mathrm{dW}}^{\′}}---\left(27\right);$

式(27)中，p和p'均表示对量测集Z的划分，W和W'分别表示p和p'中的一个子集元胞；

Γ(i)＝e^-γ(i)(γ(i))^|W| (28)；

式(28)中，|W|表示集合的势；

$dW={\mathrm{\δ}}_{\left|W\right|,1}+\underset{i=1}{\overset{J_{k|k-1}}{\Σ}}\mathrm{\Γ}\left(i\right)p_D{\mathrm{\Φ}}_W^{\left(i\right)}{w}_{k|k-1}^{\left(i\right)}---\left(29\right);$

式(29)中，δ_|W|,1表示若|W|＝1，则δ_|W|,1＝1，否则，δ_|W|,1＝0；

${\mathrm{\Φ}}_W^{\left(i\right)}=\underset{z\∈W}{\Π}\frac{{\mathrm{\φ}}_z\left(m_{k|k-1}^{\left(i\right)}\right)}{{\mathrm{\λ}}_k{c}_k\left(z\right)}---\left(30\right);$

式(30)中，表示在元胞W中的量测值概率，λ_kc_k(z)表示k时刻杂波的强度，c_k(z)表示杂波概率密度；λ_k表示杂波的平均数，通常设定每个时刻杂波出现的个数服从Poisson分布：

${\mathrm{\φ}}_z\left(m_{k|k-1}^{\left(i\right)}\right)=N\left(z_k\right|{\hat{z}}_{k|k-1}^{\left(i\right)},R_k+\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{z}_{l,k|k-1}^{\left(i\right)}{\left(z_{l,k|k-1}^{\left(i\right)}\right)}^T-{\hat{z}}_{k|k-1}^{\left(i\right)}{\left({\hat{z}}_{k|k-1}^{\left(i\right)}\right)}^T)---\left(31\right);$

量测更新后的后验强度为如下的高斯混合形式：

$\begin{array}{c}{\mathrm{\υ}}_{k|k}\left(x_k\right)={\mathrm{\υ}}_{k|k}^{ND}\left(x_k\right)+\underset{p_k\∠Z_k}{\Σ}\underset{W_k\∠p_k}{\Σ}{\mathrm{\υ}}_{k|k}^D(x_k,W_k)\\ =\underset{i=1}{\overset{J_k}{\Σ}}{w}_{k|k}^{\left(i\right)}N\left(x_k\right|m_{k|k}^{\left(i\right)},P_{k|k}^{\left(i\right)})\end{array}---\left(32\right);$

式(32)中，J_k表示k时刻的高斯项数，由此得到了各高斯分量的

本发明的有益效果在于：

(1)本发明的基于容积卡尔曼滤波的扩展目标概率假设密度滤波方法针对非线性系统下的扩展目标跟踪，不需要计算非线性函数的雅克比矩阵，滤波精度与EK-EPHD方法几乎相同；

(2)本发明的基于容积卡尔曼滤波的扩展目标概率假设密度滤波方法能够解决EK-EPHD处理不了的非线性函数雅克比矩阵不存在或难以求解的跟踪问题，该方法可以用于非线性系统中的扩展目标跟踪。

附图说明

图1是单个目标的真实轨迹及分别采用本发明的CK-EPHD滤波方法及EK-EPHD滤波方法的滤波估计对比图；

图2是在整个仿真时间里的目标量测值图谱；

图3是采用传统EK-GMPHD滤波方法估计出来的目标个数图；

图4是在整个时间段内目标的真实个数及分别采用本发明的CK-EPHD滤波方法和传统的EK-EPHD滤波方法估计得到的结果对比图；

图5是采用本发明的CK-EPHD滤波方法和传统的EK-EPHD滤波方法对应的OSPA距离对比图；

图6是多个扩展目标的真实轨迹与采用本发明的CK-EPHD滤波方法、传统的EK-EPHD滤波方法滤波后得到的估计对比图；

图7是在整个仿真时间里的多个扩展目标量测值图谱；

图8是采用传统的EK-GMPHD滤波方法得到的多个扩展目标估计个数图；

图9是目标的真实个数及分别采用本发明的CK-EPHD滤波方法和传统的EK-EPHD滤波方法得到的结果对比图；

图10是在整个仿真时间里本发明的CK-EPHD滤波方法和传统的EK-EPHD滤波方法对应的OSPA距离对比图。

具体实施方式

下面结合具体实施方式对本发明进行详细说明。

扩展目标与一般意义下的点目标不同，点目标假设每个时刻至多产生一个量测，而扩展目标则假设每一时刻可能会产生多个量测。

设定k时刻的扩展目标状态集为：

其中，表示k时刻第i个目标的状态向量，N_k表示k时刻的扩展目标数。

通常，在未特别说明的情况下，扩展目标的状态是指它质心的运动状态，具体包括扩展目标质心的位置、速度等；

相应地，k时刻得到的量测集为

其中，表示k时刻第i个量测值，M_k表示量测值的个数。

量测集由真实扩展目标产生的量测和杂波构成，且二者不可区分，在多个扩展目标的跟踪系统中，扩展目标状态的动态模型为：

$X_k={\left\{x_k^{\left(i\right)}\right\}}_{i=1}^{N_k}=f(X_{k-1},v_k)---\left(1\right);$

其中，ν_k是零均值高斯白噪声，协方差为Q_k，f(.)表示状态转移函数。

量测模型可写为：

$Z_k={\left\{z_k^{\left(i\right)}\right\}}_{i=1}^{M_k}=h(X_k,w_k)---\left(2\right);$

其中，w_k是零均值高斯白噪声，协方差为R_k，h(.)表示量测函数。

Granstrom等已证明，若扩展目标PHD的预测能够写为高斯混合形式，则量测更新后的PHD也为高斯混合形式，进而各个时刻的预测和更新PHD也都服从高斯混合形式，这一递推实现的过程即为GM-EPHD滤波。这与传统的点目标GM-PHD滤波方法不同，由于扩展目标在每一时刻都会有多个量测，所以在滤波估计前必须对量测集进行划分，然后再进行一步预测和量测更新。

本发明的基于容积卡尔曼滤波的扩展目标概率假设密度滤波方法，具体按照以下步骤实施：

步骤1、预先设定k-1时刻后验强度的高斯混合形式为：

${\mathrm{\υ}}_{k-1}\left(x_{k-1}\right)=\underset{i=1}{\overset{J_{k-1}}{\Σ}}{w}_{k-1}^{\left(i\right)}N\left(x_{k-1}\right|m_{k-1}^{\left(i\right)},P_{k-1}^{\left(i\right)});$

其中：J_k-1表示k-1时刻的高斯项数，分别表示第i个高斯项的权值、均值和协方差，N(:|m,P)表示一个具有均值m、协方差P的高斯密度；

步骤2、对步骤1得到的高斯混合形式中第i个高斯项的权值、均值和协方差进行预测：

步骤2.1、对步骤1中得到的k-1时刻的高斯项方差做Cholesky分解(即平方根法)，经Cholesky分解后得到：

$P_{k-1}^{\left(i\right)}=S_{k-1}^{\left(i\right)}{\left(S_{k-1}^{\left(i\right)}\right)}^T---\left(3\right);$

步骤2.2、计算出步骤1中高斯分量的容积点，具体按照以下算法实施：

$x_{l,k-1}^{\left(i\right)}=S_{k-1}^{\left(i\right)}{\mathrm{\ξ}}_l+m_{k-1}^{\left(i\right)}---\left(4\right);$

式(4)中，ξ_l为第l个标准容积点，[1]_l表示第l列，[1]与状态向量的维数有关，例如：当状态向量维数为2时，[1]表示下列点集：标准容积点的标号l＝1,2,…,m，而m等于状态向量维数的2倍；

步骤2.3、根据状态方程计算第i个高斯项的第l个预测容积点(l＝1,2,...,m)，具体按照以下算法实施：

$x_{l,k|k-1}^{\left(i\right)}=f\left(x_{l,k-1}^{\left(i\right)}\right)---\left(5\right);$

式(5)中，表示状态转移函数；

步骤2.4、计算得到存活目标的进一步预测均值和方差，具体按照以下算法实施：

$m_{k|k-1}^{\left(i\right)}=\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{x}_{l,k|k-1}^{\left(i\right)}---\left(6\right);$

$P_{k|k-1}^{\left(i\right)}=\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{x}_{l,k|k-1}^{\left(i\right)}{\left(x_{l,k|k-1}^{\left(i\right)}\right)}^T-m_{k|k-1}^{\left(i\right)}{\left(m_{k|k-1}^{\left(i\right)}\right)}^T+Q_{k-1}---\left(7\right);$

式(7)中，Q_k-1为过程噪声方差；

步骤2.5、对各高斯项的权值进行进一步预测，具体按照以下算法实施：

对于新生目标的高斯项，其权值预测为：

$w_{k|k-1}^{\left(i\right)}=w_{\mathrm{\γ},k}^{\left(j\right)}---\left(8\right);$

式(8)中，表示k时刻新生目标第j个高斯项的权值；

对于衍生目标的高斯项，其权值预测为：

$w_{k|k-1}^{\left(i\right)}=w_{k-1}^{\left(l\right)}{w}_{\mathrm{\β},k}^{\left(j\right)}---\left(9\right);$

式(9)中，表示k-1时刻的第l个高斯项的权值，表示k时刻由对应的目标衍生出的第j个高斯项的权值；

对于存活目标的高斯项，其权值预测为：

$w_{k|k-1}^{\left(i\right)}=p_S{w}_{k-1}^{\left(j\right)}---\left(10\right);$

式(10)中，p_S表示目标存活概率；

根据上述的算法，进一步预测强度的高斯混合形式为：

${\mathrm{\υ}}_{k|k-1}\left(x_k\right)=\underset{i=1}{\overset{J_{k|k-1}}{\Σ}}{w}_{k|k-1}^{\left(i\right)}N\left(x_k\right|m_{k|k-1}^{\left(i\right)},P_{k|k-1}^{\left(i\right)})---\left(11\right);$

式(11)中，J_k|k-1表示进一步预测的高斯项数；

步骤3、根据步骤2得到的预测结果进行量测更新，得到各高斯分量的估计值：

步骤3.1、量测更新分为两部分：一部分为未检测到的目标强度，另一部分为检测到的目标强度；

若更新未检测到目标强度，则按照以下算法实施：

$w_{k|k}^{\left(i\right)}=(1-(1-e^{-\mathrm{\γ}\left(m_{k|k-1}^{\left(i\right)}\right)}\left)p_D\right)w_{k|k-1}^{\left(i\right)}---\left(12\right);$

式(12)中，表示由目标状态所产生的量测个数的期望，表示至少产生一个量测的概率，p_D为检测率，为有效检测率；

$m_{k|k}^{\left(i\right)}=m_{k|k-1}^{\left(i\right)}---\left(13\right);$

$P_{k|k}^{\left(i\right)}=P_{k|k-1}^{\left(i\right)}---\left(14\right);$

若未检测到的目标后验强度可表示为：

${\mathrm{\υ}}_{k|k}^{ND}\left(x_k\right)=\underset{i=1}{\overset{J_{k|k-1}}{\Σ}}{w}_{k|k}^{\left(i\right)}N\left(x_k\right|m_{k|k}^{\left(i\right)},P_{k|k}^{\left(i\right)});$

若检测到的目标强度，具体按照以下算法实施：

对做Cholesky分解，满足以下关系：

$P_{k|k-1}^{\left(i\right)}=S_{k|k-1}^{\left(i\right)}{\left(S_{k|k-1}^{\left(i\right)}\right)}^T;$

则新的容积点为：

$x_{l,k|k-1}^{\left(i\right)}=S_{k|k-1}^{\left(i\right)}{\mathrm{\ξ}}_l+m_{k|k-1}^{\left(i\right)}---\left(15\right);$

式(15)中，标准容积点ξ_l标号l＝1,2,…,m，而m等于状态向量维数的2倍；

步骤3.2、利用量测函数计算出传递的容积点，具体按照以下算法实施：

$z_{l,k|k-1}^{\left(i\right)}=h\left(x_{l,k|k-1}^{\left(i\right)}\right)---\left(16\right);$

式(16)中，h(·)为目标的量测函数；

步骤3.3、对步骤3.2得到的进行扩维，具体按照以下算法实施：

${\stackrel{\‾}{zz}}_{l,k|k-1}^{\left(i\right)}=repeat(z_{l,k|k-1}^{\left(i\right)},absW,1)---\left(17\right);$

式(17)中，absW表示当前时刻的每个量测划分里的元胞W所包含的量测的个数，其中，当前时刻的每个量测划分里的元胞W为已知量，repeat(A,m,n)函数表示创建一个m×n矩阵，矩阵中的每个值都为A；

步骤3.4、估计量测预测，具体按照以下算法实施：

${\hat{z}}_{k|k-1}^{\left(i\right)}=\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{z}_{l,k|k-1}^{\left(i\right)}---\left(18\right);$

${\stackrel{\‾}{zz}}_{k|k-1}^{\left(i\right)}=\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{\stackrel{\‾}{zz}}_{l,k|k-1}^{\left(i\right)}---\left(19\right);$

步骤3.5、更新积分点的状态和协方差，具体按照以下算法实施：

$m_{k|k}^{\left(i\right)}=m_{k|k-1}^{\left(i\right)}+K_k^{\left(i\right)}({\[z_1,...,z_{\left|W\right|}\]}^T-{\stackrel{\‾}{zz}}_{k|k-1}^{\left(i\right)})---\left(20\right);$

$P_{k|k}^{\left(i\right)}=P_{k|k-1}^{\left(i\right)}-K_k^{\left(i\right)}{P}_{zz}^{\left(i\right)}{\left(K_k^{\left(i\right)}\right)}^T---\left(21\right);$

式(20)中，[z₁,…,z_|W|]^T为相应元胞W中的真实量测值，为滤波增益，具体按照以下算法实施：

$K_k^{\left(i\right)}=P_{xz}^{\left(i\right)}{\left(P_{zz}^{\left(i\right)}\right)}^{-1}---\left(22\right);$

$P_{zz}^{\left(i\right)}={\stackrel{\‾}{R}}_k+\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{\stackrel{\‾}{zz}}_{l,k|k-1}^{\left(i\right)}{\left({\stackrel{\‾}{zz}}_{l,k|k-1}^{\left(i\right)}\right)}^T-{\stackrel{\‾}{zz}}_{k|k-1}^{\left(i\right)}{\left({\stackrel{\‾}{zz}}_{k|k-1}^{\left(i\right)}\right)}^T---\left(23\right);$

$P_{xz}^{\left(i\right)}=\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{x}_{l,k|k-1}^{\left(i\right)}{\left({\stackrel{\‾}{zz}}_{l,k|k-1}^{\left(i\right)}\right)}^T-m_{k|k-1}^{\left(i\right)}{\left({\stackrel{\‾}{zz}}_{k|k-1}^{\left(i\right)}\right)}^T---\left(24\right);$

式(23)中，表示扩维后的量测噪声协方差矩阵：

式(25)中，blkdiag(.)表示块对角矩阵，|W_k|表示当前元胞W_k的所包含的量测值的个数；

步骤3.6、更新相应的权值：

$w_{k|k}^{\left(i\right)}=w_p\frac{\mathrm{\Γ}\left(i\right)p_D}{dW}{\mathrm{\Φ}}_W^{\left(i\right)}{w}_{k|k-1}^{\left(i\right)}---\left(26\right);$

式(26)中，w_p是划分的权重，且满足：

$w_p=\frac{{\Π}_{W\∈p}dW}{{\Σ}_{p^{\′}\∠Z^{\′}}{\Π}_{W^{\′}\∈p^{\′}}{\mathrm{dW}}^{\′}}---\left(27\right);$

式(27)中，p和p'均表示对量测集Z的划分，W和W'分别表示p和p'中的一个子集元胞，采用右上标“'”来对同一公式中分子分母出现的具有相同含义的符号进行区分；

Γ(i)＝e^-γ(i)(γ(i))^|W| (28)；

式(28)中，|W|表示集合的势；

$dW={\mathrm{\δ}}_{\left|W\right|,1}+\underset{i=1}{\overset{J_{k|k-1}}{\Σ}}\mathrm{\Γ}\left(i\right)p_D{\mathrm{\Φ}}_W^{\left(i\right)}{w}_{k|k-1}^{\left(i\right)}---\left(29\right);$

式(29)中，δ_|W|,1表示若|W|＝1，则δ_|W|,1＝1，否则，δ_|W|,1＝0；

${\mathrm{\Φ}}_W^{\left(i\right)}=\underset{z\∈W}{\Π}\frac{{\mathrm{\φ}}_z\left(m_{k|k-1}^{\left(i\right)}\right)}{{\mathrm{\λ}}_k{c}_k\left(z\right)}---\left(30\right);$

式(30)中，表示在元胞W中的量测值概率，λ_kc_k(z)表示k时刻杂波的强度，c_k(z)表示杂波概率密度；λ_k表示杂波的平均数，通常设定每个时刻杂波出现的个数服从Poisson分布：

${\mathrm{\φ}}_z\left(m_{k|k-1}^{\left(i\right)}\right)=N\left(z_k\right|{\hat{z}}_{k|k-1}^{\left(i\right)},R_k+\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{z}_{l,k|k-1}^{\left(i\right)}{\left(z_{l,k|k-1}^{\left(i\right)}\right)}^T-{\hat{z}}_{k|k-1}^{\left(i\right)}{\left({\hat{z}}_{k|k-1}^{\left(i\right)}\right)}^T)---\left(31\right);$

量测更新后的后验强度为如下的高斯混合形式：

$\begin{array}{c}{\mathrm{\υ}}_{k|k}\left(x_k\right)={\mathrm{\υ}}_{k|k}^{ND}\left(x_k\right)+\underset{p_k\∠Z_k}{\Σ}\underset{W_k\∠p_k}{\Σ}{\mathrm{\υ}}_{k|k}^D(x_k,W_k)\\ =\underset{i=1}{\overset{J_k}{\Σ}}{w}_{k|k}^{\left(i\right)}N\left(x_k\right|m_{k|k}^{\left(i\right)},P_{k|k}^{\left(i\right)})\end{array}---\left(32\right);$

式(32)中，J_k表示k时刻的高斯项数，由此得到了各高斯分量的

本发明的方法结合了容积卡尔曼滤波与GM-EPHD算法，是一种新的适用于非线性系统下的CK-EPHD滤波方法。

由于高斯项数目会随着跟踪步数的增加而急速增加，计算量不断增大。为了降低计算量，可以通过控制每一步的高斯项数目解决该问题。合理控制高斯项数目的典型方法如文献“Ba-Ngu Vo,Wing-Kin Ma.The Gaussian mixture probability hypothesisdensity filter,IEEE Transactions on Signal Processing,2006,54(11):4091-4104.”中所述的合并与裁剪方法。

以下通过实施例验证本发明的基于容积卡尔曼滤波的扩展目标概率假设密度滤波方法的有效性和实用性：

采用对比实验的形式，在二维空间中分别给出单个扩展目标和多个扩展目标的跟踪实验，具体的仿真条件详见每个实验的描述：

实施例1：单个扩展目标的跟踪实验

预先设定目标的状态转移方程与量测方程分别为：

x_k＝F_kx_k-1+G_kν_k (33)；

$z_k={\left[\begin{array}{cc}\arctan (y_k/x_k)& \sqrt{{x_k}^2+{y_k}^2}\end{array}\right]}^T+w_k---\left(34\right);$

式(33)及式(34)中，F_k是状态转移矩阵，目标的状态向量(x_k,y_k)表示目标的位置，表示目标的速度；

当目标做匀速运动，即采用匀速模型时，

当目标做常数转弯运动，即采用匀速转弯模型时，

状态噪声转移矩阵为：

$G_k=\left[\begin{array}{cc}{T}^2/2& 0\\ T& 0\\ 0& T^2/2\\ 0& T\end{array}\right];$

观测噪声ν_k和量测噪声w_k都是零均值高斯白噪声，其协方差分别为Q_k＝diag([0.5,0.5])，量测噪声标准差σ_θ＝5×(π/180)，σ_r＝8，采样周期T＝1s，转弯速率Ω＝(π/80)rad/s，量测向量z_k包括偏转角和径向距离两个分量。

设定新生目标随机集的PHD为：

γ_k(x)＝0.1N(x；m_γ,P_γ)；

上式中，m_γ＝[250 6 250-15]^T，P_γ＝diag([100 25 100 25])；

杂波均匀分布于观测区域，数目服从均值为λ_k＝10的泊松分布，扩展目标的量测值个数服从均值为γ(i)＝3的泊松分布。

目标的存活概率和检测概率分别为p_S＝0.99和p_D＝0.90，合并门限U_prun＝4，修剪门限T_prun＝1e-5，势误差与状态误差的调节因子c_ospa＝70，最优子模型分配距离取2阶距离p_ospa＝2，最大高斯项数为J_max＝100，整个仿真时间为100s，监视区域为[-π/2,π/2]rad×[0,1500]m。

如图1所示，图中显示了目标的真实轨迹与两种方法的滤波估计结果，由图1可知，使用本发明的CK-EPHD滤波方法不仅能达到EK-EPHD的滤波效果，而且能够去除掉EK-EPHD所不能达到的杂波，例如，在位置(1200，-300)左右有三个杂波点，用EK-EPHD方法不能过滤掉，但本发明的CK-EPHD滤波方法却能达到；如图2所示，是在整个监控区域的目标量测集。

如图3所示，给出了用传统EK-GMPHD滤波方法估计出来的目标个数，从图3中我们可看出传统的滤波方法完全不能准确地估计目标个数，其平均估计目标数约为10，而真实目标数为1。

如图4所示，图4中显示了在整个时间段内目标的真实个数及本发明的CK-EPHD滤波和传统EK-EPHD滤波的估计结果，由图中可知，本发明的CK-EPHD的估计结果与传统EK-EPHD的基本一致，在54时刻这两种滤波方法的估计结果都出现了误差，但本发明的CK-EPHD滤波方法在下一时刻就能够正确的估计出结果，而EK-EPHD依旧估计错误。

如图5所示，给出本发明的CK-EPHD滤波方法和传统的EK-EPHD滤波方法对应的OSPA距离，可以从图中看到，两种方法的OSPA距离也基本一致，其中在55时刻左右的CK-EPHD的OSPA距离要比EK-EPHD的低一点；并且由仿真实验可知，在整个仿真步骤里，CK-EPHD方法的平均OSPA距离要比EK-EPHD的略小些。

实施例2

多个扩展目标的跟踪

目标的状态转移方程和量测方程与实施例1相同，参数设置也与其相同；

设定在整个仿真区域有3个目标，目标1在k＝1时刻出现，消亡于k＝100时刻；目标2在k＝11时刻出现并消亡于k＝100时刻；目标3在k＝66时刻出现并消亡于k＝100时刻；目标1和目标2均做匀速运动，目标3做转弯运动；

设新生目标随机集的PHD为：

${\mathrm{\γ}}_k\left(x\right)=0.1N(x;m_{\mathrm{\γ}}^{\left(1\right)},P_{\mathrm{\γ}})+0.1N(x;m_{\mathrm{\γ}}^{\left(2\right)},P_{\mathrm{\γ}})+0.1N(x;m_{\mathrm{\γ}}^{\left(3\right)},P_{\mathrm{\γ}});$

上式中， P_γ＝diag([100 25 100 25]^T)；

杂波均匀分布于整个观测区域，数目服从均值为λ_k＝5的泊松分布，量测个数服从γ(i)＝5的泊松分布，其他各仿真参数与单个扩展目标实验相同。

由图6可知，本发明的CK-EPHD滤波方法能够达到与传统EK-EPHD滤波方法相同的效果。

如图7所示，给出了在监控区域内的量测集。

如图8所示，图中显示了用传统EK-GMPHD滤波方法得到的多个扩展目标估计个数，从图中可以看到，它的滤波效果很差，其估计目标数始终比真实目标数大很多，即过估计。

如图9所示，图中给出了目标的真实个数及本发明的CK-EPHD滤波方法和EK-EPHD滤波方法的估计结果，由图中可知，本发明的CK-EPHD滤波方法得到的估计目标数与传统的EK-EPHD滤波方法的滤波结果几乎完全一致，只在23时刻CK-EPHD估计出了准确的数目，而传统的EK-EPHD滤波方法没有。

如图10所示，图中给出了两种方法的OSPA距离，由图10可知，两种方法的OSPA距离几乎一样，只在少有的几个时刻本发明的CK-EPHD滤波方法的OSPA距离比EK-EPHD的稍低。由仿真实验可知，在整个仿真步骤里，本发明的CK-EPHD滤波方法的平均OSPA距离比传统的EK-EPHD滤波方法的略小些。

将以上两个实施例进行对比，得出以下结果；

不论目标的轨迹是直线运动还是转弯运动，本发明的CK-EPHD方法都能够达到传统EK-EPHD方法的滤波效果，并且其平均OSPA距离都比EK-EPHD的要低一点儿。

Claims (1)

1.基于容积卡尔曼滤波的扩展目标概率假设密度滤波方法，其特征在于，具体按照以下步骤实施：

步骤1、预先设定k-1时刻后验强度的高斯混合形式，得到第i个高斯项的均值和协方差；

步骤1中的高斯混合形式为：

${\mathrm{\υ}}_{k-1}\left(x_{k-1}\right)=\underset{i=1}{\overset{J_{k-1}}{\Σ}}{w}_{k-1}^{\left(i\right)}N\left(x_{k-1}\right|m_{k-1}^{\left(i\right)},P_{k-1}^{\left(i\right)});$

其中：J_k-1表示k-1时刻的高斯项数，分别表示第i个高斯项的权值、均值和协方差，N(:|m,P)表示一个具有均值m、协方差P的高斯密度；

步骤2、对步骤1得到的高斯混合形式中第i个高斯项的权值、均值和协方差进行预测，具体按照以下步骤实施：

步骤2.1、对步骤1中得到的k-1时刻的高斯项方差做Cholesky分解，经Cholesky分解后得到：

$P_{k-1}^{\left(i\right)}=S_{k-1}^{\left(i\right)}{\left(S_{k-1}^{\left(i\right)}\right)}^T---\left(3\right);$

步骤2.2、计算出步骤1中高斯分量的容积点，具体按照以下算法实施：

$x_{l,k-1}^{\left(i\right)}=S_{k-1}^{\left(i\right)}{\mathrm{\ξ}}_l+m_{k-1}^{\left(i\right)}---\left(4\right);$

式中，ξ_l为第l个标准容积点，[1]_l表示第l列，[1]与状态向量的维数有关，当状态向量维数为2时，[1]表示下列点集：标准容积点的标号l＝1,2,…,m，而m等于状态向量维数的2倍；

步骤2.3、根据状态方程计算第i个高斯项的第l个预测容积点(l＝1,2,...,m)，具体按照以下算法实施：

$x_{l,k|k-1}^{\left(i\right)}=f\left(x_{l,k-1}^{\left(i\right)}\right)---\left(5\right);$

式中，表示状态转移函数；

步骤2.4、计算得到存活目标的进一步预测均值和方差，具体按照以下算法实施：

$m_{k|k-1}^{\left(i\right)}=\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{x}_{l,k|k-1}^{\left(i\right)}---\left(6\right);$

$P_{k|k-1}^{\left(i\right)}=\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{x}_{l,k|k-1}^{\left(i\right)}{\left(x_{l,k|k-1}^{\left(i\right)}\right)}^T-m_{k|k-1}^{\left(i\right)}{\left(m_{k|k-1}^{\left(i\right)}\right)}^T+Q_{k-1}---\left(7\right);$

式(7)中，Q_k-1为过程噪声方差；

步骤2.5、对各高斯项的权值进行进一步预测，具体按照以下算法实施：

对于新生目标的高斯项，其权值预测为：

$w_{k|k-1}^{\left(i\right)}=w_{\mathrm{\γ},k}^{\left(j\right)}---\left(8\right);$

式(8)中，表示k时刻新生目标第j个高斯项的权值；

对于衍生目标的高斯项，其权值预测为：

$w_{k|k-1}^{\left(i\right)}=w_{k-1}^{\left(l\right)}{w}_{\mathrm{\β},k}^{\left(j\right)}---\left(9\right);$

式(9)中，表示k-1时刻的第l个高斯项的权值，表示k时刻由对应的目标衍生出的第j个高斯项的权值；

对于存活目标的高斯项，其权值预测为：

$w_{k|k-1}^{\left(i\right)}=p_S{w}_{k-1}^{\left(j\right)}---\left(10\right);$

式(10)中，p_S表示目标存活概率；

根据上述的算法，进一步预测强度的高斯混合形式为：

${\mathrm{\υ}}_{k|k-1}\left(x_k\right)=\underset{i=1}{\overset{J_{k|k-1}}{\mathrm{\Σ}}}{w}_{k|k-1}^{\left(i\right)}N\left(x_k\right|m_{k|k-1}^{\left(i\right)},P_{k|k-1}^{\left(i\right)})---\left(11\right);$

式(11)中，J_k|k-1表示进一步预测的高斯项数；

步骤3、根据步骤2得到的预测结果进行量测更新，得到各高斯分量的估计值，完成滤波，具体按照以下步骤实施：

步骤3.1、量测更新分为两部分：一部分为未检测到的目标强度，另一部分为检测到的目标强度；

若更新未检测到目标强度，则按照以下算法实施：

$w_{k|k}^{\left(i\right)}=(1-(1-e^{-\mathrm{\γ}\left(m_{k|k-1}^{\left(i\right)}\right)}\left)p_D\right)w_{k|k-1}^{\left(i\right)}---\left(12\right);$

式(12)中，表示由目标状态所产生的量测个数的期望，表示至少产生一个量测的概率，p_D为检测率，为有效检测率；

$m_{k|k}^{\left(i\right)}=m_{k|k-1}^{\left(i\right)}---\left(13\right);$

$P_{k|k}^{\left(i\right)}=P_{k|k-1}^{\left(i\right)}---\left(14\right);$

若未检测到的目标后验强度可表示为：

${\mathrm{\υ}}_{k|k}^{ND}\left(x_k\right)=\underset{i=1}{\overset{J_{k|k-1}}{\mathrm{\Σ}}}{w}_{k|k}^{\left(i\right)}N\left(x_k\right|m_{k|k}^{\left(i\right)},P_{k|k}^{\left(i\right)});$

若检测到的目标强度，具体按照以下算法实施：

对做Cholesky分解，满足以下关系：

$P_{k|k-1}^{\left(i\right)}=S_{k|k-1}^{\left(i\right)}{\left(S_{k|k-1}^{\left(i\right)}\right)}^T;$

则新的容积点为：

$x_{l,k|k-1}^{\left(i\right)}=S_{k|k-1}^{\left(i\right)}{\mathrm{\ξ}}_l+m_{k|k-1}^{\left(i\right)}---\left(15\right);$

式(15)中，标准容积点ξ_l标号l＝1,2,…,m，而m等于状态向量维数的2倍；

步骤3.2、利用量测函数计算出传递的容积点，具体按照以下算法实施：

$z_{l,k|k-1}^{\left(i\right)}=h\left(x_{l,k|k-1}^{\left(i\right)}\right)---\left(16\right);$

式(16)中，h(·)为目标的量测函数；

步骤3.3、对步骤3.2得到的进行扩维，具体按照以下算法实施：

${\stackrel{\‾}{zz}}_{l,k|k-1}^{\left(i\right)}=repeat(z_{l,k|k-1}^{\left(i\right)},absW,1)---\left(17\right);$

式(17)中，absW表示当前时刻的每个量测划分里的元胞W所包含的量测的个数，其中，当前时刻的每个量测划分里的元胞W为已知量，repeat(A,m,n)函数表示创建一个m×n矩阵，矩阵中的每个值都为A；

步骤3.4、估计量测预测，具体按照以下算法实施：

${\hat{z}}_{k|k-1}^{\left(i\right)}=\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{z}_{l,k|k-1}^{\left(i\right)}---\left(18\right);$

${\stackrel{\‾}{zz}}_{k|k-1}^{\left(i\right)}=\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{\stackrel{\‾}{zz}}_{l,k|k-1}^{\left(i\right)}---\left(19\right);$

步骤3.5、更新积分点的状态和协方差，具体按照以下算法实施：

$m_{k|k}^{\left(i\right)}=m_{k|k-1}^{\left(i\right)}+K_k^{\left(i\right)}({\[z_1,...,z_{\left|W\right|}\]}^T-{\stackrel{\‾}{zz}}_{k|k-1}^{\left(i\right)})---\left(20\right);$

$P_{k|k}^{\left(i\right)}=P_{k|k-1}^{\left(i\right)}-K_k^{\left(i\right)}{P}_{zz}^{\left(i\right)}{\left(K_k^{\left(i\right)}\right)}^T---\left(21\right);$

式(20)中，[z₁,…,z_|W|]^T为相应元胞W中的真实量测值，为滤波增益，具体按照以下算法实施：

$K_k^{\left(i\right)}=P_{xz}^{\left(i\right)}{\left(P_{zz}^{\left(i\right)}\right)}^{-1}---\left(22\right);$

$P_{zz}^{\left(i\right)}={\stackrel{\‾}{R}}_k+\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{\stackrel{\‾}{zz}}_{l,k|k-1}^{\left(i\right)}{\left({\stackrel{\‾}{zz}}_{l,k|k-1}^{\left(i\right)}\right)}^T-{\stackrel{\‾}{zz}}_{k|k-1}^{\left(i\right)}{\left({\stackrel{\‾}{zz}}_{k|k-1}^{\left(i\right)}\right)}^T---\left(23\right);$

$P_{xz}^{\left(i\right)}=\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{x}_{l,k|k-1}^{\left(i\right)}{\left({\stackrel{\‾}{zz}}_{l,k|k-1}^{\left(i\right)}\right)}^T-m_{k|k-1}^{\left(i\right)}{\left({\stackrel{\‾}{zz}}_{k|k-1}^{\left(i\right)}\right)}^T---\left(24\right);$

式(23)中，表示扩维后的量测噪声协方差矩阵：

式(25)中，blkdiag(.)表示块对角矩阵，|W_k|表示当前元胞W_k的所包含的量测值的个数；

步骤3.6、更新相应的权值：

$w_{k|k}^{\left(i\right)}=w_p\frac{\mathrm{\Γ}\left(i\right)p_D}{dW}{\mathrm{\Φ}}_W^{\left(i\right)}{w}_{k|k-1}^{\left(i\right)}---\left(26\right);$

式(26)中，w_p是划分的权重，且满足：

$w_p=\frac{{\mathrm{\Π}}_{W\∈p}dW}{{\mathrm{\Σ}}_{p^{\′}\∠Z^{\′}}{\mathrm{\Π}}_{W^{\′}\∈p^{\′}}{\mathrm{dW}}^{\′}}---\left(27\right);$

式(27)中，p和p'均表示对量测集Z的划分，W和W'分别表示p和p'中的一个子集元胞；

Γ(i)＝e^-γ(i)(γ(i))^|W| (28)；

式(28)中，|W|表示集合的势；

$dW={\mathrm{\δ}}_{\left|W\right|,1}+\underset{i=1}{\overset{J_{k|k-1}}{\Σ}}\mathrm{\Γ}\left(i\right)p_D{\mathrm{\Φ}}_W^{\left(i\right)}{w}_{k|k-1}^{\left(i\right)}---\left(29\right);$

式(29)中，δ_|W|,1表示若|W|＝1，则δ_|W|,1＝1，否则，δ_|W|,1＝0；

${\mathrm{\Φ}}_W^{\left(i\right)}=\underset{z\∈W}{\Π}\frac{{\mathrm{\φ}}_z\left(m_{k|k-1}^{\left(i\right)}\right)}{{\mathrm{\λ}}_k{c}_k\left(z\right)}---\left(30\right);$

式(30)中，表示在元胞W中的量测值概率，λ_kc_k(z)表示k时刻杂波的强度，c_k(z)表示杂波概率密度；λ_k表示杂波的平均数，通常设定每个时刻杂波出现的个数服从Poisson分布：

${\mathrm{\φ}}_z\left(m_{k|k-1}^{\left(i\right)}\right)=N\left(z_k\right|{\hat{z}}_{k|k-1}^{\left(i\right)},R_k+\frac{1}{m}\underset{l=1}{\overset{m}{\Σ}}{z}_{l,k|k-1}^{\left(i\right)}{\left(z_{l,k|k-1}^{\left(i\right)}\right)}^T-{\hat{z}}_{k|k-1}^{\left(i\right)}{\left({\hat{z}}_{k|k-1}^{\left(i\right)}\right)}^T)---\left(31\right);$

量测更新后的后验强度为如下的高斯混合形式：

$\begin{array}{c}{\mathrm{\υ}}_{k|k}\left(x_k\right)={\mathrm{\υ}}_{k|k}^{ND}\left(x_k\right)+\underset{p_k\∠Z_k}{\mathrm{\Σ}}\underset{W_k\∠p_k}{\mathrm{\Σ}}{\mathrm{\υ}}_{k|k}^D\left(x_k,W_k\right)\\ =\underset{i=1}{\overset{J_k}{\mathrm{\Σ}}}{w}_{k|k}^{\left(i\right)}N\left(x_k|m_{k|k}^{\left(i\right)},P_{k|k}^{\left(i\right)}\right)\end{array}---\left(32\right);$

式(32)中，J_k表示k时刻的高斯项数，由此得到了各高斯分量的