CN103902812B - 一种粒子滤波方法、装置及目标跟踪方法、装置 - Google Patents

Abstract

本发明公开了一种粒子滤波方法、装置及目标跟踪方法、装置，该粒子滤波方法包括利用高斯-厄米特积分构建上一目标观测时刻的多个积分点概率密度函数；根据多个积分点概率密度函数获取积分点的近似粒子集；根据目标的相关特性修正近似粒子集以获取预测粒子集；根据预测粒子集获取当前目标观测时刻的目标状态预测概率密度函数；根据当前目标观测时刻的目标状态预测概率密度函数获取当前目标观测时刻的目标状态后验概率密度函数。通过上述方式，本发明能够增强粒子的多样性和准确性，有效提高滤波精度以及目标状态的估计性能。

Description

一种粒子滤波方法、装置及目标跟踪方法、装置

技术领域

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

背景技术

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

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

此外，现有技术还采用另一种非线性滤波方法：粒子滤波方法，现有技术所采用的一类粒子滤波方法由于粒子退化现象的存在，需要进行重采样，从而影响粒子滤波的并行实现。现有技术另一类粒子滤波方法则无需进行重采样，如高斯粒子滤波（GPF），快速高斯粒子滤波算法，拟蒙特卡罗-高斯粒子滤波(QMC-GPF)算法等，这类方法由于在时间更新时只是简单地采用状态转移函数进行粒子采样，当目标观测点的采样时间间隔较大或目标运动模型不够精确时，粒子的多样性以及准确性较差，粒子并不能有效表示目标的后验概率分布，从而降低粒子滤波的性能。

发明内容

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

为解决上述技术问题，本发明的第一方面是：提供一种粒子滤波方法，包括利用高斯-厄米特积分构建上一目标观测时刻的多个积分点概率密度函数；根据多个积分点概率密度函数获取积分点的近似粒子集；根据目标的相关特性修正近似粒子集以获取预测粒子集；根据预测粒子集获取当前目标观测时刻的目标状态预测概率密度函数；根据当前目标观测时刻的目标状态预测概率密度函数获取当前目标观测时刻的目标状态后验概率密度函数，完成粒子滤波过程。

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

获取k-₁时刻的目标状态后验概率密度函数，具体如下式所示：

$p\left(x_{k-1}\right|z_{1:k-1})=N\left({\hat{x}}_{k-1|k-1,}{P}_{k-1|k-1}\right)---\left(1\right)$

其中，k-1时刻表示上一目标观测时刻，为均值，P_k-1|k-1为协方差；

估计k-1时刻的目标状态后验概率密度函数对应的积分点x_l,k-1|k-1，具体如下式所示：

$x_{l,k-1|k-1}=\sqrt{P_{k-1|k-1}}{\mathrm{\ξ}}_l+{\hat{x}}_{k-1|k-1}---\left(2\right)$

其中，l＝1,2,...,m，ξ_l表示高斯-厄米特积分点；

以k-1时刻的目标状态后验概率密度函数对应的积分点x_l,k-1|k-1为均值，P_k-1|k-1为协方差，构建k-1时刻的多个积分点概率密度函数 $N\left({\hat{x}}_{l,k-1|k-1,}{P}_{k-1|k-1}\right).$

其中，积分点x_l,k-1|k-1的近似粒子集为其中 ${\left\{x_{l,k-1|k-1}^i\right\}}_{i=1}^{N_s}~N\left({\hat{x}}_{l,k-1|k-1,}{P}_{k-1|k-1}\right),i=\mathrm{1,2},...,N_s.$

其中，根据目标的相关特性修正近似粒子以获取预测粒子集的步骤具体包括：

获取近似粒子集中每个近似粒子的期望值具体如下式所示：

$x_{l,k-1|k-1}^i=f_k(x_{l,k-1|k-1}^i,v_{k-1})---\left(3\right)$

${\mathrm{\α}}_k^i=\frac{T^2\·v^2\·{\mathrm{\σ}}_v^2\left(k\right)}{\mathrm{\λ}\·{\mathrm{\σ}}_m^2\left(k\right)+T^2\·v^2{\·\mathrm{\σ}}_v^2\left(k\right)}---\left(4\right)$

${\mathrm{\μ}}_{l,k}^i\left(A_k\right)=x_{l,k|k-1}^i+{\mathrm{\α}}_k^i(z_k-h_k\left(x_{l,k|k-1}^i\right))---\left(5\right)$

其中，目标的相关特性包括目标观测、采样时间间隔以及目标速度，z_k表示k时刻的目标观测，k时刻表示当前目标观测时刻，T表示采样时间间隔，v表示目标速度，λ为常数，σ_m(k)表示观测误差标准差，σ_v(k)表示观测新息标准差，f_k表示系统状态转移函数，v_k-1表示状态的过程噪声，h_k表示非线性的观测函数；

获取预测粒子集具体如下式所示：

$x_{l,k|k-1}^i={\mathrm{\μ}}_{l,k}^i\left(A_k\right)---\left(6\right).$

其中，根据预测粒子集获取当前目标观测时刻的目标状态预测概率密度函数的步骤具体包括：

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

${\hat{x}}_{l,k|k-1}=\frac{1}{N_s}\underset{i=1}{\overset{N_s}{\mathrm{\Σ}}}{x}_{l,k|k-1}^i---\left(7\right)$

$P_{l,k|k-1}=\frac{1}{N_s}\underset{i=1}{\overset{N_s}{\mathrm{\Σ}}}(x_{l,k|k-1}^i-{\hat{x}}_{l,k|k-1}){(x_{l,k|k-1}^i-{\hat{x}}_{l,k|k-1})}^T---\left(8\right)$

根据积分点x_l,k-1|k-1的均值和协方差P_l,k|k-1获取k时刻的目标状态预测概率密度函数_p(x_k|z_1:k-1)，具体如下式所示：

$p\left(x_k\right|z_{1:k-1})=N\left({\hat{x}}_{k|k-1,}{P}_{k|k-1}\right)---\left(9\right)$

${\hat{x}}_{k|k-1}=\underset{l=1}{\overset{m}{\mathrm{\Σ}}}{w}_l{\hat{x}}_{l,k|k-1}---\left(10\right)$

$P_{k|k-1}=\underset{l=1}{\overset{m}{\mathrm{\Σ}}}{w}_l{P}_{l,k|k-1}---\left(11\right)$

其中，为k时刻的目标状态预测概率密度函数p(x_k|z_1:k-1)的均值，P_k|k-1为k时刻的目标状态预测概率密度函数p(x_k|z_1:k-1)的协方差。

