[发明专利]基于容积卡尔曼滤波的扩展目标概率假设密度滤波方法

摘要

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

主权项

基于容积卡尔曼滤波的扩展目标概率假设密度滤波方法，其特征在于，具体按照以下步骤实施：步骤1、预先设定k‑1时刻后验强度的高斯混合形式，得到第i个高斯项的均值和协方差；步骤1中的高斯混合形式为：υk-1(xk-1)=Σi=1Jk-1wk-1(i)N(xk-1|mk-1(i),Pk-1(i));]]>其中：Jk‑1表示k‑1时刻的高斯项数，分别表示第i个高斯项的权值、均值和协方差，N(:|m,P)表示一个具有均值m、协方差P的高斯密度；步骤2、对步骤1得到的高斯混合形式中第i个高斯项的权值、均值和协方差进行预测，具体按照以下步骤实施：步骤2.1、对步骤1中得到的k‑1时刻的高斯项方差做Cholesky分解，经Cholesky分解后得到：Pk-1(i)=Sk-1(i)(Sk-1(i))T---(3);]]>步骤2.2、计算出步骤1中高斯分量的容积点，具体按照以下算法实施：xl,k-1(i)=Sk-1(i)ξl+mk-1(i)---(4);]]>式中，ξl为第l个标准容积点，[1]l表示第l列，[1]与状态向量的维数有关，当状态向量维数为2时，[1]表示下列点集：标准容积点的标号l＝1,2,…,m，而m等于状态向量维数的2倍；步骤2.3、根据状态方程计算第i个高斯项的第l个预测容积点(l＝1,2,...,m)，具体按照以下算法实施：xl,k|k-1(i)=f(xl,k-1(i))---(5);]]>式中，表示状态转移函数；步骤2.4、计算得到存活目标的进一步预测均值和方差，具体按照以下算法实施：mk|k-1(i)=1mΣl=1mxl,k|k-1(i)---(6);]]>Pk|k-1(i)=1mΣl=1mxl,k|k-1(i)(xl,k|k-1(i))T-mk|k-1(i)(mk|k-1(i))T+Qk-1---(7);]]>式(7)中，Qk‑1为过程噪声方差；步骤2.5、对各高斯项的权值进行进一步预测，具体按照以下算法实施：对于新生目标的高斯项，其权值预测为：wk|k-1(i)=wγ,k(j)---(8);]]>式(8)中，表示k时刻新生目标第j个高斯项的权值；对于衍生目标的高斯项，其权值预测为：wk|k-1(i)=wk-1(l)wβ,k(j)---(9);]]>式(9)中，表示k‑1时刻的第l个高斯项的权值，表示k时刻由对应的目标衍生出的第j个高斯项的权值；对于存活目标的高斯项，其权值预测为：wk|k-1(i)=pSwk-1(j)---(10);]]>式(10)中，pS表示目标存活概率；根据上述的算法，进一步预测强度的高斯混合形式为：υk|k-1(xk)=Σi=1Jk|k-1wk|k-1(i)N(xk|mk|k-1(i),Pk|k-1(i))---(11);]]>式(11)中，Jk|k‑1表示进一步预测的高斯项数；步骤3、根据步骤2得到的预测结果进行量测更新，得到各高斯分量的估计值，完成滤波，具体按照以下步骤实施：步骤3.1、量测更新分为两部分：一部分为未检测到的目标强度，另一部分为检测到的目标强度；若更新未检测到目标强度，则按照以下算法实施：wk|k(i)=(1-(1-e-γ(mk|k-1(i)))pD)wk|k-1(i)---(12);]]>式(12)中，表示由目标状态所产生的量测个数的期望，表示至少产生一个量测的概率，pD为检测率，为有效检测率；mk|k(i)=mk|k-1(i)---(13);]]>Pk|k(i)=Pk|k-1(i)---(14);]]>若未检测到的目标后验强度可表示为：υk|kND(xk)=Σi=1Jk|k-1wk|k(i)N(xk|mk|k(i),Pk|k(i));]]>若检测到的目标强度，具体按照以下算法实施：对做Cholesky分解，满足以下关系：Pk|k-1(i)=Sk|k-1(i)(Sk|k-1(i))T;]]>则新的容积点为：xl,k|k-1(i)=Sk|k-1(i)ξl+mk|k-1(i)---(15);]]>式(15)中，标准容积点ξl标号l＝1,2,…,m，而m等于状态向量维数的2倍；步骤3.2、利用量测函数计算出传递的容积点，具体按照以下算法实施：zl,k|k-1(i)=h(xl,k|k-1(i))---(16);]]>式(16)中，h(·)为目标的量测函数；步骤3.3、对步骤3.2得到的进行扩维，具体按照以下算法实施：zz‾l,k|k-1(i)=repeat(zl,k|k-1(i),absW,1)---(17);]]>式(17)中，absW表示当前时刻的每个量测划分里的元胞W所包含的量测的个数，其中，当前时刻的每个量测划分里的元胞W为已知量，repeat(A,m,n)函数表示创建一个m×n矩阵，矩阵中的每个值都为A；步骤3.4、估计量测预测，具体按照以下算法实施：z^k|k-1(i)=1mΣl=1mzl,k|k-1(i)---(18);]]>zz‾k|k-1(i)=1mΣl=1mzz‾l,k|k-1(i)---(19);]]>步骤3.5、更新积分点的状态和协方差，具体按照以下算法实施：mk|k(i)=mk|k-1(i)+Kk(i)([z1,...,z|W|]T-zz‾k|k-1(i))---(20);]]>Pk|k(i)=Pk|k-1(i)-Kk(i)Pzz(i)(Kk(i))T---(21);]]>式(20)中，[z1,…,z|W|]T为相应元胞W中的真实量测值，为滤波增益，具体按照以下算法实施：Kk(i)=Pxz(i)(Pzz(i))-1---(22);]]>Pzz(i)=R‾k+1mΣl=1mzz‾l,k|k-1(i)(zz‾l,k|k-1(i))T-zz‾k|k-1(i)(zz‾k|k-1(i))T---(23);]]>Pxz(i)=1mΣl=1mxl,k|k-1(i)(zz‾l,k|k-1(i))T-mk|k-1(i)(zz‾k|k-1(i))T---(24);]]>式(23)中，表示扩维后的量测噪声协方差矩阵：式(25)中，blkdiag(.)表示块对角矩阵，|Wk|表示当前元胞Wk的所包含的量测值的个数；步骤3.6、更新相应的权值：wk|k(i)=wpΓ(i)pDdWΦW(i)wk|k-1(i)---(26);]]>式(26)中，wp是划分的权重，且满足：wp=ΠW∈pdWΣp′∠Z′ΠW′∈p′dW′---(27);]]>式(27)中，p和p'均表示对量测集Z的划分，W和W'分别表示p和p'中的一个子集元胞；Γ(i)＝e‑γ(i)(γ(i))|W|   (28)；式(28)中，|W|表示集合的势；dW=δ|W|,1+Σi=1Jk|k-1Γ(i)pDΦW(i)wk|k-1(i)---(29);]]>式(29)中，δ|W|,1表示若|W|＝1，则δ|W|,1＝1，否则，δ|W|,1＝0；ΦW(i)=Πz∈Wφz(mk|k-1(i))λkck(z)---(30);]]>式(30)中，表示在元胞W中的量测值概率，λkck(z)表示k时刻杂波的强度，ck(z)表示杂波概率密度；λk表示杂波的平均数，通常设定每个时刻杂波出现的个数服从Poisson分布：φz(mk|k-1(i))=N(zk|z^k|k-1(i),Rk+1mΣl=1mzl,k|k-1(i)(zl,k|k-1(i))T-z^k|k-1(i)(z^k|k-1(i))T)---(31);]]>量测更新后的后验强度为如下的高斯混合形式：υk|k(xk)=υk|kND(xk)+Σpk∠ZkΣWk∠pkυk|kD(xk,Wk)=Σi=1Jkwk|k(i)N(xk|mk|k(i),Pk|k(i))---(32);]]>式(32)中，Jk表示k时刻的高斯项数，由此得到了各高斯分量的