CN105205313A - 模糊高斯和粒子滤波方法、装置及目标跟踪方法、装置 - Google Patents

Abstract

本发明公开了一种模糊高斯和粒子滤波方法、装置及目标跟踪方法、装置，该模糊粒子滤波方法包括：利用高斯和方法构建上一目标时刻的状态后验概率密度函数、观测噪声概率密度函数和状态噪声概率密度函数，利用高斯-厄米特积分规则和蒙特卡罗原理获取当前时刻目标状态的预测概率密度函数，利用模糊聚类原理获取当前目标观测时刻的粒子权值和积分点权值，并计算各高斯项的权值，以计算每个高斯分布的均值和协方差，并对高斯项重采样获取G个权值较大的高斯项，然后利用高斯和原理得到当前目标观测时刻的状态后验概率密度函数，完成粒子滤波。通过上述方式，本发明能够有效提高滤波精度以及目标状态的估计性能。

Description

模糊高斯和粒子滤波方法、装置及目标跟踪方法、装置

技术领域

本发明涉及非线性滤波领域，特别是涉及一种模糊高斯和粒子滤波方法、装置及目标跟踪方法、装置。

背景技术

在飞机、航空飞行器、车辆等目标的运动过程中，常常需要对目标的实时状态进行估计以实现对目标的跟踪，飞机等目标的运动***模型一般属于非线性随机***。非线性滤波技术为非线性随机***中进行状态估计的常用手段。

根据应用背景的不同，现有技术非线性滤波技术主要分为两类：第一类是针对非线性高斯环境下的状态估计问题，如扩展卡尔曼滤波器(EKF)、无迹卡尔曼滤波器(UKF)、积分卡尔曼滤波(QKF)、截断无迹卡尔曼滤波(IUKF)，这类方法主要是利用泰勒级数展开或数值计算等线性近似技术对非线性的***模型进行近似，忽略近似高阶项对滤波性能的影响。第二类是针对非线性非高斯环境下的状态估计问题，如高斯和滤波器(GSF)、高斯和积分卡尔曼滤波器(GS-QKF)，这类高斯和方法主要是利用多个混合高斯将状态的后验概率密度函数近似成单个高斯函数，然而，与上述EKF等方法类似，这类高斯和方法都必须进行线性化，对于强非线性非高斯***，此类滤波器的滤波精度并不高，且滤波器的高斯混合项的数量随着时间快速增长。

此外，为了对非线性非高斯噪声进行处理，现有技术还采用另一种非线性滤波方法：高斯和粒子滤波方法，对于大规模被动传感器***中观测信息有限、数据丢失率高、具有非周期、非线性、非高斯特征的观测数据，现有技术所采用的一类高斯和粒子滤波方法由于在时间更新时只是简单地采用状态转移函数进行粒子采样，粒子的多样性以及准确性较差，粒子并不能有效表示目标的后验概率分布，从而降低粒子滤波的性能。

发明内容

本发明主要解决的技术问题是提供一种模糊高斯和粒子滤波方法、装置及目标跟踪方法、装置，能够增强粒子的多样性和准确性，有效提高滤波精度以及目标状态的估计性能。

为解决上述技术问题，本发明的第一方面是：提供一种模糊高斯和粒子滤波方法，包括：利用高斯和构建上一目标观测时刻的状态后验概率密度函数、观测噪声概率密度函数以及状态噪声概率密度函数；根据所述上一目标观测时刻的状态后验概率密度函数和所述状态噪声概率密度函数，利用高斯-厄米特积分和蒙特卡罗原理获取当前目标观测时刻的目标状态预测概率密度函数；根据所述当前目标观测时刻的目标状态预测概率密度函数和所述观测噪声概率密度函数获取状态粒子集；利用模糊聚类原理获取当前目标观测时刻的粒子权值和积分点权值，并计算各高斯项的权值；根据所述状态粒子集和所述粒子权值、所述积分点权值计算每个高斯分布的均值和协方差；根据所述高斯项的权值对高斯项进行重采样，获取G个权值较大的高斯项，G为正整数；以及根据获取的G个高斯项，利用高斯和原理得到当前目标观测时刻的状态后验概率密度函数，完成粒子滤波。

其中，所述根据上一目标时刻的状态后验概率密度函数和状态噪声概率密度函数，利用高斯-厄米特积分规则和蒙特卡罗原理获取当前时刻目标状态的预测概率密度函数的步骤包括：利用高斯-厄米特积分构建上一目标观测时刻的多个积分点概率密度函数；根据所述多个积分点概率密度函数获取所述积分点的近似粒子集；其中，所述积分点的近似粒子集为 ${\left\{x_{(n+1|n)g^{\′}}^{l,i}\right\}}_{i=1}^N,{\left\{x_{\left(n\right|n)g^{\′}}^{l,i}\right\}}_{i=1}^N~N({\hat{x}}_{\left(n\right|n)g^{\′}}^l,P_{\left(n\right|n)g}),i=1,2,......,$ N；获取所述近似粒子集中每个近似粒子的预测粒子集；以及根据所述预测粒子集获取当前目标观测时刻的目标状态预测概率密度函数。

其中，所述利用高斯-厄米特积分构建上一目标观测时刻的多个积分点概率密度函数的步骤具体包括：获取n时刻每个高斯分布的高斯-厄米特积分点的后验概率密度函数，具体如下式所示：

$p\left(x_n\right|y_{0:n})=\underset{g=1}{\overset{G}{\Σ}}{\mathrm{\ω}}_{ng}N(x_n;{\hat{x}}_{\left(n\right|n)g},P_{\left(n\right|n)g})---\left(5\right)$

其中，n时刻表示上一目标观测时刻，为第g个高斯分布函数的均值，P_(n|n)g为第g个高斯分布函数的协方差；

估计n时刻的目标状态后验概率密度函数对应的积分点具体如下式所示：

$x_{\left(n\right|n)g^{\′}}^l=\sqrt{P_{\left(n\right|n)g}}\·{\mathrm{\ξ}}^l+{\hat{x}}_{\left(n\right|n)g}+{\widetilde{\mathrm{\μ}}}_{(n+1)k}---\left(6\right)$

其中，g’＝g+(k-1)K；g＝1，2，……，G；k＝1，2，……，K；l＝1，2，……，m；ξ_l为高斯-厄米特积分点；

以n时刻的目标状态后验概率密度函数对应的积分点为均值，P_(n|n)g为协方差，构建n时刻的多个积分点概率密度函数

其中，公式(5)、(6)中的非高斯过程噪声和量测噪声的概率密度函数利用高斯和方法近似得到如下式所示：

$p\left(u_n\right)=\underset{k=1}{\overset{K}{\Σ}}{\mathrm{\α}}_{k}N(u_n;{\widetilde{\mathrm{\μ}}}_{nk},{\widetilde{\mathrm{\Σ}}}_{nk})---\left(7\right)$

$p\left(v_n\right)=\underset{j=1}{\overset{L}{\Σ}}{\mathrm{\β}}_{j}N(v_n;{\widetilde{\mathrm{\μ}}}_{nj},{\widetilde{\mathrm{\Σ}}}_{nj})---\left(8\right)$

其中，α_k和β_j均为非负常数，且满足和 和分别表示第k个高斯概率密度函数的均值和协方差。

其中，所述根据预测粒子集获取当前目标观测时刻的目标状态预测概率密度函数的步骤具体包括：根据所述预测粒子集获取n时刻的目标状态后验概率密度函数对应的积分点的均值和协方差具体如下式所示：

${\hat{x}}_{(n+1|n)g^{\′}}^l=\frac{1}{N}\underset{i=1}{\overset{N}{\Σ}}{x}_{(n+1|n)g^{\′}}^{l,i}---\left(9\right)$

$P_{(n+1|n)g^{\′}}^l=\frac{1}{N}\underset{i=1}{\overset{N}{\Σ}}(x_{(n+1|n)g^{\′}}^{l,i}-{\hat{x}}_{(n+1|n)g^{\′}}^l){(x_{(n+1|n)g^{\′}}^{l,i}-{\hat{x}}_{(n+1|n)g^{\′}}^l)}^T---\left(10\right)$

根据所述积分点的均值和协方差获取n+1时刻的目标状态第g’个高斯概率密度函数的均值和协方差P_(n+1|n)g'，具体如下式所示：

${\hat{x}}_{(n+1|n)g^{\′}}=\underset{l=1}{\overset{m}{\Σ}}{\mathrm{\ω}}_l{\hat{x}}_{(n+1|n)g^{\′}}^l---\left(11\right)$

$P_{(n+1|n)g^{\′}}=\underset{l=1}{\overset{m}{\Σ}}{\mathrm{\ω}}_l{P}_{(n+1|n)g^{\′}}^l---\left(12\right)$