根据当前目标观测时刻的目标状态预测概率密度函数获取当前目标观测时刻的目标状态后验概率密度函数的步骤具体包括：

估计k时刻的目标状态预测概率密度函数_p(x_k|z_1:k-1)对应的积分点具体如下式所示：

$P_{k|k-1}=\sqrt{P_{k|k-1}}{\left(\sqrt{P_{k|k-1}}\right)}^T---\left(12\right)$

${\hat{x}}_{l,k|k-1}=\sqrt{P_{k|k-1}}{\mathrm{\ξ}}_l+{\hat{x}}_{k|k-1}---\left(13\right)$

根据积分点构建k时刻的多个积分点概率密度函数 $N\left({\hat{x}}_{l,k-1|k-1,}{P}_{k-1|k-1}\right);$

根据k时刻的多个积分点概率密度函数获取积分点的近似粒子集具体如下式所示：

${\left\{x_{l,k|k-1}^i\right\}}_{i=1}^m~N\left({\hat{x}}_{l,k-1|k-1,}{P}_{k|k-1}\right)---\left(14\right)$

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

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

根据积分点的均值和协方差P_l,k|k获取k时刻的目标状态后验概率密度函数p(x_k|z_1:k)，具体如下式所示：

$p\left(x_k\right|z_{1:k})=N\left({\hat{x}}_{k|k,}{P}_{k|k}\right)---\left(18\right)$

${\hat{x}}_{k|k}=\underset{l=1}{\overset{m}{\mathrm{\Σ}}}{w}_l{\hat{x}}_{l,k|k}---\left(19\right)$

$P_{k|k}=\underset{l=1}{\overset{m}{\mathrm{\Σ}}}{w}_l{P}_{l,k|k}---\left(20\right)$

其中，为k时刻的目标状态后验概率密度函数p(x_k|z_1:k)的均值，P_k|k为k时刻的目标状态后验概率密度函数p(x_k|z_1:k)的协方差。

为解决上述技术问题，本发明的第二方面是：提供一种粒子滤波装置，包括积分点概率密度函数构建模块，用于利用高斯-厄米特积分构建上一目标观测时刻的多个积分点概率密度函数；近似粒子集获取模块，用于根据多个积分点概率密度函数获取积分点的近似粒子集；预测粒子集获取模块，用于根据目标的相关特性修正近似粒子集以获取预测粒子集；目标状态预测概率密度函数获取模块，用于根据预测粒子集获取当前目标观测时刻的目标状态预测概率密度函数；目标状态后验概率密度函数，用于根据当前目标观测时刻的目标状态预测概率密度函数获取当前目标观测时刻的目标状态后验概率密度函数。

为解决上述技术问题，本发明的第三方面是：提供一种目标跟踪方法，包括接收当前目标观测时刻以及当前目标观测时刻之前所观测的目标状态；利用高斯-厄米特积分以及当前目标观测时刻之前所观测的目标状态构建上一目标观测时刻的多个积分点概率密度函数；根据多个积分点概率密度函数获取积分点的近似粒子集；根据目标的相关特性修正近似粒子集以获取预测粒子集；根据预测粒子集获取当前目标观测时刻的目标状态预测概率密度函数；根据当前目标观测时刻的目标状态预测概率密度函数以及当前目标观测时刻所观测的目标状态获取当前目标观测时刻的目标状态后验概率密度函数；利用当前目标观测时刻的目标状态后验概率密度函数对目标状态进行估计，以获得当前目标观测时刻的目标状态估计值；输出当前目标观测时刻的目标状态估计值，以实现对目标的跟踪。

为解决上述技术问题，本发明的第四方面是：提供一种目标跟踪装置，包括观测数据接收模块，用于接收当前目标观测时刻以及当前目标观测时刻之前所观测的目标状态；积分点概率密度函数构建模块，用于利用高斯-厄米特积分以及当前目标观测时刻之前所观测的目标状态构建上一目标观测时刻的多个积分点概率密度函数；近似粒子集获取模块，用于根据多个积分点概率密度函数获取积分点的近似粒子集；预测粒子集获取模块，用于根据目标的相关特性修正近似粒子集以获取预测粒子集；目标状态预测概率密度函数获取模块，用于根据预测粒子集获取当前目标观测时刻的目标状态预测概率密度函数；目标状态后验概率密度函数，用于根据当前目标观测时刻的目标状态预测概率密度函数以及当前目标观测时刻所观测的目标状态获取当前目标观测时刻的目标状态后验概率密度函数；目标状态估计模块，用于利用当前目标观测时刻的目标状态后验概率密度函数对目标状态进行估计，以获得当前目标观测时刻的目标状态估计值；目标状态估计值输出模块，用于输出当前目标观测时刻的目标状态估计值，以实现对目标的跟踪。

本发明的有益效果是：区别于现有技术的情况，本发明粒子滤波方法一实施方式利用高斯-厄米特积分构建上一目标观测时刻的多个积分点概率密度函数；然后根据多个积分点概率密度函数获取积分点的近似粒子集；接着根据目标观测、采样时间间隔以及目标速度等目标的相关特性修正近似粒子集以获取预测粒子集；进一步根据预测粒子集获取当前目标观测时刻的目标状态预测概率密度函数；最后根据当前目标观测时刻的目标状态预测概率密度函数获取当前目标观测时刻的目标状态后验概率密度函数，能够增强粒子的多样性和准确性，有效提高滤波精度以及目标状态的估计性能，在非均匀稀疏采样环境下同样能够保证目标状态的估计性能。

附图说明

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

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

图3是本发明粒子滤波方法一实施方式中根据目标的相关特性修正近似粒子以获取预测粒子集的流程图；

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

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

图6是本发明粒子滤波方法一实施方式中在过程噪声为零均值方差0.01的高斯噪声时，UKF和QKF的均方根误差对比图；

图7是本发明粒子滤波方法一实施方式中在过程噪声为零均值方差0.01的高斯噪声时，PF、APF、GPF和AQPF的均方根误差对比图；

图8是本发明粒子滤波方法一实施方式中在过程噪声为零均值方差0.01的高斯噪声时，AQPF、UKF、QKF、PF、APF和GPF在不同观测噪声方差条件下的均方根误差对比图；

图9是本发明粒子滤波方法一实施方式中在过程噪声为服从伽玛分布的非高斯噪声时，UKF和QKF的均方根误差对比图；

图10是本发明粒子滤波方法一实施方式中在过程噪声为服从伽玛分布的非高斯噪声时，PF、APF、GPF和AQPF的均方根误差对比图；

图11是本发明粒子滤波方法一实施方式中在过程噪声为服从伽玛分布的非高斯噪声时，AQPF、UKF、QKF、PF、APF和GPF在不同观测噪声方差条件下的均方根误差对比图；

