Einige Inhalte dieser Anwendung sind momentan nicht verfügbar.
Wenn diese Situation weiterhin besteht, kontaktieren Sie uns bitte unterFeedback&Kontakt
1. (CN103729637) Extended target probability hypothesis density filtering method based on cubature Kalman filtering
Anmerkung: Text basiert auf automatischer optischer Zeichenerkennung (OCR). Verwenden Sie bitte aus rechtlichen Gründen die PDF-Version.
基于容积卡尔曼滤波的扩展目标概率假设密度滤波方法


技术领域
本发明属于目标跟踪方法技术领域,涉及一种扩展目标概率假设密度滤 波方法,具体涉及一种基于容积卡尔曼滤波的扩展目标概率假设密度滤波方 法。
背景技术
目标跟踪就是结合传感器接收到的量测,对目标当前状态进行估计的过 程。传统的多目标跟踪方法,如:联合数据关联、多假设跟踪等都是基于数 据关联的,随着目标数目的增加,这些跟踪方法会存在“组合爆炸”、计算 量呈指数级增长的问题;而近年来提出的基于随机有限集的概率假设密度 (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-Kin Ma.The Gaussian mixture  probability hypothesis density filter,IEEE Transactions 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 for extended target  tracking.Proceedings of the13th International Conference on Infermation 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中的高斯混合形式为:
其中:J k-1 表示k-1时刻的高斯项数,分别表示第i个高 斯项的权值、均值和协方差,N(:|m,P)表示一个具有均值m、协方差P的高 斯密度。
步骤2具体按照以下步骤实施:
步骤2.1、对步骤1中得到的k-1时刻的高斯项方差做Cholesky 分解,经Cholesky分解后得到:
步骤2.2、计算出步骤1中高斯分量的容积点,具体按照以下 算法实施:
式中,ξ l 为第l个标准容积点,[1] l 表示第l列,[1]与状态向 量的维数有关,当状态向量维数为2时,[1]表示下列点集: 标准容积点的标号l=1,2,…,m,而m等于状态向量维数 的2倍;
步骤2.3、根据状态方程计算第i个高斯项的第l个预测容积点 (l=1,2,...,m),具体按照以下算法实施:
步骤2.4、计算得到存活目标的进一步预测均值和方差,具体按照以下 算法实施:
式(7)中,Q k-1 为过程噪声方差;
步骤2.5、对各高斯项的权值进行进一步预测,具体按照以下算法实施:
对于新生目标的高斯项,其权值预测为:
式(8)中,表示k时刻新生目标第j个高斯项的权值;
对于衍生目标的高斯项,其权值预测为:
式(9)中,表示k-1时刻的第l个高斯项的权值,表示k时刻 由对应的目标衍生出的第j个高斯项的权值;
对于存活目标的高斯项,其权值预测为:
式(10)中,p S 表示目标存活概率;
根据上述的算法,进一步预测强度的高斯混合形式为:
式(11)中,J k|k-1 表示一步预测的高斯项数。
步骤3具体按照以下步骤实施:
步骤3.1、量测更新分为两部分:一部分为未检测到的目标强度,另一 部分为检测到的目标强度;
若更新未检测到目标强度,则按照以下算法实施:
式(12)中,表示由目标状态所产生的量测个数的期望, 表示至少产生一个量测的概率,p D 为检测率,为 有效检测率;
若未检测到的目标后验强度可表示为:
若检测到的目标强度,具体按照以下算法实施:
做Cholesky分解,满足以下关系:
则新的容积点为:
式(15)中,标准容积点ξ l 标号l=1,2,…,m,而m等于状态向量维数 的2倍;
步骤3.2、利用量测函数计算出传递的容积点,具体按照以下算法实施:
式(16)中,h(·)为目标的量测函数;
步骤3.3、对步骤3.2得到的进行扩维,具体按照以下算法实施:
式(17)中,absW表示当前时刻的每个量测划分里的元胞W所包含的 量测的个数,其中,当前时刻的每个量测划分里的元胞W为已知量, repeat(A,m,n)函数表示创建一个m×n矩阵,矩阵中的每个值都为A;
步骤3.4、估计量测预测,具体按照以下算法实施:
步骤3.5、更新积分点的状态和协方差,具体按照以下算法实施:
式(20)中,[z 1 ,…,z |W| ]T为相应元胞W中的真实量测值,为滤波增 益,具体按照以下算法实施:
式(23)中,表示扩维后的量测噪声协方差矩阵:
式(25)中,blkdiag(.)表示块对角矩阵,|W k |表示当前元胞W k 的所包含 的量测值的个数;
步骤3.6、更新相应的权值:
式(26)中,w p 是划分的权重,且满足:
式(27)中,p和p'均表示对量测集Z的划分,W和W'分别表示p和p'中 的一个子集元胞;
Г(i)=e-γ(i)(γ(i))|W|  (28);
式(28)中,|W|表示集合的势;
式(29)中,δ |W|,1 表示若|W|=1,则δ |W|,1 =1,否则,δ |W|,1 =0;
式(30)中,表示在元胞W中的量测值概率,λ k c k (z)表示k时 刻杂波的强度,c k (z)表示杂波概率密度;λ k 表示杂波的平均数,通常设定 每个时刻杂波出现的个数服从Poisson分布:
量测更新后的后验强度为如下的高斯混合形式:
式(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 表示量测值的个数。
量测集由真实扩展目标产生的量测和杂波构成,且二者不可区分,在多 个扩展目标的跟踪系统中,扩展目标状态的动态模型为:
其中,ν k 是零均值高斯白噪声,协方差为Q k ,f(.)表示状态转移函数。
量测模型可写为:
其中,w k 是零均值高斯白噪声,协方差为R k ,h(.)表示量测函数。
Granstrom等已证明,若扩展目标PHD的预测能够写为高斯混合形式, 则量测更新后的PHD也为高斯混合形式,进而各个时刻的预测和更新PHD 也都服从高斯混合形式,这一递推实现的过程即为GM-EPHD滤波。这与传 统的点目标GM-PHD滤波方法不同,由于扩展目标在每一时刻都会有多个 量测,所以在滤波估计前必须对量测集进行划分,然后再进行一步预测和量 测更新。
本发明的基于容积卡尔曼滤波的扩展目标概率假设密度滤波方法,具体 按照以下步骤实施:
步骤1、预先设定k-1时刻后验强度的高斯混合形式为:
其中:J k-1 表示k-1时刻的高斯项数,分别表示第i个高 斯项的权值、均值和协方差,N(:|m,P)表示一个具有均值m、协方差P的高 斯密度;
步骤2、对步骤1得到的高斯混合形式中第i个高斯项的权值、均值和协 方差进行预测:
步骤2.1、对步骤1中得到的k-1时刻的高斯项方差做Cholesky 分解(即平方根法),经Cholesky分解后得到:
步骤2.2、计算出步骤1中高斯分量的容积点,具体按照以下 算法实施:
式(4)中,ξ l 为第l个标准容积点,[1] l 表示第l列,[1]与 状态向量的维数有关,例如:当状态向量维数为2时,[1]表示下列点集: 标准容积点的标号l=1,2,…,m,而m等于状态向量维数 的2倍;
步骤2.3、根据状态方程计算第i个高斯项的第l个预测容积点 (l=1,2,...,m),具体按照以下算法实施:
步骤2.4、计算得到存活目标的进一步预测均值和方差,具体按照以下 算法实施:
式(7)中,Q k-1 为过程噪声方差;
步骤2.5、对各高斯项的权值进行进一步预测,具体按照以下算法实施:
对于新生目标的高斯项,其权值预测为:
式(8)中,表示k时刻新生目标第j个高斯项的权值;
对于衍生目标的高斯项,其权值预测为:
式(9)中,表示k-1时刻的第l个高斯项的权值,表示k时刻 由对应的目标衍生出的第j个高斯项的权值;
对于存活目标的高斯项,其权值预测为:
式(10)中,p S 表示目标存活概率;
根据上述的算法,进一步预测强度的高斯混合形式为:
式(11)中,J k|k-1 表示一步预测的高斯项数;
步骤3、根据步骤2得到的预测结果进行量测更新,得到各高斯分量 的估计值:
步骤3.1、量测更新分为两部分:一部分为未检测到的目标强度,另一 部分为检测到的目标强度;
若更新未检测到目标强度,则按照以下算法实施:
式(12)中,表示由目标状态所产生的量测个数的期望, 表示至少产生一个量测的概率,p D 为检测率,为 有效检测率;
若未检测到的目标后验强度可表示为:
若检测到的目标强度,具体按照以下算法实施:
做Cholesky分解,满足以下关系:
则新的容积点为:
式(15)中,标准容积点ξ l 标号l=1,2,…,m,而m等于状态向量维数 的2倍;
步骤3.2、利用量测函数计算出传递的容积点,具体按照以下算法实施:
式(16)中,h(·)为目标的量测函数;
步骤3.3、对步骤3.2得到的进行扩维,具体按照以下算法实施:
式(17)中,absW表示当前时刻的每个量测划分里的元胞W所包含的 量测的个数,其中,当前时刻的每个量测划分里的元胞W为已知量, repeat(A,m,n)函数表示创建一个m×n矩阵,矩阵中的每个值都为A;
步骤3.4、估计量测预测,具体按照以下算法实施:
步骤3.5、更新积分点的状态和协方差,具体按照以下算法实施:
式(20)中,[z 1 ,…,z |W| ]T为相应元胞W中的真实量测值,为滤波增 益,具体按照以下算法实施:
式(23)中,表示扩维后的量测噪声协方差矩阵:
式(25)中,blkdiag(.)表示块对角矩阵,|W k |表示当前元胞W k 的所包含 的量测值的个数;
步骤3.6、更新相应的权值:
式(26)中,w p 是划分的权重,且满足:
式(27)中,p和p'均表示对量测集Z的划分,W和W'分别表示p和p'中 的一个子集元胞,采用右上标“'”来对同一公式中分子分母出现的具有相同含 义的符号进行区分;
Г(i)=e-γ(i)(γ(i))|W|  (28);
式(28)中,|W|表示集合的势;
式(29)中,δ |W|,1 表示若|W|=1,则δ |W|,1 =1,否则,δ |W|,1 =0;
式(30)中,表示在元胞W中的量测值概率,λ k c k (z)表示k时 刻杂波的强度,c k (z)表示杂波概率密度;λ k 表示杂波的平均数,通常设定 每个时刻杂波出现的个数服从Poisson分布:
量测更新后的后验强度为如下的高斯混合形式:
式(32)中,J k 表示k时刻的高斯项数,由此得到了各高斯分量的
本发明的方法结合了容积卡尔曼滤波与GM-EPHD算法,是一种新的适 用于非线性系统下的CK-EPHD滤波方法。
由于高斯项数目会随着跟踪步数的增加而急速增加,计算量不断增大。 为了降低计算量,可以通过控制每一步的高斯项数目解决该问题。合理控制 高斯项数目的典型方法如文献“Ba-Ngu Vo,Wing-Kin Ma.The Gaussian  mixture probability hypothesis density filter,IEEE Transactions on Signal  Processing,2006,54(11):4091-4104.”中所述的合并与裁剪方法。
以下通过实施例验证本发明的基于容积卡尔曼滤波的扩展目标概率假 设密度滤波方法的有效性和实用性:
采用对比实验的形式,在二维空间中分别给出单个扩展目标和多个扩展 目标的跟踪实验,具体的仿真条件详见每个实验的描述:
实施例1:单个扩展目标的跟踪实验
预先设定目标的状态转移方程与量测方程分别为:
x k =F k x k-1 +G k ν k   (33);
式(33)及式(34)中,F k 是状态转移矩阵,目标的状态向量 (x k ,y k )表示目标的位置,表示目标的速度;
当目标做匀速运动,即采用匀速模型时,
当目标做常数转弯运动,即采用匀速转弯模型时,
状态噪声转移矩阵为:
观测噪声ν 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为:
上式中,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的要低一点儿。