计算每个高斯概率密度函数的权值，具体如下式所示：

将G'个高斯概率密度函数的加权和近似为目标状态的预测概率密度函数，具体如下式所示：

其中，利用模糊聚类原理获取当前目标观测时刻的粒子权值和积分点权值，并计算各高斯项的权值的步骤包括：根据所述积分点构建n+1时刻的多个积分点的重要性函数根据所述n+1时刻的多个积分点的重要性函数获取积分点的近似粒子集具体如下式所示：

${\left\{x_{(n+1|n)g^{\′},j}^{l,i}\right\}}_{i=1}^N~N({\hat{x}}_{(n+1|n)g^{\′},j}^l,P_{(n+1|n)g^{\′}})---\left(21\right)$

其中，l＝1，2，……，m；g’＝1，2，……，G’；j＝1，2，……L；i＝1，2，……，N；

计算所述近似粒子集中每个粒子的权值具体如下式所示：

${\mathrm{\μ}}_{g^{\′},j}^{l,i}=\frac{1}{\underset{k=1}{\overset{N_s}{\Σ}}{\left(\frac{d_{li}}{d_{lk}}\right)}^{\frac{2}{m1-1}}}=\frac{p_l\left(y_{n+1}\right|x_{(n+1|n)g^{\′},j}^{l,i})}{\underset{k=1}{\overset{N_s}{\Σ}}{p}_l\left(y_{n+1}\right|x_{(n+1|n)g^{\′},j}^{l,k})}---\left(22\right)$

其中，为根据拉格朗日乘子法最小化目标函数而得到的y_n+1与粒子的模糊隶属度，y_n+1为当前目标观测时刻n+1时刻的目标观测。

其中，根据所述状态粒子集和所述粒子权值、所述积分点权值计算每个高斯分布的均值和协方差的步骤包括：根据所述近似粒子集和权值获取积分点的均值和协方差具体如下式所示：

根据所述积分点的权值获取高斯混合项的概率密度函数的均值和协方差以得到相应的权值，具体如下式所示：

其中， ${\hat{x}}_{(n+1|n+1)g^{\′},j}=\underset{l=1}{\overset{m}{\Σ}}{\mathrm{\ω}}_{g^{\′},j}^l{\hat{x}}_{(n+1|n+1)g^{\′},j}^l,$ $P_{(n+1|n+1)g^{\′},j}=\underset{l=1}{\overset{m}{\Σ}}{\mathrm{\ω}}_{g^{\′},j}^l{P}_{(n+1|n+1)g^{\′},j}^l.$

其中，根据所述高斯项的权值对高斯项进行重采样，获取G个权值较大的高斯项的步骤包括：将计算得到的G*K*L个高斯项的权值按照降序排列以保留前G个高斯项和权值其中，g＝1,…,G；当时，设定 ${\hat{x}}_{(n+1|n+1)g}={\hat{x}}_{(n+1|n+1)j},P_{(n+1|n+1)g}=P_{(n+1|n+1)j},$ 其中，j为每次循环时随机提取，且j∈{1,…,G}，提取到j的概率正比于标准化权值g＝1,…,G，G为循环次数且以及获取G个权值较大的高斯项。

其中，根据获取的G个高斯项，利用高斯和原理得到当前目标观测时刻的状态后验概率密度函数，完成粒子滤波的步骤具体为：利用高斯和原理，根据获取的G个高斯项，当前目标观测时刻的目标状态后验概率密度函数p(x_n+1|y_0:n+1)可以近似成均值为协方差为P_n+1|n+1的高斯分布，具体如下式所示：

为解决上述技术问题，本发明的第二方面是：提供一种模糊高斯和粒子滤波装置，包括：积分点概率密度函数构建模块，用于利用高斯和构建上一目标观测时刻的状态后验概率密度函数、观测噪声概率密度函数以及状态噪声概率密度函数；目标状态预测概率密度函数获取模块，用于根据所述上一目标观测时刻的状态后验概率密度函数和所述状态噪声概率密度函数，利用高斯-厄米特积分和蒙特卡罗原理获取当前目标观测时刻的目标状态预测概率密度函数；状态粒子集获取模块，用于根据所述当前目标观测时刻的目标状态预测概率密度函数和所述观测噪声概率密度函数获取状态粒子集；高斯项权值计算模块，用于利用模糊聚类原理获取当前目标观测时刻的粒子权值和积分点权值，并计算各高斯项的权值；高斯分布计算模块，用于根据所述状态粒子集和所述粒子权值、所述积分点权值计算每个高斯分布的均值和协方差；重采样模块，用于根据所述高斯项的权值对高斯项进行重采样，获取G个权值较大的高斯项，G为正整数；以及概率密度函数获取模块，用于根据获取的G个高斯项，利用高斯和原理得到当前目标观测时刻的状态后验概率密度函数，完成粒子滤波。

为解决上述技术问题，本发明的第三方面是：提供一种目标跟踪方法，包括：接收当前目标观测时刻以及当前目标观测时刻之前所观测的目标状态；利用高斯和构建上一目标观测时刻的状态后验概率密度函数、观测噪声概率密度函数以及状态噪声概率密度函数；根据所述上一目标观测时刻的状态后验概率密度函数和所述状态噪声概率密度函数，利用高斯-厄米特积分和蒙特卡罗原理获取当前目标观测时刻的目标状态预测概率密度函数；根据所述当前目标观测时刻的目标状态预测概率密度函数和所述观测噪声概率密度函数获取状态粒子集；利用模糊聚类原理获取当前目标观测时刻的粒子权值和积分点权值，并计算各高斯项的权值；根据所述状态粒子集和所述粒子权值、所述积分点权值计算每个高斯分布的均值和协方差；根据所述高斯项的权值对高斯项进行重采样，获取G个权值较大的高斯项，G为正整数；根据获取的G个高斯项，利用高斯和原理得到当前目标观测时刻的状态后验概率密度函数；利用所述当前目标观测时刻的目标状态后验概率密度函数对目标状态进行估计，以获得当前目标观测时刻的目标状态估计值；输出所述当前目标观测时刻的目标状态估计值，以实现对目标的跟踪。

为解决上述技术问题，本发明的第四方面是：提供一种目标跟踪装置，包括：观测数据接收模块，用于接收当前目标观测时刻以及当前目标观测时刻之前所观测的目标状态；积分点概率密度函数构建模块，用于利用高斯和构建上一目标观测时刻的状态后验概率密度函数、观测噪声概率密度函数以及状态噪声概率密度函数；目标状态预测概率密度函数获取模块，用于根据所述上一目标观测时刻的状态后验概率密度函数和所述状态噪声概率密度函数，利用高斯-厄米特积分和蒙特卡罗原理获取当前目标观测时刻的目标状态预测概率密度函数；状态粒子集获取模块，用于根据所述当前目标观测时刻的目标状态预测概率密度函数和所述观测噪声概率密度函数获取状态粒子集；高斯项权值计算模块，用于利用模糊聚类原理获取当前目标观测时刻的粒子权值和积分点权值，并计算各高斯项的权值；高斯分布计算模块，用于根据所述状态粒子集和所述粒子权值、所述积分点权值计算每个高斯分布的均值和协方差；重采样模块，用于根据所述高斯项的权值对高斯项进行重采样，获取G个权值较大的高斯项，G为正整数；概率密度函数获取模块，用于根据获取的G个高斯项，利用高斯和原理得到当前目标观测时刻的状态后验概率密度函数；目标状态估计模块，用于利用所述当前目标观测时刻的目标状态后验概率密度函数对目标状态进行估计，以获得当前目标观测时刻的目标状态估计值；目标状态估计值输出模块，用于输出所述当前目标观测时刻的目标状态估计值，以实现对目标的跟踪。

本发明的有益效果是：区别于现有技术的情况，本发明提供的模糊高斯和粒子滤波方法利用高斯和方法构建上一目标时刻的状态后验概率密度函数、观测噪声概率密度函数和状态噪声概率密度函数，能够增强粒子的多样性；然后上一时刻的状态后验概率密度函数和状态噪声概率密度函数，利用高斯-厄米特积分规则和蒙特卡罗原理获取当前时刻目标状态的预测概率密度函数，进一步获取当前时刻的目标状态粒子集，并利用模糊聚类方法和当前观测获取粒子权值和积分点权值，并计算各高斯项的权值，最后计算每个高斯分布的均值和协方差，并根据高斯项权值，对高斯项进行重采样，以根据获取的G个高斯分布得到当前时刻的状态后验概率密度函数，完成粒子滤波过程。本发明的粒子滤波方法，能够增强粒子的多样性以及粒子权值、积分点权值的准确性，有效提高滤波精度以及目标状态的估计性能，解决非线性非高斯情况下目标状态滤波问题。