图12是本发明粒子滤波方法一实施方式中在非均匀稀疏采样环境下，AQPF、UKF、QKF、PF、APF和GPF的目标跟踪轨迹和真实轨迹的对比图；

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

图14是本发明粒子滤波方法一实施方式中在非均匀稀疏采样环境下，QKF、PF、APF和GPF的均方根误差对比图；

图15是本发明粒子滤波方法一实施方式中在非均匀稀疏采样环境下，AQPF的均方根误差图；

图16是本发明粒子滤波装置一实施方式的原理框图。

具体实施方式

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

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

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

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

高斯-厄米特（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为对角矩阵的第个特征值；权值其中(v_l)₁为对角矩阵J第l个标准化特征值的第1个元素。

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

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

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

本发明粒子滤波方法首先利用高斯-厄米特积分构建上一目标观测时刻的多个积分点概率密度函数，该步骤具体包括以下子步骤：

子步骤S111：获取k-1时刻的目标状态后验概率密度函数；

获取k-1时刻的目标状态后验概率密度函数，具体如下式所示：

$p\left(x_{k-1}\right|z_{1:k-1})=N\left({\hat{x}}_{k-1|k-1,}{P}_{k-1|k-1}\right)---\left(1\right)$

其中，k-1时刻表示上一目标观测时刻，目标观测时刻即为目标观测点的采样时刻，为均值，P_k-1|k-1为协方差，均值和协方差P_k-1|k-1为根据当前目标观测时刻之前即包括k-1时刻以及k-1时刻之前传感器所观测的目标状态而获得；对协方差P_k-1|k-1进行分解可得： $P_{k-1|k-1}=\sqrt{P_{k-1|k-1}}{\left(\sqrt{P_{k-1|k-1}}\right)}^T.$

子步骤S112：估计k-1时刻的目标状态后验概率密度函数对应的积分点；

估计k-1时刻的目标状态后验概率密度函数对应的积分点x_l,k-1|k-1，具体如下式所示：

$x_{l,k-1|k-1}=\sqrt{P_{k-1|k-1}}{\mathrm{\ξ}}_l+{\hat{x}}_{k-1|k-1}---\left(2\right)$

其中，l＝1,2,...,m，ξ_l为上述高斯-厄米特积分点，m个积分点x_l,k-1|k-1即构成积分点集

子步骤S113：以k-1时刻的目标状态后验概率密度函数对应的积分点为均值，P_k-1|k-1为协方差，构建k-1时刻的多个积分点概率密度函数。

进一步以k-1时刻的目标状态后验概率密度函数对应的积分点x_l,k-1|k-1为均值，P_k-1|k-1为协方差，对应构建k-1时刻的多个积分点概率密度函数 $N\left({\hat{x}}_{l,k-1|k-1,}{P}_{k-1|k-1}\right).$

步骤S12：根据多个积分点概率密度函数获取积分点的近似粒子集；

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

步骤S13：根据目标的相关特性修正近似粒子集以获取预测粒子集；

本实施方式中目标的相关特性包括目标观测、采样时间间隔以及目标速度，在其他实施方式中目标的相关特性也可为目标观测时刻等，本步骤具体包括以下子步骤：

子步骤S131：获取近似粒子集中每个近似粒子的期望值；

引入目标观测、采样时间间隔以及目标速度三个目标的相关特性以获取近似粒子集中每个近似粒子的期望值具体如下式所示：

$x_{l,k-1|k-1}^i=f_k(x_{l,k-1|k-1}^i,v_{k-1})---\left(3\right)$

${\mathrm{\α}}_k^i=\frac{T^2\·v^2\·{\mathrm{\σ}}_v^2\left(k\right)}{\mathrm{\λ}\·{\mathrm{\σ}}_m^2\left(k\right)+T^2\·v^2{\·\mathrm{\σ}}_v^2\left(k\right)}---\left(4\right)$

${\mathrm{\μ}}_{l,k}^i\left(A_k\right)=x_{l,k|k-1}^i+{\mathrm{\α}}_k^i(z_k-h_k\left(x_{l,k|k-1}^i\right))---\left(5\right)$

每个近似粒子的期望值即为将公式（3）、（4）分别获得的结果代入公式（5）而获得。其中，表示k时刻的目标观测，k时刻表示当前目标观测时刻，T表示采样时间间隔，v表示目标速度，λ为常数，σ_m(k)表示观测误差标准差，σ_v(k)表示观测新息标准差，f_k表示系统状态转移函数，v_k-1∈Rⁿ表示状态的过程噪声，表示非线性的观测函数。其中，公式（3）中的表示在根据目标的相关特性修正近似粒子集之前每个近似粒子原先对应的状态预测值。

子步骤S132：获取预测粒子集。

获取预测粒子集具体如下式所示：

$x_{l,k|k-1}^i={\mathrm{\μ}}_{l,k}^i\left(A_k\right)---\left(6\right)$

即采用进行修正近似粒子集所获得的每个近似粒子的期望值代替公式（3）中每个近似粒子原先对应的状态预测值即获得如公式（6）所示的每个近似粒子进行修正近似粒子集之后对应的状态预测值，也即获得预测粒子集

步骤S14：根据预测粒子集获取当前目标观测时刻的目标状态预测概率密度函数；

本步骤具体包括以下子步骤：

子步骤S141：根据预测粒子集获取k-1时刻的目标状态后验概率密度函数对应的积分点的均值和协方差；

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

${\hat{x}}_{l,k|k-1}=\frac{1}{N_s}\underset{i=1}{\overset{N_s}{\mathrm{\Σ}}}{x}_{l,k|k-1}^i---\left(7\right)$

$P_{l,k|k-1}=\frac{1}{N_s}\underset{i=1}{\overset{N_s}{\mathrm{\Σ}}}(x_{l,k|k-1}^i-{\hat{x}}_{l,k|k-1}){(x_{l,k|k-1}^i-{\hat{x}}_{l,k|k-1})}^T---\left(8\right)$

子步骤S142：根据积分点的均值和协方差获取k时刻的目标状态预测概率密度函数。

根据上述积分点x_l,k-1|k-1的均值和协方差P_l,k|k-1获取k时刻即当前目标观测时刻的目标状态预测概率密度函数_p(x_k|z_1:k-1)，具体如下式所示：

$p\left(x_k\right|z_{1:k-1})=N\left({\hat{x}}_{k|k-1,}{P}_{k|k-1}\right)---\left(9\right)$

${\hat{x}}_{k|k-1}=\underset{l=1}{\overset{m}{\mathrm{\Σ}}}{w}_l{\hat{x}}_{l,k|k-1}---\left(10\right)$

$P_{k|k-1}=\underset{l=1}{\overset{m}{\mathrm{\Σ}}}{w}_l{P}_{l,k|k-1}---\left(11\right)$

其中，为k时刻的目标状态预测概率密度函数p(x_k|z_1:k-1)的均值，P_k|k-1为k时刻的目标状态预测概率密度函数p(x_k|z_1:k-1)的协方差。

步骤S15：根据当前目标观测时刻的目标状态预测概率密度函数获取当前目标观测时刻的目标状态后验概率密度函数。

本步骤具体包括以下子步骤：

子步骤S151：估计k时刻的目标状态预测概率密度函数对应的积分点；

估计k时刻的目标状态预测概率密度函数_p(x_k|z_1:k-1)对应的积分点具体如下式所示：

$P_{k|k-1}=\sqrt{P_{k|k-1}}{\left(\sqrt{P_{k|k-1}}\right)}^T---\left(12\right)$

${\hat{x}}_{l,k|k-1}=\sqrt{P_{k|k-1}}{\mathrm{\ξ}}_l+{\hat{x}}_{k|k-1}---\left(13\right)$

子步骤S152：根据积分点构建k时刻的多个积分点概率密度函数；

根据k时刻的目标状态预测概率密度函数_p(x_k|z₁ ^: _k-1)对应的积分点构建k时刻的多个积分点概率密度函数

子步骤S153：根据k时刻的多个积分点概率密度函数获取积分点的近似粒子集；

根据k时刻的多个积分点概率密度函数获取积分点的近似粒子集具体如下式所示：

${\left\{x_{l,k|k-1}^i\right\}}_{i=1}^m~N\left({\hat{x}}_{l,k-1|k-1,}{P}_{k|k-1}\right)---\left(14\right)$

子步骤S154：计算近似粒子集中每个粒子的权值；

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

其中z_k表示当前目标观测时刻k时刻的目标观测，即z_k为当前目标观测时刻传感器所观测的目标状态。

子步骤S155：根据近似粒子集和权值获取积分点的均值和协方差；

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

子步骤S156：根据积分点的均值和协方差获取k时刻的目标状态后验概率密度函数。

根据上述积分点的均值和协方差P_l,k|k获取k时刻即当前目标观测时刻的目标状态后验概率密度函数p(x_k|z_1:k)，具体如下式所示：

$p\left(x_k\right|z_{1:k})=N\left({\hat{x}}_{k|k,}{P}_{k|k}\right)---\left(18\right)$

${\hat{x}}_{k|k}=\underset{l=1}{\overset{m}{\mathrm{\Σ}}}{w}_l{\hat{x}}_{l,k|k}---\left(19\right)$

$P_{k|k}=\underset{l=1}{\overset{m}{\mathrm{\Σ}}}{w}_l{P}_{l,k|k}---\left(20\right)$

其中，为k时刻的目标状态后验概率密度函数p(x_k|z_1:k)的均值，P_k|k为k时刻的目标状态后验概率密度函数p(x_k|z_1:k)的协方差。

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

具体地，上述步骤S11-S14的时间更新阶段可对应理解为如下的过程：

首先考虑一个非线性离散时间系统:

x_k＝f_k(x_k-1,v_k-1)（21）

z_k＝h_k(x_k,e_k)（22）

其中，表示系统状态转移函数，表示非线性的观测函数，表示k时刻的目标状态，表示k时刻的目标观测；v_k-1∈Rn表示状态的过程噪声，e_k∈Rm表示目标观测噪声。最优贝叶斯滤波就是在给定观测z_1:k的基础上估计出状态x_k的概率密度函数，一般而言，贝叶斯滤波可以通过以下递推贝叶斯公式对目标后验概率密度函数进行估计，

$p\left(x_k\right|z_{1:k-1})=\∫p\left(x_k\right|x_{k-1})p\left(x_{k-1}\right|z_{1:k-1})d{x}_{k-1---\left(23\right)}$

$p\left(x_k\right|z_{1:k})=\frac{p\left(z_k\right|x_k)p\left(x_k\right|z_{1:k-1})}{p\left(z_k\right|z_{1:k-1})}---\left(24\right)$

$p\left(z_k\right|z_{1:k-1})=\∫p\left(z_k\right|x_k)p\left(x_k\right|z_{1:k-1})d{x}_{k---\left(25\right)}$

其中，预测概率密度函数_p(x_k|x_k-1,z_1:k-1)＝_p(x_k|x_k-1)可以通过状态方程和已知过程噪声v_k-1的状态分布获得。似然函数v_k-1可以通过观测模型(22)和已知的观测噪声分布e_k获得。

假设在k-1时刻的目标状态后验概率密度函数_p(x_k-1|z_1:k-1)是一均值为p(x_k-1|z_1:k-1)、协方差为P_k-1|k-1的高斯密度函数，考虑目标的相关特性A_k＝{z,v,T}对目标状态预测概率密度函数的影响，其中，z表示目标观测、v表示目标速度、T表示观测时间间隔，根据公式（23），时刻的目标状态预测概率密度函数可以写为：

$\begin{array}{c}p(x_k,A_k|z_{1:k-1},A_{k-1})={\∫}_{R^{n_x}}p(x_k,A_k|x_{k-1},A_{k-1})p\left(x_{k-1}\right|z_{1:k-1}){\mathrm{dx}}_{k-1}\\ ={\∫}_{R^{n_x}}p(x_k,A_k|x_{k-1},A_{k-1})\frac{1}{\sqrt{\left|2\mathrm{\π}{P}_{k-1|k-1}\right|}}\×\exp (-\frac{1}{2}{(x_{k-1}-{\hat{x}}_{k-1|k-1})}^T{P}_{k-1|k-1}^{-1}(x_{k-1}-{\hat{x}}_{k-1|k-1})){\mathrm{dx}}_{k-1}\end{array}---\left(26\right)$

假设改变积分变量使 $x_{k-1}=S^T\mathrm{\ξ}+{\hat{x}}_{k-1|k-1},$ 可得：

$\begin{array}{c}p(x_k,A_k|z_{1:k-1},A_{k-1})={\∫}_{R^{n_x}}p(x_k,A_k|S^T\mathrm{\ξ}+{\hat{x}}_{k-1|k-1},A_{k-1})\frac{1}{{\left(2\mathrm{\π}\right)}^{n/2}}\×\exp (-\frac{1}{2}{\mathrm{\ξ}}^T\mathrm{\ξ})\mathrm{d\ξ}\\ ={\∫}_{R^{n_x}}p(x_k,A_k|S^T\mathrm{\ξ}+{\hat{x}}_{k-1|k-1},A_{k-1})N(\mathrm{\ξ};0,I_{n_x})\mathrm{d\ξ}\end{array}---\left(27\right)$