附图说明

图1是本发明模糊高斯和粒子滤波方法一实施方式的流程图；

图2是本发明模糊高斯和粒子滤波方法一实施方式中根据上一目标观测时刻的状态后验概率密度函数和状态噪声概率密度函数，利用高斯-厄米特积分和蒙特卡罗原理获取当前目标观测时刻的目标状态预测概率密度函数的流程图；

图3是本发明模糊高斯和粒子滤波方法一实施方式中利用高斯-厄米特积分构建上一目标观测时刻的多个积分点概率密度函数的流程图；

图4是本发明模糊高斯和粒子滤波方法一实施方式中根据预测粒子集获取当前目标观测时刻的目标状态预测概率密度函数的流程图；

图5是本发明模糊高斯和粒子滤波方法一实施方式中利用模糊聚类原理获取当前目标观测时刻的粒子权值和积分点权值，并计算各高斯项的权值的流程图；

图6是本发明模糊高斯和粒子滤波方法一实施方式中根据状态粒子集、粒子权值以及积分点权值计算每个高斯分布的均值和协方差的流程图；

图7是本发明模糊高斯和粒子滤波方法一实施方式中根据高斯项的权值对高斯项进行重采样，获取G个权值较大的高斯项的流程图；

图8是本发明模糊高斯和粒子滤波方法一实施方式中在非均匀稀疏采样环境下，GSQPF、GSPF以及QPF的目标跟踪轨迹和真实轨迹的对比图；

图9是图6中30-50个采样点之间的目标跟踪轨迹放大图；

图10是本发明模糊高斯和粒子滤波方法一实施方式中在非均匀稀疏采样环境下，量测噪声均值为零、协方差为0.15的情况下，GSQPF、GSPF以及QPF的均方根误差对比图；

图11是本发明模糊高斯和粒子滤波方法一实施方式中在非均匀稀疏采样环境下，量测噪声均值为零、协方差为0.5的情况下，GSQPF、GSPF以及QPF的均方根误差对比图；

图12是本发明模糊高斯和粒子滤波装置一实施方式的原理框图；

图13是本发明目标跟踪方法一实施方式的流程图；

图14是本发明目标跟踪装置一实施方式的流程图。

具体实施方式

下面将结合本发明实施方式中的附图，对本发明实施方式中的技术方案进行清楚、完整地描述，显然，所描述的实施方式仅仅是本发明一部分实施方式，而不是全部的实施方式。基于本发明中的实施方式，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施方式，均属于本发明保护的范围。

粒子滤波方法为一种基于序列蒙特卡罗模拟的非线性滤波方法，指通过寻找一组在状态空间传播的随机样本对概率密度函数进行近似，通过粒子滤波方法对飞机、航空飞行器、车辆等目标的实时状态进行估计，实现对目标的跟踪。

请一并参阅图1-7，本发明模糊高斯和粒子滤波方法一实施方式包括以下步骤：

步骤S11，利用高斯和构建上一目标观测时刻的状态后验概率密度函数、观测噪声概率密度函数以及状态噪声概率密度函数。

预先设定动态空间模型为：

x_n＝f(x_n-1)+u_n(1)

y_n＝h(x_n)+v_n(2)

其中，为n时刻的***状态向量，为n时刻的量测值，和分别为过程噪声和观测噪声，并且是相互独立的。

状态估计就是在给定观测数据集合y_1:n的条件下估计状态向量x_n的概率密度函数p(x_n|y_1:n)。假定在n-1时刻概率密度函数p(x_n-1|y_1:n-1)已知，则预测n时刻的先验概率密度函数为：

p(x_n|y_1::n-1)＝∫p(x_n|x_n-1)p(x_n-1|y_1::n-1)dx_n-1(3)

其中，p(x_n|x_n-1)表示状态转移概率密度函数。

在时刻n获得观测值y_n后，根据贝叶斯规则，后验概率密度函数可以定义如下：

$p\left(x_n\right|y_{1::n})=\frac{p\left(y_n\right|x_n)p\left(x_n\right|y_{1::n-1})}{p\left(y_n\right|y_{1::n-1})}---\left(4\right)$

其中，p(y_n|y_1::n-1)可认为是一常数，p(y_n|x_n)表示观测似然函数，可由观测模型(2)和观测噪声v_n获得。公式(3)和公式(4)是最优贝叶斯估计的一般表达式。

步骤S12，根据所述上一目标观测时刻的状态后验概率密度函数和所述状态噪声概率密度函数，利用高斯-厄米特积分和蒙特卡罗原理获取当前目标观测时刻的目标状态预测概率密度函数。

进一步地，步骤S12具体包括以下子步骤：

子步骤S121，利用高斯-厄米特积分构建上一目标观测时刻的多个积分点概率密度函数。

具体地，高斯-厄米特(Gauss-Hermite)积分：

对于一个概率密度函数为的随机变量，根据Gauss-Hermite积分理论，下式所示的函数积分可以近似表示为：

其中，ξ_l为高斯-厄米特积分点，m为高斯-厄米特积分点的数目，w_l为高斯-厄米特积分点ξ_l相应的权值，H_m(ξ_l),l＝1,2,…,m。

进一步地，利用正交多项式与上三角矩阵之间的关系来计算高斯-厄米特积分点ξ_l及其权值w_l，假设J为一个对角元素为0的对角矩阵且

$J_{i,i+1}=\sqrt{i/2},1\≤i\≤(m-1)$

因此，将高斯-厄米特积分点ξ_l设置为其中ε_l为对角矩阵的第l个特征值；权值(v_l)₁为对角矩阵J第l个标准化特征值的第1个元素。

由于矢量中各个元素分量互不相关，因此可以将上面所述的一维积分点的情况推广到多维积分点的情况：假设一个具有高斯密度的随机矢量x，表示n_x×n_x的单位矩阵，多维积分点公式如下式所示：

$\begin{array}{c}E\left(f\right(x\left)\right)=\underset{R^{n_x}}{\∫}f\left(x\right)N(x;0,I_{n_x})dx\\ =\underset{l_{n_x}=1}{\overset{m}{\Σ}}{w}_{l_{n_x}}...\underset{l=1}{\overset{m}{\Σ}}{w}_{l}f({\mathrm{\ξ}}_{l_1},{\mathrm{\ξ}}_{l_2},...,{\mathrm{\ξ}}_{l_{n_x}})\\ =\underset{l=1}{\overset{m^{n_x}}{\Σ}}{w}_{l}f\left({\mathrm{\ξ}}_l\right)\end{array}$

其中，高斯-厄米特积分点权值

利用高斯-厄米特积分构建上一目标观测时刻的多个积分点概率密度函数具体包括如下步骤：

子步骤S20，获取n时刻每个高斯分布的高斯-厄米特积分点的后验概率密度函数。

具体地，在时刻n目标状态的后验概率密度函数为G个高斯密度函数的加权和，具体如下式所示：

$p\left(x_n\right|y_{0:n})=\underset{g=1}{\overset{G}{\Σ}}{\mathrm{\ω}}_{ng}N(x_n;{\hat{x}}_{\left(n\right|n)g},P_{\left(n\right|n)g})---\left(5\right)$

其中，n时刻表示上一目标观测时刻，目标观测时刻即为目标观测点的采样时刻，为第g个高斯分布函数的均值，P_(n|n)g为第g个高斯分布函数的协方差。ω_ng为对应的权值，均值和协方差P_(n|n)g为根据当前目标观测时刻之前，即包括n时刻以及n时刻之前传感器所观测的目标状态而获得。

子步骤S21，估计n时刻的目标状态后验概率密度函数对应的积分点

具体地，状态空间中包括G个高斯分布，估计n时刻的目标状态后验概率密度函数对应的每个高斯分布的高斯-厄米特积分点具体如下式所示：

$x_{\left(n\right|n)g^{\′}}^l=\sqrt{P_{\left(n\right|n)g}}\·{\mathrm{\ξ}}^l+{\hat{x}}_{\left(n\right|n)g}+{\widetilde{\mathrm{\μ}}}_{(n+1)k}---\left(6\right)$

其中，g’＝g+(k-1)K；g＝1，2，……，G；k＝1，2，……，K；l＝1，2，……，m；ξ_l为高斯-厄米特积分点，相应的权系数为{ω_l}。

子步骤S22，以n时刻的目标状态后验概率密度函数对应的积分点为均值，P_(n|n)g为协方差，构建n时刻的多个积分点概率密度函数

其中，公式(5)、(6)中的非高斯过程噪声和量测噪声的概率密度函数利用高斯和方法近似得到如下式所示：

$p\left(u_n\right)=\underset{k=1}{\overset{K}{\Σ}}{\mathrm{\α}}_{k}N(u_n;{\widetilde{\mathrm{\μ}}}_{nk},{\widetilde{\mathrm{\Σ}}}_{nk})---\left(7\right)$