应用上述高斯-厄米特积分规则，同时，利用蒙特卡罗近似非线性的状态转移概率密度函数，式（27）的预测概率密度函数_p(x_k,A_k|z_1:k-1,A_k-1)可以近似为：

$\begin{array}{c}p(x_k,A_k|z_{1:k-1},A_{k-1})=\underset{l=1}{\overset{m}{\mathrm{\Σ}}}{w}_l\·p(x_k,A_k|S^T{\mathrm{\ξ}}_l+{\hat{x}}_{k-1|k-1},A_{k-1})\\ =\underset{l=1}{\overset{m}{\mathrm{\Σ}}}\underset{i=1}{\overset{N_s}{\mathrm{\Σ}}}(w_l\·u_{l,i}\·p(x_k,A_k|x_{l,k-1|k-1}^i,A_{k-1}))\end{array}---\left(28\right)$

其中，表示高斯-厄米特积分点ξ_l的先验概率密度函数也即积分点概率密度函数，l＝1,2,...,m，表示从积分点概率密度函数中抽取的粒子，u_l.i表示粒子相应的权值，m表示高斯-厄米特积分点ξ_l数量，N_s表示粒子个数。

由于各个积分点、粒子相互独立，因此有：

$\begin{array}{c}p(x_k,A_k|x_{l,k-1|k-1}^i,A_{k-1})=p\left(x_k\right|A_k,x_{l,k-1|k-1}^i)p\left(A_k\right|A_{k-1},x_{l,k-1|k-1}^i)\\ =p\left({\mathrm{\μ}}_{l,k}^i\left(A_k\right)\right|x_{l,k-1|k-1}^i)\×\underset{j=1}{\overset{3}{\∏}}p\left(A_{k,j}\right|A_{k-1,j},x_{l,k-1|k-1}^i)\end{array}---\left(29\right)$

其中，表示融入目标的相关特性条件下粒子的期望值，在时间更新阶段引入目标观测、观测时间间隔以及目标速度三个因素利用上述公式（3）、（4）、（5）对每个粒子进行自适应修正；进而利用粒子的期望值代替粒子的状态预测值最后根据上述公式（7）-（11）获取k时刻的目标状态预测概率密度函数，从而完成时间更新。

此外，通过分析上述高斯-厄米特积分规则可以发现：积分点虽然丰富多样，但许多积分点远远偏离目标预测位置，即许多积分点明显不可能是目标点，此时如果仍利用此类积分点构建积分点概率密度函数，则可能会降低目标跟踪的性能，另外会增加粒子滤波算法的计算复杂性。因此在其他实施方式中，在构建积分点概率密度函数之前可先对积分点进行有效性判断，剔除对提高目标跟踪性能没有贡献的积分点，例如上述不可能是目标点的积分点，从而提高粒子滤波方法的计算效率和实用性。

本发明的粒子滤波方法为目标特性辅助的积分粒子滤波方法（AQPF），以下将以两个例子对本发明的AQPF方法的性能进行评估以及与现有技术的UKF、QKF、粒子滤波（PF）、辅助粒子滤波（APF）和GPF方法的性能进行对比。第一个例子为研究非线性环境下单变量非平稳增长模型（UNGM）；第二个粒子为分析非均匀稀疏采样环境下的目标跟踪问题，对应利用实际观测采集的雷达航迹数据对本发明以及现有的上述方法进行评估和对比。

第一个例子——单变量非平稳增长模型（UNGM）：

单变量非平稳增长模型的状态方程和观测方程分别如下：

$x_k=\mathrm{\α}{x}_{k-1}+\mathrm{\β}\frac{x_{k-1}}{1+x_{k-1}^2}+\mathrm{\γ}\cos \left(1.2(k-1)\right)+v_k$

$z_k=\left\{\begin{array}{cc}{\mathrm{\φ}}_1{x}_k^2+e_k& k\≤30\\ {\mathrm{\φ}}_2{x}_k^3-2+e_k& k>30\end{array}\right.$

其中，e_k表示零均值方差为0.01的高斯噪声；状态方程和观测方程的其他参数设置如下：α＝0.5，β＝25，γ＝8，φ₁＝0.2，φ₂＝0.5。所有实验进行100次蒙特卡罗仿真，状态的实际初始值x₀服从[0，1]之间的均匀分布。

情况1：当过程噪声v_k为零均值方差为0.01的高斯噪声时，图6为现有的UKF和QKF的均方根误差对比图，图7为现有的PF、APF、GPF以及本发明的AQPF四种粒子滤波器的均方根误差对比图。综合图6和图7可以看出：UKF和QKF的滤波性能要明显差于其他四种粒子滤波方法，这主要是由于在观测方程和状态方程为强非线性时，滤波算法的非线性近似性能快速下降，造成滤波性能降低；另外从图7可以看出：AQPF的滤波性能优于PF和APF滤波器，与GPF的滤波性能相近。图8为本发明的AQPF方法以及现有的上述各种方法在不同观测噪声方差条件的平均均方根误差对比图，从图8可以看出，随着噪声方差的增大，PF和APF的滤波性能反而变好，这主要是因为PF和APF都是利用状态转移先验作为重要性密度函数，当观测噪声很小时，观测似然函数呈尖峰状，抽取的粒子远离观测似然，反而变成无效粒子，造成滤波性能下降。同时，对比可以看出提出方法AQPF对不同的噪声水平都能获得一致估计。

情况2：当过程噪声v_k为服从伽玛分布的非高斯噪声Γ(2,3)时，图9为UKF和QKF的均方根误差对比图，图10为现有的PF、APF、GPF以及本发明的AQPF四种粒子滤波器的均方根误差对比图，图11为本发明的AQPF方法以及现有的上述各种方法在不同观测噪声方差条件的平均均方根误差对比图。由图9-11可以看出：UKF和QKF的滤波性能最差，AQPF的滤波性能优于PF和APF，与GPF的滤波性能相近。

第二个例子——非均匀稀疏采样环境下的目标跟踪：

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

$x_k=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]x_{k-1}+\begin{array}{}\end{array}\left[\begin{array}{cc}{T}^2/2& 0\\ T& 0\\ 0& T^2/2\\ 0& T\end{array}\right]v_k$

$z_k=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]x_k+e_k$