$p\left(v_n\right)=\underset{j=1}{\overset{L}{\Σ}}{\mathrm{\β}}_{j}N(v_n;{\widetilde{\mathrm{\μ}}}_{nj},{\widetilde{\mathrm{\Σ}}}_{nj})---\left(8\right)$

其中，α_k和β_j均为非负常数，且满足和 和分别表示第k个高斯概率密度函数的均值和协方差。

子步骤S122，根据所述多个积分点概率密度函数获取所述积分点的近似粒子集。

根据上述多个积分点概率密度函数获取n时刻的目标状态后验概率密度函数对应的积分点的近似粒子集其中， ${\left\{x_{\left(n\right|n)g^{\′}}^{l,i}\right\}}_{i=1}^N~N({\hat{x}}_{\left(n\right|n)g^{\′}}^l,P_{\left(n\right|n)g}),i=1,2,......,N.$

子步骤S123，获取所述近似粒子集中每个近似粒子的预测粒子集。

子步骤S124，根据所述预测粒子集获取当前目标观测时刻的目标状态预测概率密度函数。

具体包括如下子步骤：

子步骤S30，根据所述预测粒子集获取n时刻的目标状态后验概率密度函数对应的积分点的均值和协方差具体如下式所示：

${\hat{x}}_{(n+1|n)g^{\′}}^l=\frac{1}{N}\underset{i=1}{\overset{N}{\Σ}}{x}_{(n+1|n)g^{\′}}^{l,i}---\left(9\right)$

$P_{(n+1|n)g^{\′}}^l=\frac{1}{N}\underset{i=1}{\overset{N}{\Σ}}(x_{(n+1|n)g^{\′}}^{l,i}-{\hat{x}}_{(n+1|n)g^{\′}}^l){(x_{(n+1|n)g^{\′}}^{l,i}-{\hat{x}}_{(n+1|n)g^{\′}}^l)}^T---\left(10\right)$

子步骤S31，根据所述积分点的均值和协方差获取n+1时刻的目标状态第g’个高斯概率密度函数的均值和协方差P_(n+1|n)g'，具体如下式所示：

${\hat{x}}_{(n+1|n)g^{\′}}=\underset{l=1}{\overset{m}{\Σ}}{\mathrm{\ω}}_l{\hat{x}}_{(n+1|n)g^{\′}}^l---\left(11\right)$

$P_{(n+1|n)g^{\′}}=\underset{l=1}{\overset{m}{\Σ}}{\mathrm{\ω}}_l{P}_{(n+1|n)g^{\′}}^l---\left(12\right)$

子步骤S32，计算每个高斯概率密度函数的权值，具体如下式所示：

子步骤S33，将G'个高斯概率密度函数的加权和近似为目标状态的预测概率密度函数，具体如下式所示：

具体地，假设在时刻n目标状态的后验概率密度函数为G个高斯密度函数的加权和，具体如下式所示：

$p\left(x_n\right|y_{0:n})=\underset{g=1}{\overset{G}{\Σ}}{\mathrm{\ω}}_{ng}N(x_n;{\hat{x}}_{\left(n\right|n)g},P_{\left(n\right|n)g})---\left(15\right)$

其中，和P_(n|n)g分别表示第g个高斯分布函数的均值和协方差，ω_ng表示对应的权值。

在n+1时刻目标的预测概率密度函数p(x_n+1|y_0:n)可表示为：

$\begin{array}{c}p\left(x_{n+1}\right|y_{0:n})=\∫p\left(x_{n+1}\right|x_n)p\left(x_n\right|y_{0:n}){\mathrm{dx}}_n\\ =\∫{\mathrm{\Σ}}_{k=1}^K{\mathrm{\α}}_{k}N(x_{n+1};f(x_n)+{\widetilde{\mathrm{\μ}}}_{(n+1)k}{\widetilde{\mathrm{\Σ}}}_{(n+1)k})\×...\\ {\mathrm{\Σ}}_{g=1}^G{\mathrm{\ω}}_{ng}N(x_n;{\hat{x}}_{\left(n\right|n)g},P_{\left(n\right|n)g}){\mathrm{dx}}_n\\ ={\mathrm{\Σ}}_{g=1}^G{\mathrm{\Σ}}_{k=1}^K{\mathrm{\α}}_k{\mathrm{\ω}}_{ng}\∫N(x_{n+1};f(x_n)+{\widetilde{\mathrm{\μ}}}_{(n+1)k},{\widetilde{\mathrm{\Σ}}}_{(n+1)k})\×...\\ N(x_n;{\hat{x}}_{\left(n\right|n)g},P_{\left(n\right|n)g}){\mathrm{dx}}_n\\ ={\mathrm{\Σ}}_{g=1}^G{\mathrm{\Σ}}_{k=1}^K{\mathrm{\α}}_k{\mathrm{\ω}}_{ng}\∫N(x_{n+1};f(x_n)+{\widetilde{\mathrm{\μ}}}_{(n+1)k},{\widetilde{\mathrm{\Σ}}}_{(n+1)k})\×...\\ \frac{1}{\sqrt{2{\mathrm{\πP}}_{\left(n\right|n)g}}}\·\exp (-\frac{{(x_n-{\hat{x}}_{\left(n\right|n)g})}^T(x_n-{\hat{x}}_{\left(n\right|n)g})}{2P_{\left(n\right|n)g}}){\mathrm{dx}}_n\end{array}---\left(16\right)$

假设 $P_{\left(n\right|n)g}=S_g^T\·S_g,$ 改变积分变量 $x_n=S_g^T\mathrm{\ξ}+{\hat{x}}_{\left(n\right|n)g},$ 则

$\begin{array}{c}p\left(x_{n+1}\right|y_{0:n})={\mathrm{\Σ}}_{g=1}^G{\mathrm{\Σ}}_{k=1}^K{\mathrm{\α}}_k{\mathrm{\ω}}_{ng}\∫N(x_{n+1};f(S_g^T\mathrm{\ξ}+{\hat{x}}_{\left(n\right|n)g})+{\widetilde{\mathrm{\μ}}}_{(n+1)k},{\widetilde{\mathrm{\Σ}}}_{(n+1)k})...\\ \frac{1}{{\left(2\mathrm{\π}\right)}^{n/2}}\·\exp (-\frac{{\mathrm{\ξ}}^T\mathrm{\ξ}}{2})d\mathrm{\ξ}\\ ={\mathrm{\Σ}}_{g=1}^G{\mathrm{\Σ}}_{k=1}^K{\mathrm{\α}}_k{\mathrm{\ω}}_{ng}\∫N(x_{n+1};f(S_g^T\mathrm{\ξ}+{\hat{x}}_{\left(n\right|n)g})+{\widetilde{\mathrm{\μ}}}_{(n+1)k},{\widetilde{\mathrm{\Σ}}}_{(n+1)k})N(\mathrm{\ξ};0,I_{n_x})d\mathrm{\ξ}\end{array}---\left(17\right)$

应用上述高斯-厄米特积分规则的同时，利用蒙特卡罗近似非线性的状态转移概率密度函数，公式(17)的预测概率密度函数p(x_n+1|y_0:n)可以近似为：

$\begin{array}{c}p\left(x_{n+1}\right|y_{0:n})={\mathrm{\Σ}}_{g=1}^G{\mathrm{\Σ}}_{k=1}^K{\mathrm{\α}}_k{\mathrm{\ω}}_{ng}\[{\mathrm{\Σ}}_{l=1}^m{\mathrm{\ω}}_l\·N(x_{n+1};f(S_g^T\mathrm{\ξ}+{\hat{x}}_{\left(n\right|n)g})+{\widetilde{\mathrm{\μ}}}_{(n+1)k},{\widetilde{\mathrm{\Σ}}}_{(n+1)k})\]\\ ={\mathrm{\Σ}}_{g=1}^G{\mathrm{\Σ}}_{k=1}^K{\mathrm{\Σ}}_{l=1}^m{\mathrm{\α}}_k{\mathrm{\ω}}_{ng}{\mathrm{\ω}}_l\·N(x_{n+1};f(S_g^T\mathrm{\ξ}+{\hat{x}}_{\left(n\right|n)g})+{\widetilde{\mathrm{\μ}}}_{(n+1)k},{\widetilde{\mathrm{\Σ}}}_{(n+1)k})\\ ={\mathrm{\Σ}}_{g=1}^G{\mathrm{\Σ}}_{k=1}^K{\mathrm{\Σ}}_{l=1}^m{\mathrm{\Σ}}_{i=1}^N{\mathrm{\α}}_k{\mathrm{\ω}}_{ng}{\mathrm{\ω}}_l\·u_{l,i}\·N(x_{n+1};f(x_{l\left(n\right|n)g}^i)+{\widetilde{\mathrm{\μ}}}_{(n+1)k},{\widetilde{\mathrm{\Σ}}}_{(n+1)k})\end{array}---\left(18\right)$