其中，目标状态向量为x_k、y_k分别表示时刻目标在x、y方向上的位置，分别表示时刻目标在x、y方向上的速度；过程噪声v_k～N(0,Q)，其中观测噪声e_k～N(0,R)，其中R=diag([0.15²km²0.15²km²])目标初始状态x₀的先验密度服从其中初始状态估计和关联协方差分别假设为： ${\hat{x}}_{0|0}=[104.58\mathrm{km}-0.144\mathrm{km}{s}^{-1}60.216\mathrm{km}0.066\mathrm{km}{s}^{-1}{]}^T,$ ${\hat{P}}_{0|0}=\mathrm{diag}[0.144k{m}^{2}0.002^2{\mathrm{km}}^2{s}^{-2}0.144{\mathrm{km}}^{2}0.02^2{\mathrm{km}}^2{s}^{-2}{]}^T,$ 雷达假设设在原点。

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

为了对比本发明的AQPF方法以及现有的上述各种方法的跟踪精度，图14给出了QKF、PF、APF、GPF的均方根误差对比结果，图15给出了本发明的AQPF方法的均方根误差结果。由图14和15可以看出：QKF、PF、APF、GPF等方法在目标机动或采样时间间隔增大时，跟踪性能快速下降，而AQPF方法则能够较好地对目标进行跟踪。

进一步地，表1给出了PF、APF、GPF和AQPF四种粒子滤波器的均方根误差随粒子数变化的情况。所有实验进行100次蒙特卡罗仿真，观测误差为0.15km。

表1四种粒子滤波器的均方根误差随粒子数的变化情况（km）

从表1中可以看出，上述四种方法的均方根误差随着粒子数的增加均呈下降的趋势。在粒子数小于200时，PF、APF和GPF的均方根误差均超过了1km，而对于AQPF，在粒子数大于100时，粒子数对算法的滤波性能影响较小，均方根误差都在0.2km左右，能够对目标状态进行较好的估计。根据AQPF对粒子数并不敏感这一特点，在对目标进行跟踪时，在保证跟踪性能的前提下，通过适当地减少粒子数可以提高AQPF的运行效率以及增加算法的实时性能。

可以理解，本发明粒子滤波方法一实施方式利用高斯-厄米特积分构建上一目标观测时刻的多个积分点概率密度函数，能够增强粒子的多样性；然后根据多个积分点概率密度函数获取积分点的近似粒子集；接着根据目标观测、采样时间间隔以及目标速度等目标的相关特性修正近似粒子集以获取预测粒子集，通过在粒子滤波的时间更新阶段有效融合上述目标的相关特性，能够增强粒子的准确性；进一步根据预测粒子集获取当前目标观测时刻的目标状态预测概率密度函数；最后根据当前目标观测时刻的目标状态预测概率密度函数获取当前目标观测时刻的目标状态后验概率密度函数。本发明的粒子滤波方法构建基于高斯-厄米特积分的积分点概率密度函数，根据预测粒子集获取目标状态预测概率密度函数，在粒子滤波的过程中无需进行重采样过程。本发明的粒子滤波方法能够应用于非线性高斯噪声以及非线性非高斯噪声的情况，有效提高滤波精度以及目标状态的估计性能，在非均匀稀疏采样环境下同样能够保证目标状态的估计性能。

请参阅图16，本发明粒子滤波装置包括：

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

近似粒子集获取模块22，用于根据积分点概率密度函数构建模块21构建的多个积分点概率密度函数获取积分点的近似粒子集。

预测粒子集获取模块23，用于根据目标的相关特性修正上述近似粒子集以获取预测粒子集。

目标状态预测概率密度函数获取模块24，用于根据预测粒子集获取当前目标观测时刻的目标状态预测概率密度函数。

目标状态后验概率密度函数25，用于根据当前目标观测时刻的目标状态预测概率密度函数获取当前目标观测时刻的目标状态后验概率密度函数。

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

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

利用高斯-厄米特积分以及上述当前目标观测时刻之前所观测的目标状态构建上一目标观测时刻的多个积分点概率密度函数；

根据多个积分点概率密度函数获取积分点的近似粒子集；

根据目标的相关特性修正近似粒子集以获取预测粒子集；

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

根据当前目标观测时刻的目标状态预测概率密度函数以及上述当前目标观测时刻所观测的目标状态获取当前目标观测时刻的目标状态后验概率密度函数；

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

输出上述当前目标观测时刻的目标状态估计值，以实现对飞机、航空飞行器、车辆等目标的跟踪。

本发明实施方式还提供一种目标跟踪装置，包括：

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

积分点概率密度函数构建模块，用于利用高斯-厄米特积分以及当前目标观测时刻之前所观测的目标状态构建上一目标观测时刻的多个积分点概率密度函数。

近似粒子集获取模块，用于根据多个积分点概率密度函数获取积分点的近似粒子集。

预测粒子集获取模块，用于根据目标的相关特性修正近似粒子集以获取预测粒子集。

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

目标状态后验概率密度函数，用于根据当前目标观测时刻的目标状态预测概率密度函数以及当前目标观测时刻所观测的目标状态获取当前目标观测时刻的目标状态后验概率密度函数。

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

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

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

Claims (6)

1.一种粒子滤波方法，其特征在于，包括：

获取k-1时刻的目标状态后验概率密度函数，具体如下式所示：

$p\left(x_{k-1}\right|z_{1:k-1})=N\left({\hat{x}}_{x_{k-1|k-1,}}{P}_{k-1|k-1}\right)---\left(1\right)$

其中，k-1时刻表示上一目标观测时刻，为均值，P_k-1|k-1为协方差；

估计k-1时刻的目标状态后验概率密度函数对应的积分点x_l,k-1|k-1，具体如下式所示：

$x_{l,k-1|k-1}=\sqrt{P_{k-1|k-1}}{\mathrm{\ξ}}_l+{\hat{x}}_{k-1|k-1}---\left(2\right)$

其中，l＝1,2,...,m，ξ_l表示高斯-厄米特积分点；

以k-1时刻的目标状态后验概率密度函数对应的积分点x_l,k-1|k-1为均值，P_k-1|k-1为协方差，构建k-1时刻的多个积分点概率密度函数

根据所述多个积分点概率密度函数获取积分点的近似粒子集；所述积分点x_l,k-1|k-1的近似粒子集为其中 ${\left\{x_{l,k-1|k-1}^i\right\}}_{i=1}^{N_s}~N\left({\hat{x}}_{l,k-1|k-1,}{P}_{k-1|k-1}\right),$ i＝1,2,...,N_s；

获取所述近似粒子集中每个近似粒子的期望值具体如下式所示：

$x_{l,k|k-1}^i=f_k\left(x_{l,k-1|k-1}^i,v_{k-1}\right)---\left(3\right)$

${\mathrm{\α}}_k^i=\frac{T^2\·v^2\·{\mathrm{\σ}}_v^2\left(k\right)}{\mathrm{\λ}\·{\mathrm{\σ}}_m^2\left(k\right)+T^2\·v^2\·{\mathrm{\σ}}_v^2\left(k\right)}---\left(4\right)$

${\mathrm{\μ}}_{l,k}^i\left(A_k\right)=x_{l,k|k-1}^i+{\mathrm{\α}}_k^i(z_k-h_k(x_{l,k|k-1}^i\left)\right)---\left(5\right)$

其中，所述目标的相关特性包括目标观测、采样时间间隔以及目标速度，z_k表示k时刻的目标观测，k时刻表示当前目标观测时刻，T表示采样时间间隔，v表示目标速度，λ为常数，σ_m(k)表示观测误差标准差，σ_v(k)表示观测新息标准差，f_k表示系统状态转移函数，v_k-1表示状态的过程噪声，h_k表示非线性的观测函数；

获取预测粒子集具体如下式所示：

$x_{l,k|k-1}^i={\mathrm{\μ}}_{l,k}^i\left(A_k\right)---\left(6\right);$

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

根据所述当前目标观测时刻的目标状态预测概率密度函数获取当前目标观测时刻的目标状态后验概率密度函数，完成粒子滤波过程。

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

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

${\hat{x}}_{l,k|k-1}=\frac{1}{N_s}\underset{i=1}{\overset{N_s}{\Σ}}{x}_{l,k|k-1}^i---\left(7\right)$

$P_{l,k|k-1}=\frac{1}{N_s}\underset{i=1}{\overset{N_s}{\Σ}}(x_{l,k|k-1}^i-{\hat{x}}_{l,k|k-1}){(x_{l,k|k-1}^i-{\hat{x}}_{l,k|k-1})}^T---\left(8\right)$

根据所述积分点x_l,k-1|k-1的均值和协方差P_l,k|k-1获取k时刻的目标状态预测概率密度函数p(x_k|z_1:k-1)，具体如下式所示：

$p\left(x_k\right|z_{1:k-1})=N\left({\hat{x}}_{k|k-1,}{P}_{k|k-1}\right)---\left(9\right)$

${\hat{x}}_{k|k-1}=\underset{l=1}{\overset{m}{\Σ}}{w}_l{\hat{x}}_{l,k|k-1}---\left(10\right)$

$P_{k|k-1}=\underset{l=1}{\overset{m}{\Σ}}{w}_l{P}_{l,k|k-1}---\left(11\right)$

其中，为k时刻的目标状态预测概率密度函数p(x_k|z_1:k-1)的均值，P_k|k-1为k时刻的目标状态预测概率密度函数p(x_k|z_1:k-1)的协方差。

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

估计所述k时刻的目标状态预测概率密度函数p(x_k|z_1:k-1)对应的积分点具体如下式所示：

$P_{k|k-1}=\sqrt{P_{k|k-1}}{\left(\sqrt{P_{k|k-1}}\right)}^T---\left(12\right)$

${\hat{x}}_{l,k|k-1}=\sqrt{P_{k|k-1}}{\mathrm{\ξ}}_l+{\hat{x}}_{k|k-1}---\left(13\right)$

根据所述积分点构建k时刻的多个积分点概率密度函数

根据所述k时刻的多个积分点概率密度函数获取积分点的近似粒子集具体如下式所示：

${\left\{x_{l,k|k-1}^i\right\}}_{i=1}^m~N\left({\hat{x}}_{l,k|k-1},P_{k|k-1}\right)---\left(14\right)$

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

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

根据所述积分点的均值和协方差P_l,k|k获取k时刻的目标状态后验概率密度函数p(x_k|z_1:k)，具体如下式所示：

$p\left(x_k\right|z_{1:k})=N\left({\hat{x}}_{k|k,}{P}_{k|k}\right)---\left(18\right)$

${\hat{x}}_{k|k}=\underset{l=1}{\overset{m}{\Σ}}{w}_l{\hat{x}}_{l,k|k}---\left(19\right)$

$P_{k|k}=\underset{l=1}{\overset{m}{\Σ}}{w}_l{P}_{l,k|k}---\left(20\right)$

其中，为k时刻的目标状态后验概率密度函数p(x_k|z_1:k)的均值，P_k|k为k时刻的目标状态后验概率密度函数p(x_k|z_1:k)的协方差。

4.一种粒子滤波装置，其特征在于，包括：

积分点概率密度函数构建模块，用于：

获取k-1时刻的目标状态后验概率密度函数，具体如下式所示：

$p\left(x_{k-1}\right|z_{1:k-1})=N\left({\hat{x}}_{k-1|k-1,}{P}_{k-1|k-1}\right)---\left(1\right)$

其中，k-1时刻表示上一目标观测时刻，为均值，P_k-1|k-1为协方差；

估计k-1时刻的目标状态后验概率密度函数对应的积分点x_l,k-1|k-1，具体如下式所示：

$x_{l,k-1|k-1}=\sqrt{P_{k-1|k-1}}{\mathrm{\ξ}}_l+{\hat{x}}_{k-1|k-1}---\left(2\right)$

其中，l＝1,2,...,m，ξ_l表示高斯-厄米特积分点；

以k-1时刻的目标状态后验概率密度函数对应的积分点x_l,k-1|k-1为均值，P_k-1|k-1为协方差，构建k-1时刻的多个积分点概率密度函数

近似粒子集获取模块，用于根据所述多个积分点概率密度函数获取积分点的近似粒子集；所述积分点x_l,k-1|k-1的近似粒子集为其中 ${\left\{x_{l,k-1|k-1}^i\right\}}_{i=1}^{N_s}~N\left({\hat{x}}_{l,k-1|k-1,}{P}_{k-1|k-1}\right),i=1,2,\mathrm{...},N_s;$

预测粒子集获取模块，用于获取所述近似粒子集中每个近似粒子的期望值具体如下式所示：

$x_{l,k|k-1}^i=f_k(x_{l,k-1|k-1}^i,v_{k-1})---\left(3\right)$

${\mathrm{\α}}_k^i=\frac{T^2\·v^2\·{\mathrm{\σ}}_v^2\left(k\right)}{\mathrm{\λ}\·{\mathrm{\σ}}_m^2\left(k\right)+T^2\·v^2\·{\mathrm{\σ}}_v^2\left(k\right)}---\left(4\right)$

${\mathrm{\μ}}_{l,k}^i\left(A_k\right)=x_{l,k|k-1}^i+{\mathrm{\α}}_k^i(z_k-h_k(x_{l,k|k-1}^i\left)\right)---\left(5\right)$