进一步地，重新定义g'＝g+(k-1)K,G'＝GK,α_g'＝α_kω_ng，则公式(18)可以写为：

$p\left(x_{n+1}\right|y_{0:n})={\mathrm{\Σ}}_{g^{\′}=1}^{G^{\′}}{\mathrm{\Σ}}_{l=1}^m{\mathrm{\Σ}}_{i=1}^N{\mathrm{\α}}_{g^{\′}}{\mathrm{\ω}}_l\·u_{l,i}\·N(x_{n+1};f(x_{l\left(n\right|n)g}^{l,i})+{\widetilde{\mathrm{\μ}}}_{(n+1)k},{\widetilde{\mathrm{\Σ}}}_{(n+1)k})---\left(19\right)$

其中，表示均值为协方差为的高斯概率密度函数，表示从状态转移概率密度函数中抽取的状态粒子，u_l.i表示粒子相应的权值，m表示高斯-厄米特积分点ξ_l的数量，N表示粒子个数。

根据粒子集预测概率密度函数p(x_n+1|y_0:n)可以近似为G'个高斯概率密度函数的加权和，根据上述公式(15)-(19)获取n+1时刻的目标状态预测概率密度函数，从而完成时间更新。

步骤S13，根据所述当前目标观测时刻的目标状态预测概率密度函数和所述观测噪声概率密度函数获取状态粒子集。

具体地，估计所述n+1时刻的目标状态预测概率密度函数p(x_n+1|y_0:n)对应的积分点具体如下式所示：

$x_{(n+1|n)g^{\′},j}^l=\sqrt{P_{(n+1|n)g^{\′}}}\·{\mathrm{\ξ}}^l+{\hat{x}}_{(n+1|n)g},---\left(20\right)$

其中，g'＝1,...G'；l＝1,...m；j＝1,...,L。

步骤S14，利用模糊聚类原理获取当前目标观测时刻的粒子权值和积分点权值，并计算各高斯项的权值。

具体包括如下子步骤：

子步骤S141，根据所述积分点构建n+1时刻的多个积分点的重要性函数 $N({\hat{x}}_{(n+1|n)g^{\′},j}^l,P_{(n+1|n)g^{\′}}).$

子步骤S142，根据所述n+1时刻的多个积分点的重要性函数获取积分点的近似粒子集具体如下式所示：

${\left\{x_{(n+1|n)g^{\′},j}^{l,i}\right\}}_{i=1}^N~N({\hat{x}}_{(n+1|n)g^{\′},j}^l,P_{(n+1|n)g^{\′}})---\left(21\right)$

其中，l＝1，2，……，m；g’＝1，2，……，G’；j＝1，2，……L；i＝1，2，……，N。

子步骤S143，计算所述近似粒子集中每个粒子的权值具体如下式所示：

${\mathrm{\μ}}_{g^{\′},j}^{l,i}=\frac{1}{\underset{k=1}{\overset{N_s}{\Σ}}{\left(\frac{d_{li}}{d_{lk}}\right)}^{\frac{2}{m1-1}}}=\frac{p_l\left(y_{n+1}\right|x_{(n+1|n)g^{\′},j}^{l,i})}{\underset{k=1}{\overset{N_s}{\Σ}}{p}_l\left(y_{n+1}\right|x_{(n+1|n)g^{\′},j}^{l,k})}---\left(22\right)$

其中，为根据拉格朗日乘子法最小化目标函数而得到的y_n+1与粒子的模糊隶属度，y_n+1为当前目标观测时刻n+1时刻的目标观测；

上述子步骤143的粒子权值计算可对应理解为如下的过程：

基于粒子滤波的特点，在粒子间定义一种概率相似性测度函数，利用基于概率测度的模糊聚类方法得到粒子的模糊隶属度，以代替粒子权值。同时，为了自适应计算积分点权值，通过模糊加权指数自适应计算积分点权值，具体如下。

假设表示重要性密度函数提取的N个粒子，y_n+1表示n+1时刻接收到的观测粒子。根据模糊C均值聚类(FCM)算法，表示观测粒子y_n+1源于粒子的模糊隶属度(也可以说是概率)，且满足如下约束：

$\underset{i=1}{\overset{N}{\Σ}}{\mathrm{\μ}}_{g^{\′},j}^{l,i}=1,{\mathrm{\μ}}_{g^{\′},j}^{l,i}\∈\[0,1\],l=1,2,...,m---\left(24\right)$

于是，构建如下目标函数：

$E_i=\underset{i=1}{\overset{N}{\Σ}}{\left({\mathrm{\μ}}_{g^{\′},j}^{l,i}\right)}^{m_1}{d}_{il}^2---\left(25\right)$

其中，l＝1,2,…,m；_m1表示加权指数，表示粒子与观测粒子y_n+1之间的距离。为了更好地反映粒子与观测粒子的隶属关系，加快收敛速度，定义一种基于概率的相似性测度方法，定义如下：

$d_{il}^2=\frac{1}{{\mathrm{\α}}_l{p}_l\left(y_{n+1}\right|x_{(n+1|n)g^{\′},j}^{l,i})}---\left(26\right)$

其中，表示在粒子的条件下，观测粒子y_n+1的条件似然函数，由观测方程可知：

$p_l\left(y_{n+1}\right|x_{(n+1|n)g^{\′},j}^{l,i})=\frac{1}{{\left(2\mathrm{\π}\right)}^{d/2}\sqrt{\left|S\right|}}\exp \left(\frac{{(y_{n+1}-h(x_{(n+1|n)g^{\′},j}^{l,i}\left)\right)}^T{S}^{-1}(y_{n+1}-h(x_{(n+1|n)g^{\′},j}^{l,i}\left)\right)}{2}\right)---\left(27\right)$

其中，d表示状态粒子的维数，S表示观测信息协方差。为了最优化目标函数，建立新的目标函数，如下式所示：

$J_l=\underset{i=1}{\overset{N}{\Σ}}{\left({\mathrm{\μ}}_{g^{\′},j}^{l,i}\right)}^{m_1}{\left(d_{il}\right)}^2+{\mathrm{\λ}}_l(\underset{i=1}{\overset{N}{\Σ}}{\mathrm{\μ}}_{g^{\′},j}^{l,i}-1),l=1,2,...,m---\left(28\right)$

根据拉格朗日乘子法，与FCM类似，最小化目标函数(28)，可得观测粒子y_n+1与粒子的模糊隶属度之间的关系，具体如下式所示：

${\mathrm{\μ}}_{g^{\′},j}^{l,i}=\frac{1}{\underset{k=1}{\overset{N_s}{\Σ}}{\left(\frac{d_{li}}{d_{lk}}\right)}^{\frac{2}{m1-1}}}=\frac{p_l\left(y_{n+1}\right|x_{(n+1|n)g^{\′},j}^{l,i})}{\underset{k=1}{\overset{N_s}{\Σ}}{p}_l\left(y_{n+1}\right|x_{(n+1|n)g^{\′},j}^{l,k})}---\left(29\right)$

其中，l＝1,2,…,m,i＝1,2,…,N。

于是，观测更新阶段的粒子权值可以定义如下：

子步骤S144，根据每个积分点概率密度函数的粒子集，自适应计算积分点权值，具体如下式所示：

其中，l＝1，2，……，m。

由高斯-厄米特积分规则可知，积分点权值通常为常数。在实验中发现，存在大量的积分点实际上远离目标的预测位置，显然，这些点对于目标状态的更新基本上不起任何作用。因此，为了减少无效积分点在滤波过程中的作用，基于粒子权值和模糊加权指数，提出如公式(31)一种自适应的积分点权值计算方法。

步骤S15，根据所述状态粒子集、所述粒子权值以及所述积分点权值计算每个高斯分布的均值和协方差。

具体包括如下子步骤：

子步骤S151，根据所述近似粒子集和权值获取积分点的均值和协方差具体如下式所示：

子步骤S152，根据所述积分点的权值获取高斯混合项的概率密度函数的均值和协方差，以得到相应的权值。

具体地，根据所述积分点的权值获取高斯混合项的概率密度函数的均值和协方差具体如下式所示：

${\hat{x}}_{(n+1|n+1)g^{\′},j}=\underset{l=1}{\overset{m}{\Σ}}{\mathrm{\ω}}_{g^{\′},j}^i{\hat{x}}_{(n+1|n+1)g^{\′},j}^l---\left(34\right)$

$P_{(n+1|n+1)g^{\′},j}=\underset{l=1}{\overset{m}{\Σ}}{\mathrm{\ω}}_{g^{\′},j}^l{P}_{(n+1|n+1)g^{\′},j}^l---\left(35\right)$

得到相应的权值，具体如下式所示：

其中，

步骤S16，根据所述高斯项的权值对高斯项进行重采样，获取G个权值较大的高斯项。

具体包括如下子步骤：

子步骤S161，将计算得到的G*K*L个高斯项的权值按照降序排列以保留前G个高斯项和权值

其中，g＝1,…,G。

子步骤S162，判断是否若是，则进入子步骤S163，否则，进入子步骤S164。

子步骤S163，设定 ${\hat{x}}_{(n+1|n+1)g}={\hat{x}}_{(n+1|n+1)j},P_{(n+1|n+1)g}=P_{(n+1|n+1)j},$ 其中，j为每次循环时随机提取，且j∈{1,…,G}，提取到j的概率正比于标准化权值g＝1,…,G，G为循环次数且

子步骤S164，获取G个权值较大的高斯项。

经过子步骤S161-S164，近似状态后验概率密度函数的高斯混合项由G项增加到了G*K*L项。随着时间的递推，算法的高斯项也会呈快速增长，造成计算量太大，无法进行实际应用。因此需要对高斯项进行重采样，以防止其快速增长。

步骤S17，根据获取的G个高斯项，利用高斯和原理得到当前目标观测时刻的状态后验概率密度函数，完成粒子滤波。

具体地，利用高斯和原理，根据获取的G个高斯项，当前目标观测时刻的目标状态后验概率密度函数p(x_n+1|y_0:n+1)可以近似成均值为协方差为P_n+1|n+1的高斯分布，具体如下式所示：

进一步地，随着时间地更新，在n+1时刻，当接收到观测目标y_n+1，则后验概率密度函数可以表示为：

$\begin{array}{c}p\left(x_{n+1}\right|y_{0:n+1})=C_{n+1}p\left(y_{n+1}\right|x_{n+1})p\left(x_{n+1}\right|y_{0:n})\\ \≈C_{n+1}p\left(y_{n+1}\right|x_{n+1})p\left({\hat{x}}_{n+1|n}\right|P_{n+1|n})\end{array}---\left(39\right)$

由式(14)可知，观测目标的预测概率密度函数p(y_n+1|x_n+1)可以定义如下：

$p\left(y_{n+1}\right|x_{n+1})=\underset{j=1}{\overset{L}{\Σ}}{\mathrm{\β}}_{j}N(y_{n+1};h(x_{n+1})+{\widetilde{\mathrm{\μ}}}_{(n+1)j},{\widetilde{\mathrm{\Σ}}}_{(n+1)j})---\left(40\right)$

则，目标状态的后验概率密度函数可以表示为：

其中，C_n+1为标准化常数，与时间更新类似。将后验概率密度函数p(x_n+1|y_0:n+1)近似为混合高斯模型，则基于预测概率密度函数p(x_n+1|y_0:n)的积分点构建积分点概率密度函数，抽取滤波粒子集计算出目标状态的估计均值x_n+1|n+1和协方差P_n+1|n+1，因此后验概率密度函数可以表示为

上述步骤S11-S13对应为本发明粒子滤波方法的时间更新阶段，步骤S14-S17对应为本发明粒子滤波方法的观测更新阶段，通过时间更新阶段获得当前目标观测时刻的目标状态预测概率密度函数，在观测更新阶段进一步利用时间更新阶段所获得的目标状态预测概率密度函数而获得当前目标观测时刻的目标状态后验概率密度函数，从而完成粒子滤波过程。

本发明的粒子滤波方法为基于高斯-厄米特的积分粒子滤波方法(GSQPF)，以下将举例对本发明的GSQPF方法的性能进行评估以及与现有技术的GSPF和QPF方法的性能进行对比。

在本例子中将利用实际观测采集的一批雷达航迹数据对本发明的GSQPF方法以及现有的上述方法进行评估和对比。雷达航迹数据中包括50个非周期的航迹点，飞行时间为504s。由于航迹点的非周期性，因此采样时间间隔T＝t(n+1)-t(n)也是对应变化的，且某些航迹点的采样时间间隔达到30s以上，其中n表示采样次数，t(n+1)表示第n+1次采样时的时刻，t(n)表示第n次采样时的时刻。在本例子的实验中采用如下的目标跟踪模型：

$x_n=\left(\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right)x_{n-1}+\left(\begin{array}{cc}{T}^2/2& 0\\ T& 0\\ 0& T^2/2\\ 0& T\end{array}\right)u_n$

$y_n=\left(\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right)x_n+v_n$

其中，目标状态向量为 $x_n={\left\{\begin{array}{cccc}{x}_n& {\stackrel{\·}{x}}_n& y_n& {\stackrel{\·}{y}}_n\end{array}\right\}}^T,$ x_n、y_n分别表示n时刻目标在x、y方向上的位置， 分别表示n时刻目标在x_n、y_n方向上的速度，采样间隔为1s。过程噪声u_n为混合高斯模型 $p\left(u_n\right)=\underset{k=1}{\overset{K}{\Σ}}{\mathrm{\α}}_{k}N(u_n;{\widetilde{\mathrm{\μ}}}_{nk},{\widetilde{\mathrm{\Σ}}}_{nk})=\mathrm{\α}N(0,R_{11})+(1-\mathrm{\α})N(0,R_{12}).$ 其中，R₁₁＝diag([0.01²km²s⁴0.01²km²s⁴]),R₁₂＝diag([0.03²km²s⁴0.03²km²s⁴])，α＝0.8。量测噪声v_n为零均值协方差R＝diag([0.15²km²,0.15²km²])的高斯分布。目标初始状态x₀的先验密度服从其中初始状态x₀为[104.58km-0.144kms^-160.22km0.066kms^-1]^T，初始状态估计和关联协方差分别假设为： ${\hat{x}}_{0|0}={\[104.58km-0.144{\mathrm{kms}}^{-1}60.22km0.066{\mathrm{kms}}^{-1}\]}^T,$ ${\hat{P}}_{0|0}=diag\[{0.15}^2{\mathrm{km}}^{2}0.01{\mathrm{km}}^2{s}^{-2}{0.15}^2{\mathrm{km}}^{2}0.01{\mathrm{km}}^2{s}^{-2}\],$ 雷达假设设在原点。

图8为本发明的GSQPF方法以及现有的上述各种方法的目标跟踪轨迹与真实轨迹的对比图，图9为本发明的GSQPF以及现有的上述各种方法在30-50个采样点之间的目标跟踪轨迹放大图。由图8和9可以看出：在目标机动或者目标观测点的采样时间间隔比较大时，QPF和GSPF的估计轨迹明显偏离真实轨迹，而本发明的GSQPF方法的估计轨迹基本与真实轨迹吻合，估计性能优于现有的上述各种方法，其原因为本发明的GSQPF方法利用多个积分点概率密度函数作为重要性密度函数，并引入了最新的目标观测、采样时间间隔以及目标速度等目标的相关特性修正近似粒子集，改善了粒子的多样性和准确性，从而提高了粒子滤波的目标跟踪精度。

为了对比本发明的GSQPF方法以及现有的上述各种方法的跟踪精度，图10给出了在量测噪声均值为零、协方差为0.15的情况下，GSQPF、GSPF以及QPF的均方根误差对比结果，图11给出了在量测噪声均值为零、协方差为0.5的情况下，GSQPF、GSPF以及QPF的均方根误差对比结果。由图10和11可以看出，GSPF和QPF方法在目标动机或采样时间间隔增大时，跟踪性能快速下降，而GSQPF方法则能够较好地对目标进行跟踪。

可以理解，本发明提供的模糊高斯和粒子滤波方法利用高斯和方法构建上一目标时刻的状态后验概率密度函数、观测噪声概率密度函数和状态噪声概率密度函数，能够增强粒子的多样性；然后上一时刻的状态后验概率密度函数和状态噪声概率密度函数，利用高斯-厄米特积分规则和蒙特卡罗原理获取当前时刻目标状态的预测概率密度函数，进一步获取当前时刻的目标状态粒子集，并利用模糊聚类方法和当前观测获取粒子权值和积分点权值，并计算各高斯项的权值，最后计算每个高斯分布的均值和协方差，并根据高斯项权值，对高斯项进行重采样，以根据获取的G个高斯分布得到当前时刻的状态后验概率密度函数，完成粒子滤波过程。本发明的粒子滤波方法，能够增强粒子的多样性以及粒子权值、积分点权值的准确性，有效提高滤波精度以及目标状态的估计性能，解决非线性非高斯情况下目标状态滤波问题。

请参阅图12，本发明模糊高斯和粒子滤波装置40包括：