其中，所述目标的相关特性包括目标观测、采样时间间隔以及目标速度，z_k表示k时刻的目标观测，k时刻表示当前目标观测时刻，T表示采样时间间隔，v表示目标速度，λ为常数，σ_m(k)表示观测误差标准差，σ_v(k)表示观测新息标准差，f_k表示系统状态转移函数，v_k-1表示状态的过程噪声，h_k表示非线性的观测函数；

以及获取预测粒子集具体如下式所示：

$x_{l,k|k-1}^i={\mathrm{\μ}}_{l,k}^i\left(A_k\right)---\left(6\right);$

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

目标状态后验概率密度函数，用于根据所述当前目标观测时刻的目标状态预测概率密度函数获取当前目标观测时刻的目标状态后验概率密度函数。

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

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

获取k-1时刻的目标状态后验概率密度函数，具体如下式所示：

$p\left(x_{k-1}\right|z_{1:k-1})=N\left({\hat{x}}_{k-1|k-1,}{P}_{k-1|k-1}\right)---\left(1\right)$

其中，k-1时刻表示上一目标观测时刻，为均值，P_k-1|k-1为协方差；

估计k-1时刻的目标状态后验概率密度函数对应的积分点x_l,k-1|k-1，具体如下式所示：

$x_{l,k-1|k-1}=\sqrt{P_{k-1|k-1}}{\mathrm{\ξ}}_l+{\hat{x}}_{k-1|k-1}---\left(2\right)$

其中，l＝1,2,...,m，ξ_l表示高斯-厄米特积分点；

以k-1时刻的目标状态后验概率密度函数对应的积分点x_l,k-1|k-1为均值，P_k-1|k-1为协方差，构建k-1时刻的多个积分点概率密度函数

根据所述多个积分点概率密度函数获取积分点的近似粒子集；所述积分点x_l,k-1|k-1的近似粒子集为其中 ${\left\{x_{l,k-1|k-1}^i\right\}}_{i=1}^{N_s}~N\left({\hat{x}}_{l,k-1|k-1,}{P}_{k-1|k-1}\right),$ $i=1,2,\mathrm{...},N_s;$

获取所述近似粒子集中每个近似粒子的期望值具体如下式所示：

$x_{l,k|k-1}^i=f_k(x_{l,k-1|k-1}^i,v_{k-1})---\left(3\right)$

${\mathrm{\α}}_k^i=\frac{T^2\·v^2\·{\mathrm{\σ}}_v^2\left(k\right)}{\mathrm{\λ}\·{\mathrm{\σ}}_m^2\left(k\right)+T^2\·v^2\·{\mathrm{\σ}}_v^2\left(k\right)}---\left(4\right)$

${\mathrm{\μ}}_{l,k}^i\left(A_k\right)=x_{l,k|k-1}^i+{\mathrm{\α}}_k^i(z_k-h_k(x_{l,k|k-1}^i\left)\right)---\left(5\right)$

其中，所述目标的相关特性包括目标观测、采样时间间隔以及目标速度，z_k表示k时刻的目标观测，k时刻表示当前目标观测时刻，T表示采样时间间隔，v表示目标速度，λ为常数，σ_m(k)表示观测误差标准差，σ_v(k)表示观测新息标准差，f_k表示系统状态转移函数，v_k-1表示状态的过程噪声，h_k表示非线性的观测函数；

获取预测粒子集具体如下式所示：

$x_{l,k|k-1}^i={\mathrm{\μ}}_{l,k}^i\left(A_k\right)---\left(6\right);$

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

根据所述当前目标观测时刻的目标状态预测概率密度函数以及当前目标观测时刻所观测的目标状态获取当前目标观测时刻的目标状态后验概率密度函数；

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

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

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

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

积分点概率密度函数构建模块，用于：

获取k-1时刻的目标状态后验概率密度函数，具体如下式所示：

$p\left(x_{k-1}\right|z_{1:k-1})=N\left({\hat{x}}_{k-1|k-1,}{P}_{k-1|k-1}\right)---\left(1\right)$

其中，k-1时刻表示上一目标观测时刻，为均值，P_k-1|k-1为协方差；

估计k-1时刻的目标状态后验概率密度函数对应的积分点x_l,k-1|k-1，具体如下式所示：

$x_{l,k-1|k-1}=\sqrt{P_{k-1|k-1}}{\mathrm{\ξ}}_l+{\hat{x}}_{k-1|k-1}---\left(2\right)$

其中，l＝1,2,...,m，ξ_l表示高斯-厄米特积分点；

以k-1时刻的目标状态后验概率密度函数对应的积分点x_l,k-1|k-1为均值，P_k-1|k-1为协方差，构建k-1时刻的多个积分点概率密度函数

近似粒子集获取模块，用于根据所述多个积分点概率密度函数获取积分点的近似粒子集；所述积分点x_l,k-1|k-1的近似粒子集为其中 ${\left\{x_{l,k-1|k-1}^i\right\}}_{i=1}^{N_s}~N\left({\hat{x}}_{l,k-1|k-1,}{P}_{k-1|k-1}\right),i=1,2,\mathrm{...},N_s;$

预测粒子集获取模块，用于获取所述近似粒子集中每个近似粒子的期望值具体如下式所示：

$x_{l,k|k-1}^i=f_k\left(x_{l,k-1|k-1}^i,v_{k-1}\right)---\left(3\right)$

${\mathrm{\α}}_k^i=\frac{T^2\·v^2\·{\mathrm{\σ}}_v^2\left(k\right)}{\mathrm{\λ}\·{\mathrm{\σ}}_m^2\left(k\right)+T^2\·v^2\·{\mathrm{\σ}}_v^2\left(k\right)}---\left(4\right)$

${\mathrm{\μ}}_{l,k}^i\left(A_k\right)=x_{l,k|k-1}^i+{\mathrm{\α}}_k^i(z_k-h_k(x_{l,k|k-1}^i\left)\right)---\left(5\right)$

其中，所述目标的相关特性包括目标观测、采样时间间隔以及目标速度，z_k表示k时刻的目标观测，k时刻表示当前目标观测时刻，T表示采样时间间隔，v表示目标速度，λ为常数，σ_m(k)表示观测误差标准差，σ_v(k)表示观测新息标准差，f_k表示系统状态转移函数，v_k-1表示状态的过程噪声，h_k表示非线性的观测函数；以及

获取预测粒子集具体如下式所示：

$x_{l,k|k-1}^i={\mathrm{\μ}}_{l,k}^i\left(A_k\right)---\left(6\right);$

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

目标状态后验概率密度函数，用于根据所述当前目标观测时刻的目标状态预测概率密度函数以及当前目标观测时刻所观测的目标状态获取当前目标观测时刻的目标状态后验概率密度函数；

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

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