积分点概率密度函数构建模块41，用于利用高斯和构建上一目标观测时刻的状态后验概率密度函数、观测噪声概率密度函数以及状态噪声概率密度函数；

目标状态预测概率密度函数获取模块42，用于根据所述上一目标观测时刻的状态后验概率密度函数和所述状态噪声概率密度函数，利用高斯-厄米特积分和蒙特卡罗原理获取当前目标观测时刻的目标状态预测概率密度函数；

状态粒子集获取模块43，用于根据所述当前目标观测时刻的目标状态预测概率密度函数和所述观测噪声概率密度函数获取状态粒子集；

高斯项权值计算模块44，用于利用模糊聚类原理获取当前目标观测时刻的粒子权值和积分点权值，并计算各高斯项的权值；

高斯分布计算模块45，用于根据所述状态粒子集和所述粒子权值、所述积分点权值计算每个高斯分布的均值和协方差；

重采样模块46，用于根据所述高斯项的权值对高斯项进行重采样，获取G个权值较大的高斯项，G为正整数；以及

概率密度函数获取模块47，用于根据获取的G个高斯项，利用高斯和原理得到当前目标观测时刻的状态后验概率密度函数，完成粒子滤波。

请参阅图13，本发明实施方式还提供一种目标跟踪方法，包括：

步骤S500，接收当前目标观测时刻以及当前目标观测时刻之前所观测的目标状态；

步骤S501，利用高斯和构建上一目标观测时刻的状态后验概率密度函数、观测噪声概率密度函数以及状态噪声概率密度函数；

步骤S502，根据所述上一目标观测时刻的状态后验概率密度函数和所述状态噪声概率密度函数，利用高斯-厄米特积分和蒙特卡罗原理获取当前目标观测时刻的目标状态预测概率密度函数；

步骤S503，根据所述当前目标观测时刻的目标状态预测概率密度函数和所述观测噪声概率密度函数获取状态粒子集；

步骤S504，利用模糊聚类原理获取当前目标观测时刻的粒子权值和积分点权值，并计算各高斯项的权值；

步骤S505，根据所述状态粒子集和所述粒子权值、所述积分点权值计算每个高斯分布的均值和协方差；

步骤S506，根据所述高斯项的权值对高斯项进行重采样，获取G个权值较大的高斯项，G为正整数；以及

步骤S507，根据获取的G个高斯项，利用高斯和原理得到当前目标观测时刻的状态后验概率密度函数；

步骤S508，利用所述当前目标观测时刻的目标状态后验概率密度函数对目标状态进行估计，以获得当前目标观测时刻的目标状态估计值；

步骤S509，输出所述当前目标观测时刻的目标状态估计值，以实现对目标的跟踪。

请参阅图14，本发明实施方式还提供一种目标跟踪装600，包括：

观测数据接收模块601，用于接收当前目标观测时刻以及当前目标观测时刻之前所观测的目标状态；

积分点概率密度函数构建模块602，用于利用高斯和构建上一目标观测时刻的状态后验概率密度函数、观测噪声概率密度函数以及状态噪声概率密度函数；

目标状态预测概率密度函数获取模块603，用于根据所述上一目标观测时刻的状态后验概率密度函数和所述状态噪声概率密度函数，利用高斯-厄米特积分和蒙特卡罗原理获取当前目标观测时刻的目标状态预测概率密度函数；

状态粒子集获取模块604，用于根据所述当前目标观测时刻的目标状态预测概率密度函数和所述观测噪声概率密度函数获取状态粒子集；

高斯项权值计算模块605，用于利用模糊聚类原理获取当前目标观测时刻的粒子权值和积分点权值，并计算各高斯项的权值；

高斯分布计算模块606，用于根据所述状态粒子集和所述粒子权值、所述积分点权值计算每个高斯分布的均值和协方差；

重采样模块607，用于根据所述高斯项的权值对高斯项进行重采样，获取G个权值较大的高斯项，G为正整数；以及

概率密度函数获取模块608，用于根据获取的G个高斯项，利用高斯和原理得到当前目标观测时刻的状态后验概率密度函数；

目标状态估计模块609，用于利用所述当前目标观测时刻的目标状态后验概率密度函数对目标状态进行估计，以获得当前目标观测时刻的目标状态估计值；

目标状态估计值输出模块610，用于输出所述当前目标观测时刻的目标状态估计值，以实现对目标的跟踪。

以上所述仅为本发明的实施方式，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (11)

1.一种模糊高斯和粒子滤波方法，其特征在于，包括：

利用高斯和构建上一目标观测时刻的状态后验概率密度函数、观测噪声概率密度函数以及状态噪声概率密度函数；

根据所述上一目标观测时刻的状态后验概率密度函数和所述状态噪声概率密度函数，利用高斯-厄米特积分和蒙特卡罗原理获取当前目标观测时刻的目标状态预测概率密度函数；

根据所述当前目标观测时刻的目标状态预测概率密度函数和所述观测噪声概率密度函数获取状态粒子集；

利用模糊聚类原理获取当前目标观测时刻的粒子权值和积分点权值，并计算各高斯项的权值；

根据所述状态粒子集和所述粒子权值、所述积分点权值计算每个高斯分布的均值和协方差；

根据所述高斯项的权值对高斯项进行重采样，获取G个权值较大的高斯项，G为正整数；以及

根据获取的G个高斯项，利用高斯和原理得到当前目标观测时刻的状态后验概率密度函数，完成粒子滤波。

2.根据权利要求1所述的方法，其特征在于，所述根据上一目标时刻的状态后验概率密度函数和状态噪声概率密度函数，利用高斯-厄米特积分规则和蒙特卡罗原理获取当前时刻目标状态的预测概率密度函数的步骤包括：

利用高斯-厄米特积分构建上一目标观测时刻的多个积分点概率密度函数；

根据所述多个积分点概率密度函数获取所述积分点的近似粒子集；其中，所述积分点的近似粒子集为 i＝1，2，……，N；

获取所述近似粒子集中每个近似粒子的预测粒子集；以及

根据所述预测粒子集获取当前目标观测时刻的目标状态预测概率密度函数。

3.根据权利要求2所述的方法，其特征在于，所述利用高斯-厄米特积分构建上一目标观测时刻的多个积分点概率密度函数的步骤具体包括：

获取n时刻每个高斯分布的高斯-厄米特积分点的后验概率密度函数，具体如下式所示：

$p\left(x_n\right|y_{0:n})=\underset{g=1}{\overset{G}{\Σ}}{\mathrm{\ω}}_{ng}N(x_n;{\hat{x}}_{\left(n\right|n)g},P_{\left(n\right|n)g})---\left(5\right)$

其中，n时刻表示上一目标观测时刻，为第g个高斯分布函数的均值，P_(n|n)g为第g个高斯分布函数的协方差；

估计n时刻的目标状态后验概率密度函数对应的积分点具体如下式所示：

$x_{\left(n\right|n)g^{\′}}^l=\sqrt{P_{\left(n\right|n)g}}\·{\mathrm{\ξ}}^l+{\hat{x}}_{\left(n\right|n)g}+{\widetilde{\mathrm{\μ}}}_{(n+1)k}---\left(6\right)$

其中，g’＝g+(k-1)K；g＝1，2，……，G；k＝1，2，……，K；l＝1，2，……，m；为高斯-厄米特积分点；

以n时刻的目标状态后验概率密度函数对应的积分点为均值，P_(n|n)g为协方差，构建n时刻的多个积分点概率密度函数

其中，公式(5)、(6)中的非高斯过程噪声和量测噪声的概率密度函数利用高斯和方法近似得到如下式所示：

$p\left(u_n\right)=\underset{k=1}{\overset{K}{\Σ}}{\mathrm{\α}}_{k}N(u_n;{\widetilde{\mathrm{\μ}}}_{nk},{\widetilde{\mathrm{\Σ}}}_{nk})---\left(7\right)$

$p\left(v_n\right)=\underset{j=1}{\overset{L}{\Σ}}{\mathrm{\β}}_{j}N(v_n;{\widetilde{\mathrm{\μ}}}_{nj},{\widetilde{\mathrm{\Σ}}}_{nj})---\left(8\right)$

其中，α_k和β_j均为非负常数，且满足和 和分别表示第k个高斯概率密度函数的均值和协方差。

4.根据权利要求3所述的方法，其特征在于，所述根据预测粒子集获取当前目标观测时刻的目标状态预测概率密度函数的步骤具体包括：

根据所述预测粒子集获取n时刻的目标状态后验概率密度函数对应的积分点的均值和协方差具体如下式所示：

${\hat{x}}_{(n+1|n)g^{\′}}^l=\frac{1}{N}\underset{i=1}{\overset{N}{\Σ}}{x}_{(n+1|n)g^{\′}}^{l,i}---\left(9\right)$

$P_{(n+1|n)g^{\′}}^l=\frac{1}{N}\underset{i=1}{\overset{N}{\Σ}}(x_{(n+1|n)g^{\′}}^{l,i}-{\hat{x}}_{(n+1|n)g^{\′}}^l){(x_{(n+1|n)g^{\′}}^{l,i}-{\hat{x}}_{(n+1|n)g^{\′}}^l)}^T---\left(10\right)$

根据所述积分点的均值和协方差获取n+1时刻的目标状态第g’个高斯概率密度函数的均值和协方差P_(n+1|n)g'，具体如下式所示：

${\hat{x}}_{(n+1|n)g^{\′}}=\underset{l=1}{\overset{m}{\Σ}}{\mathrm{\ω}}_l{\hat{x}}_{(n+1|n)g^{\′}}^l---\left(11\right)$

$P_{(n+1|n)g^{\′}}=\underset{l=1}{\overset{m}{\Σ}}{\mathrm{\ω}}_l{P}_{(n+1|n)g^{\′}}^l---\left(12\right)$

计算每个高斯概率密度函数的权值，具体如下式所示：

将G'个高斯概率密度函数的加权和近似为目标状态的预测概率密度函数，具体如下式所示：

5.根据权利要求4所述的方法，其特征在于，利用模糊聚类原理获取当前目标观测时刻的粒子权值和积分点权值，并计算各高斯项的权值的步骤包括：

根据所述积分点构建n+1时刻的多个积分点的重要性函数 $N({\hat{x}}_{(n+1|n)g^{\′},j}^l,P_{(n+1|n)g^{\′}});$

根据所述n+1时刻的多个积分点的重要性函数获取积分点的近似粒子集具体如下式所示：

${\left\{x_{(n+1|n)g^{\′},j}^{l,i}\right\}}_{i=1}^N~N({\hat{x}}_{(n+1|n)g^{\′},j}^l,P_{(n+1|n)g^{\′}})---\left(21\right)$

其中，l＝1，2，……，m；g’＝1，2，……，G’；j＝1，2，……L；i＝1，2，……，N；

计算所述近似粒子集中每个粒子的权值具体如下式所示：

${\mathrm{\μ}}_{g^{\′},j}^{l,i}=\frac{1}{\underset{k=1}{\overset{N_s}{\Σ}}{\left(\frac{d_{li}}{d_{lk}}\right)}^{\frac{2}{m1-1}}}=\frac{p_l\left(y_{n+1}\right|x_{(n+1|n)g^{\′},j}^{l,i})}{\underset{k=1}{\overset{N_s}{\Σ}}{p}_l\left(y_{n+1}\right|x_{(n+1|n)g^{\′},j}^{l,k})}---\left(22\right)$

其中，为根据拉格朗日乘子法最小化目标函数而得到的y_n+1与粒子的模糊隶属度，y_n+1为当前目标观测时刻n+1时刻的目标观测。

6.根据权利要求5所述的方法，其特征在于，根据所述状态粒子集和所述粒子权值、所述积分点权值计算每个高斯分布的均值和协方差的步骤包括：

根据所述近似粒子集和权值获取积分点的均值和协方差具体如下式所示：

根据所述积分点的权值获取高斯混合项的概率密度函数的均值和协方差以得到相应的权值，具体如下式所示：

其中， $P_{(n+1|n+1)g^{\′},j}=\underset{l=1}{\overset{m}{\Σ}}{\mathrm{\ω}}_{g^{\′},j}^l{P}_{(n+1|n+1)g^{\′},j}^l.$

7.根据权利要求6所述的方法，其特征在于，根据所述高斯项的权值对高斯项进行重采样，获取G个权值较大的高斯项的步骤包括：

将计算得到的G*K*L个高斯项的权值按照降序排列以保留前G个高斯项和权值其中，g＝1,…,G；

当时，设定 ${\hat{x}}_{(n+1|n+1)g}={\hat{x}}_{(n+1|n+1)j},P_{(n+1|n+1)g}=P_{(n+1|n+1)j},$ 其中，j为每次循环时随机提取，且j∈{1,…,G}，提取到j的概率正比于标准化权值G为循环次数且以及

获取G个权值较大的高斯项。

8.根据权利要求7所述的方法，其特征在于，根据获取的G个高斯项，利用高斯和原理得到当前目标观测时刻的状态后验概率密度函数，完成粒子滤波的步骤具体为：

利用高斯和原理，根据获取的G个高斯项，当前目标观测时刻的目标状态后验概率密度函数p(x_n+1|y_0:n+1)可以近似成均值为协方差为P_n+1|n+1的高斯分布，具体如下式所示：

9.一种模糊高斯和粒子滤波装置，其特征在于，包括：

积分点概率密度函数构建模块，用于利用高斯和构建上一目标观测时刻的状态后验概率密度函数、观测噪声概率密度函数以及状态噪声概率密度函数；

目标状态预测概率密度函数获取模块，用于根据所述上一目标观测时刻的状态后验概率密度函数和所述状态噪声概率密度函数，利用高斯-厄米特积分和蒙特卡罗原理获取当前目标观测时刻的目标状态预测概率密度函数；

状态粒子集获取模块，用于根据所述当前目标观测时刻的目标状态预测概率密度函数和所述观测噪声概率密度函数获取状态粒子集；

高斯项权值计算模块，用于利用模糊聚类原理获取当前目标观测时刻的粒子权值和积分点权值，并计算各高斯项的权值；

高斯分布计算模块，用于根据所述状态粒子集和所述粒子权值、所述积分点权值计算每个高斯分布的均值和协方差；

重采样模块，用于根据所述高斯项的权值对高斯项进行重采样，获取G个权值较大的高斯项，G为正整数；以及

概率密度函数获取模块，用于根据获取的G个高斯项，利用高斯和原理得到当前目标观测时刻的状态后验概率密度函数，完成粒子滤波。

10.一种目标跟踪方法，其特征在于，包括：

接收当前目标观测时刻以及当前目标观测时刻之前所观测的目标状态；

利用高斯和构建上一目标观测时刻的状态后验概率密度函数、观测噪声概率密度函数以及状态噪声概率密度函数；

根据所述上一目标观测时刻的状态后验概率密度函数和所述状态噪声概率密度函数，利用高斯-厄米特积分和蒙特卡罗原理获取当前目标观测时刻的目标状态预测概率密度函数；

根据所述当前目标观测时刻的目标状态预测概率密度函数和所述观测噪声概率密度函数获取状态粒子集；

利用模糊聚类原理获取当前目标观测时刻的粒子权值和积分点权值，并计算各高斯项的权值；

根据所述状态粒子集和所述粒子权值、所述积分点权值计算每个高斯分布的均值和协方差；

根据所述高斯项的权值对高斯项进行重采样，获取G个权值较大的高斯项，G为正整数；

根据获取的G个高斯项，利用高斯和原理得到当前目标观测时刻的状态后验概率密度函数；

利用所述当前目标观测时刻的目标状态后验概率密度函数对目标状态进行估计，以获得当前目标观测时刻的目标状态估计值；

输出所述当前目标观测时刻的目标状态估计值，以实现对目标的跟踪。

11.一种目标跟踪装置，其特征在于，包括：

观测数据接收模块，用于接收当前目标观测时刻以及当前目标观测时刻之前所观测的目标状态；

积分点概率密度函数构建模块，用于利用高斯和构建上一目标观测时刻的状态后验概率密度函数、观测噪声概率密度函数以及状态噪声概率密度函数；

目标状态预测概率密度函数获取模块，用于根据所述上一目标观测时刻的状态后验概率密度函数和所述状态噪声概率密度函数，利用高斯-厄米特积分和蒙特卡罗原理获取当前目标观测时刻的目标状态预测概率密度函数；

状态粒子集获取模块，用于根据所述当前目标观测时刻的目标状态预测概率密度函数和所述观测噪声概率密度函数获取状态粒子集；

高斯项权值计算模块，用于利用模糊聚类原理获取当前目标观测时刻的粒子权值和积分点权值，并计算各高斯项的权值；

高斯分布计算模块，用于根据所述状态粒子集和所述粒子权值、所述积分点权值计算每个高斯分布的均值和协方差；

重采样模块，用于根据所述高斯项的权值对高斯项进行重采样，获取G个权值较大的高斯项，G为正整数；

概率密度函数获取模块，用于根据获取的G个高斯项，利用高斯和原理得到当前目标观测时刻的状态后验概率密度函数；

目标状态估计模块，用于利用所述当前目标观测时刻的目标状态后验概率密度函数对目标状态进行估计，以获得当前目标观测时刻的目标状态估计值；

目标状态估计值输出模块，用于输出所述当前目标观测时刻的目标状态估计值，以实现对目标的跟踪。