CN104376581A - 一种采用自适应重采样的高斯混合无迹粒子滤波算法 - Google Patents

Abstract

本发明涉及一种采用自适应重采样的高斯混合无迹粒子滤波算法。本发明从两个方面优化普通单高斯粒子滤波算法，即重要性密度函数和重要性重采样。采用高斯混合无迹变换作为粒子滤波的重要性密度函数，能够准确估计系统状态。在传统残差重采样的基础上，本发明提出了一种简单而有效的自适应残差重采样，缓解了粒子退化贫化现象。为了评估发明算法的性能，系统采用一种不确定模型——随机游走模型作为状态模型。仿真结果表明发明算法在跟踪精度、状态估计和粒子集多样性方面均优于普通单高斯粒子滤波算法。

Description

一种采用自适应重采样的高斯混合无迹粒子滤波算法

技术领域

本发明涉及非线性滤波算法领域，具体涉及一种采用自适应残差重采样的高斯混合无迹粒子滤波方法，应用于图像领域中目标跟踪。

背景技术

非线性滤波问题一直以来都是图像处理、人工智能领域的研究热点，其在智能监控、自动控制、导航、金融管理数据分析、机动目标跟踪、经济统计、数字通信等领域具有重要应用价值。随着滤波跟踪模型复杂性的增强和对滤波精度需求的不断提高，传统的非线性滤波方法已不能满足实际要求。粒子滤波作为一种新型的非线性滤波方法，其不受系统模型特性和噪声分布的限制，更符合实际滤波任务的要求，因此在非线性、非高斯动态系统的滤波问题中受到了广泛关注。

粒子滤波基于蒙特卡洛模拟思想，其基本算法基于贝叶斯采样估计的序贯重要性采样(Sequential Importance Sampling，SIS)。粒子滤波基本方法是：通过寻找一组在状态空间中传播的随机样本对后验概率密度函数进行近似，以样本均值代替积分运算，从而获得状态最小方差估计的过程，这些样本即称为”粒子“。对于非高斯非线性平稳随机过程，假定k-1时刻系统的后验概率密度为p(s_k-1|z_k-1)，依据重要性密度函数选取n个随机样本点，也即“粒子”，并分配相应的粒子权值。k时刻获得测量信息后，经过状态和时间更新过程，得到更新后的n个粒子和粒子权值。系统k时刻的后验概率密度p(s_k-1|z_k-1)可以用这些粒子和权值近似表示。随着粒子数目n的增加，粒子的概率密度函数逐渐逼近状态的概率密度函数，粒子滤波估计即达到了最优贝叶斯估计的效果。

对于序贯重要性采样(SIS)算法而言，粒子数匮乏是其主要缺陷。粒子数匮乏是指随着迭代次数增加，粒子集中除了少数粒子具有较大权值以外，其余粒子的权值均可以忽略不计，粒子丧失多样性的现象，从而使得支撑粒子集不再能够有效地逼近状态的后验分布。Doucet从理论上证明了SIS算法出现粒子数匮乏现象的必然性，降低该现象影响的最有效方法是选择重要性密度函数和采用重采样方法。

Zaritskii已经证明最好的采样函数是状态的后验密度函数本身，并称其为最优采样函数。一般情况下，很难直接从后验概率密度函数中采样粒子，为此引入了容易采样的重要性密度函数(Importance Density)进行采样。重要性密度函数一般采用高斯密度函数，对于中等程度的非线性模型，单高斯密度函数运行结果非常有效，然而对于高维和深度的非线性模型，单高斯密度函数运行结果就比较差。为了解决深度非线性模型的采样函数选择问题，引入了高斯混合概率密度模型。另外针对如何从重要性密度函数的优化设计解决粒子退化问题，研究了基于非线性滤波修正的重要性密度函数方法。通过无迹卡尔曼(Unscented Kalman Filter，UKF)滤波算法与高斯混合密度函数模型的结合，设计一种新的粒子滤波重要性密度函数，由于融入了更多的最新观测信息，提高了所产生预测粒子的精度和稳定性，从而能够有效避免粒子退化、保持粒子的多样性，在观测噪声较大的环境下具有更好的状态估计精度。

重采样算法的研究改进是粒子滤波中的重要问题，其基本思想是减少或剔除小权值粒子，对大权值粒子则按照其权值大小进行复制。经典的重采样算法有多项式重采样(multi-nomial resample)、分层重采样(stratified resample)、系统重采样(systematic resample)和残余粒子重采样(residual resample)。但是经过重采样后，大权值粒子被多次赋值，粒子集的多样性丧失，又带来了样本贫化问题。在当前重采样算法的基础上，提出一种简单有效的自适应残差重采样算法。所提算法克服了传统重采样算法的不足，改善了粒子集的组成结构，从而更有效的表达系统状态的后验概率密度，在克服粒子退化现象的同时避免了粒子贫化问题。

非线性、非高斯的状态估计问题广泛存在于各种科学研究和工程实践中，粒子滤波则为该类问题提供了一种行之有效的解决方案，但是粒子滤波理论及算法的发展还不够完善，存在许多有待改进的问题，因此对粒子滤波算法的深入研究具有重要的理论意义和广泛的应用前景。

发明内容

本发明的目的在与从优化重要性密度函数和改进重采样性能二个角度出发，研究粒子滤波的优化改进算法，从而更有效的估计系统状态的后验概率分布，在克服粒子退化现象的同时避免了样本贫化的问题。

本发明的目的是通过以下技术方案来实现。

考虑如下的非线性离散时间系统的动态状态空间模型：

$\left\{\begin{array}{c}{s}_k=f_k(s_{k-1},w_{k-1})\\ z_k=h_k(s_k,v_k)\end{array}\right.---\left(1\right)$

其中k表示的时刻，s_k∈Rⁿ为系统状态向量，在已知初始状态分布p(s₀)的情况下，通过系统状态函数f(·)按时间传播。z_k∈R^m是条件独立的观测向量，在给定状态的情况下，依据观测似然函数p(z_k|s_k)产生。f_k:Rⁿ×R^r→Rⁿ是系统的非线性状态函数。h_k:Rⁿ×R^p→R^m是系统的观测函数。w_k-1∈R^r、v_k∈R^p分别为系统过程噪声和观测噪声。

粒子滤波是一种基于蒙特卡洛模拟和递推贝叶斯估计的近似后验概率密度方法，通过n个粒子状态和其对应的权值近似估计系统状态，相应的，系统的后验概率密度函数可以利用一系列随机粒子的加权和来近似表示：

$p\left(s_{0:k}\right|z_{1:k})\≈\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i\mathrm{\δ}(s_{0:k}-s_{0:k}^i)---\left(2\right)$

其中w是粒子对应的权重，i表示粒子数，N代表粒子总数。

粒子滤波采用序贯重要性采样(SIS)算法，对于SIS粒子数匮乏是其主要的缺陷，为了解决此问题，本发明将从重要性密度函数和重采样二个方面优化粒子滤波算法。

高斯混合无迹粒子滤波算法结合了高斯混合方法和无迹变换算法作为粒子滤波的重要性密度函数，提高了粒子滤波系统状态估计的准确性。假设在第k时刻s_k是系统的状态向量，其包括n个元素，对应的状态向量的均值和方差分别为和P_s。无迹变换的主要思想是选择2n+1个带权值的样本S＝{χ_j,w_j}来近似表达系统状态的后验概率密度函数，这些样本被称为χ点集。

对于时刻k，粒子i，无迹变换样本j，χ点集的定义如下：

${\mathrm{\χ}}_k^{\left(i\right)j}=\left\{\begin{array}{cc}{\stackrel{\‾}{s}}_k^i& j=0\\ {\stackrel{\‾}{s}}_k^i+{\left(\sqrt{(n+\mathrm{\λ})P_x}\right)}_j,& j=1,...n\\ {\stackrel{\‾}{s}}_k^i-{\left(\sqrt{(n+\mathrm{\λ})P_x}\right)}_j,& j=n+1,...,2n\end{array}\right.---\left(3\right)$

然而，对于每个χ点j，其相应的权值为：

$w^j=\left\{\begin{array}{c}{w}_m^0=\mathrm{\λ}/(n+\mathrm{\λ})\\ w_c^0=\mathrm{\λ}/(n+\mathrm{\λ})+(1-{\mathrm{\α}}^2+\mathrm{\β})\\ w_m^j=w_c^j=1/\left[2(n+\mathrm{\λ})\right],j=1,...,2n\end{array}\right.---\left(4\right)$

其中λ＝α²(n+κ)-n为尺度化后的比例参数，和分别表示χ点的均值权值和协方差权值。α确定了χ点集的分布情况，可用于调节χ点和均值的距离；β是非负加权参数，包含分布的高阶矩信息；κ为第二尺度参数，目的是保证方差阵的半正定性。

对于每个粒子i，粒子的状态向量通过无迹变换和高斯混合来得到更新。对于每一个时刻k，高斯混合无迹粒子滤波的步骤如下：

初始化：

根据高斯概率密度函数随机采样得到n个粒子，计算初始化粒子均值和方差P_s。

计算χ点集：

利用式(3)计算k(k＞0)时刻的χ点集

时间更新：

${\mathrm{\χ}}_{k|k-1}^{\left(i\right)j}=f\left({\mathrm{\χ}}_{k-1}^{\left(i\right)j}\right)---\left(5\right)$

${\stackrel{\‾}{s}}_{k|k-1}^{\left(i\right)}=\underset{j=0}{\overset{2n}{\mathrm{\Σ}}}{w}_m^j{\mathrm{\χ}}_{k|k-1}^{\left(i\right)j}---\left(6\right)$

$P_{s,k|k-1}^{\left(i\right)}=\underset{j=0}{\overset{2n}{\mathrm{\Σ}}}{w}_c^j{[{\mathrm{\χ}}_{k|k-1}^{\left(i\right)j}-{\stackrel{\‾}{s}}_{k|k-1}^{\left(i\right)}][{\mathrm{\χ}}_{k|k-1}^{\left(i\right)j}-{\stackrel{\‾}{s}}_{k|k-1}^{\left(i\right)}]}^T+Q_k---\left(7\right)$

${\mathrm{\γ}}_{k|k-1}^{\left(i\right)j}=h\left({\mathrm{\χ}}_{k-1}^{\left(i\right)j}\right)---\left(8\right)$

${\stackrel{\‾}{z}}_{k|k-1}^{\left(i\right)}=\underset{j=0}{\overset{2n}{\mathrm{\Σ}}}{w}_m^j{\mathrm{\γ}}_{k|k-1}^{\left(i\right)j}---\left(9\right)$

其中，χ是状态向量s的sigma点集，γ是观测向量z的sigma点集。

测量更新：

$P_{z,k|k-1}^{\left(i\right)}=\underset{j=0}{\overset{2n}{\mathrm{\Σ}}}{w}_c^j{[{\mathrm{\γ}}_{k|k-1}^{\left(i\right)j}-{\stackrel{\‾}{z}}_{k|k-1}^{\left(i\right)}][{\mathrm{\γ}}_{k|k-1}^{\left(i\right)j}-{\stackrel{\‾}{z}}_{k|k-1}^{\left(i\right)}]}^T+R_k---\left(10\right)$

$P_{\mathrm{sz},k}^{\left(i\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_c^j{[{\mathrm{\χ}}_{k|k-1}^{\left(i\right)j}-{\stackrel{\‾}{s}}_{k|k-1}^{\left(i\right)}][{\mathrm{\γ}}_{k|k-1}^{\left(i\right)j}-{\stackrel{\‾}{z}}_{k|k-1}^{\left(i\right)}]}^T---\left(11\right)$

$K_k=P_{\mathrm{sz},k}^{\left(i\right)}{\left(P_{z,k|k-1}^{\left(i\right)}\right)}^{-1}---\left(12\right)$

${\stackrel{\‾}{s}}_k^{\left(i\right)}={\stackrel{\‾}{s}}_{k|k-1}^{\left(i\right)}+K_k(z_k-{\stackrel{\‾}{z}}_{k|k-1}^{\left(i\right)})---\left(13\right)$

$P_{s,k}^{\left(i\right)}=P_{s,k|k-1}^{\left(i\right)}-K_k{P}_{z,k}^{\left(i\right)}{K}_k^T---\left(14\right)$

其中，K_k是第k个时刻粒子滤波器的增益，P表示的是对应上下标的方差。z_k代表真实的测量值，而表示根据无迹变换得到的测量估计值。

粒子权值计算：

高斯混合算法应用在粒子权值计算上，高斯混合公式中包含了先验状态信息、无迹变换后的状态信息和测量信息，其计算权值方法如下。

${\stackrel{\‾}{z}}_k^{\left(i\right)}=f\left({\stackrel{\‾}{s}}_k^{\left(i\right)}\right)---\left(15\right)$

$w_{1,k}^i=e^{-\frac{1}{2}{(z_k-{\stackrel{\‾}{z}}_k^{\left(i\right)})}^T\mathrm{inv}\left(R\right)(z_k-{\stackrel{\‾}{z}}_k^{\left(i\right)})}---\left(16\right)$

$w_{2,k}^i=e^{-\frac{1}{2}{({\stackrel{\‾}{s}}_k^{\left(i\right)}-s_k)}^T\mathrm{inv}\left(Q\right)({\stackrel{\‾}{s}}_k^{\left(i\right)}-s_k)}---\left(17\right)$

$w_k^i=w_{1,k}^i+w_{2,k}^i---\left(18\right)$

其中，Q和R分别是系统过程噪声和观测噪声的方差。

归一化粒子权值：

$w_k^i=\frac{w_k^i}{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i}---\left(19\right)$

最后，计算得到系统的状态

${\hat{s}}_k\≈\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\stackrel{\‾}{s}}_k^i{w}_k^i---\left(20\right)$

在粒子滤波算法中普遍存在的一个问题是粒子重要性权值的退化现象，随着粒子迭代次数的增加，粒子重要性权值也随机的增大或缩小，使得小部分粒子权值很大，而大部分粒子权值几乎为零，既浪费了系统资源，又不能精确的估计系统的状态。为改善这种现象，重采样被人们提了出来，然而，过重采样会导致粒子贫化现象。为提高重采样性能，本发明提出了一种简单而有效的自适应残差重采样方法。在传统重采样算法的基础上，本方法补偿了一些权值较小的支撑粒子，进而提高粒子的多样性。自适应残差重采样方法步骤如下：

1、一般根据有效样本数来衡量粒子集的退化程度，有效样本数的定义如下：

${\hat{N}}_{\mathrm{eff}}=\frac{1}{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\left(w_k^i\right)}^2}---\left(21\right)$

有效粒子数越小，表明退化现象越严重。一般在滤波过程中预先设定一个重采样阈值N_th，若则进行重采样。

2、对原始粒子集N表示粒子集总个数，用N与粒子集重要性权值相乘并取整，每个粒子得到映射的个子代，那么新映射的粒子集的粒子总数为：得到的新的部分粒子集为：求出残余粒子数m＝N-N′，如果m＞0，就继续下一步，否则，转到最后一步。

3、对原始粒子集按照下式重新修正重要性粒子权值

$w_k^{i^{\′}}=({\mathrm{Nw}}_k^i-n_k^i)/m---\left(22\right)$

在新的粒子集我们从中选择粒子权值最大的前m个粒子作为支撑粒子。

${\{s_k^i,w_k^i\}}_{i^{\′}=1}^m{\max }_m{\{w_k^{i^{\′}},w_k^{i^{\′}}\}}_{i=1}^N---\left(23\right)$

4、输出最终的粒子集

${\left\{{\tilde{s}}_k^i\right\}}_{i^{\′}=1}^N=\{{\left\{s_k^i\right\}}_{i=1}^{N^{\′}},{\left\{s_k^i\right\}}_{i^{\′}=1}^m\}---\left(24\right)$

本发明的整体实现流程框图如图1所示。

本发明的有益效果在于：

本发明提出了一种适用于非线性非高斯模型采用自适应残差重采样的高斯混合无迹粒子滤波算法，采用高斯混合无迹粒子滤波作为重要性采样函数，自适应残差重采样更新粒子结构，提高了粒子滤波的性能。通过具体实施例中的仿真对比结果，可以看出和普通粒子滤波相比，本发明所提算法在非线性目标跟踪模型中具有更好的一致性和鲁棒性。

附图说明

图1采用自适应残差重采样的高斯混合无迹粒子滤波算法的流程框图；

图2发明算法和普通单高斯粒子滤波算法目标位置跟踪结果对比图；(a)单高斯普通粒子滤波算法目标位置跟踪结果；(b)高斯混合粒子滤波算法(发明算法)目标位置跟踪结果；

图3发明算法和普通单高斯粒子滤波算法x方向跟踪结果对比图；(a)单高斯普通粒子滤波算法x方向跟踪结果；(b)高斯混合粒子滤波算法(发明算法)x方向跟踪结果；

图4发明算法和普通单高斯粒子滤波算法y方向跟踪结果对比图；(a)单高斯普通粒子滤波算法y方向跟踪结果；(b)高斯混合粒子滤波算法(发明算法)y方向跟踪结果；

图5两种算法的状态估计均方根误差trMSE对比结果；

图6两种算法的有效粒子数对比结果。

具体实施方式

下面给出本发明的具体实施例，说明本发明的有效性。

考虑如下的非线性离散时间系统的动态状态空间模型：

$\left\{\begin{array}{c}{s}_k=f_k(s_{k-1},w_{k-1})\\ z_k=h_k(s_k,v_k)\end{array}\right.$

其中s_k∈Rⁿ为系统状态向量，在已知初始状态分布p(s₀)的情况下，通过系统状态函数f(·)按时间传播。z_k∈R^m是条件独立的观测向量，在给定状态的情况下，依据观测似然函数p(z_k|s_k)产生。f_k:Rⁿ×R^r→Rⁿ是系统的非线性状态函数。h_k:Rⁿ×R^p→R^m是系统的观测函数。w_k-1∈R^r、v_k∈R^p分别为系统过程噪声和观测噪声。

状态空间模型f_k：

为了证明本发明在非线性系统的优良性能，考虑状态函数f_k用随机游走动态模型：

$s_{k+1}=\left[\begin{array}{cccc}1& 0& \mathrm{\Δt}& 0\\ 0& 1& 0& \mathrm{\Δt}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]s_k+\left[\begin{array}{cc}\frac{{\mathrm{\Δt}}^2}{2}& 0\\ 0& \frac{\mathrm{\Δ}{t}^2}{2}\\ \mathrm{\Δt}& 0\\ 0& \mathrm{\Δt}\end{array}\right]\left[\begin{array}{c}{w}_k^x\\ w_k^y\end{array}\right]$

其中Δt是采样间隔，和是随着时间k改变的零均值不相关高斯加速度。在该模型中状态向量s_k的表示形式如下所示：

$s_k={[x,y,v_x,v_y]}_k^T$

(x,y)是目标的坐标，v_x和v_y是x轴、y轴方向的速度。

观测空间模型h_k：

为了观测运动目标的坐标和速度，观测空间引入距离和距率来作为观测向量。距离R表示的是运动目标和观测点之间的距离，定义如下：

R＝[(x_v-x_o)²+(y_v-y_o)²]^1/2

(x_o,y_o)是观测点的坐标，保持不变；(x_v,y_v)是运动目标的坐标。

距率也被称为多普勒速率或者是径向速度，可以由距离公式得到，其定义为：

$\begin{array}{c}\stackrel{\·}{R}=\frac{\mathrm{dR}}{\mathrm{dt}}=\frac{2[(x_v-x_o)v_v^x+(y_v-y_o)v_v^y]}{2{[{(x_v-x_o)}^2+{(y_v-y_o)}^2]}^{1/2}}\\ =\frac{[(x_v-x_o)v_v^x+(y_v-y_o)v_v^y]}{R}\end{array}$

其中，和是运动目标的速度。

为了得到更多的观测信息，在测量模型中采用二个静态的观测点。观测向量定义如下：

$z_k={[R_a,{\stackrel{\·}{R}}_a,R_b,{\stackrel{\·}{R}}_b]}_k$

R_a和R_b是观测点a或观测点b到运动目标的距离，和表示的是观测点相对于运动目标的距率。

根据以上建立的离散动态模型方程，通过蒙特卡洛仿真证明发明算法的性能。除了采用自适应残差重采样的高斯混合无迹粒子滤波算法，同时还实现了对普通单高斯粒子滤波的仿真试验，以作对比。试验参数的初始化设置和无迹变换参数设置如表1所示。

表1：试验参数设置

试验结果及分析

图1是普通单高斯粒子滤波和所提算法目标跟踪与实际运动轨迹在二维空间中的对比结果。根据图1所示的结果，在随机游走非线性状态模型中，所提算法与实际目标轨迹具有更好的一致性，状态估计准确性高，而普通单高斯粒子滤波随着迭代次数的增加，跟踪轨迹出现了偏差。

图2和图3分别是对x轴和y轴的跟踪结果，与实际目标轨迹对比，所提算法依然是具有较高的一致性。

为了从数值上对所发明算法进行评估，采用状态估计均方根误差，其定义如下：

$\mathrm{trMSE}=\mathrm{diag}({(\tilde{S}-S)}^{\′}\×(\tilde{S}-S))$

其中S表示所有迭代的目标实际状态向量值，大小是na×endk，同样的，是状态的估计向量值。图4表示的是普通单高斯粒子滤波和所提算法的状态估计均方根误差对比图。从图4结果可以很明显的看出所提算法的状态均方误差小于普通单高斯粒子滤波算法。这说明所提算法有更好的收敛性和鲁棒性。

有效粒子数目可以用来衡量粒子集的退化程度，式(21)给出了有效粒子数的定义。一般的有效粒子数越小，粒子集退化越严重。图5给出了有效粒子集的仿真结果，可以看出所提算法的有效粒子数多于普通单高斯粒子滤波的有效粒子数结果表明，所提算法能够更有效的克服粒子集的退化现象。

通过20次蒙特卡洛仿真试验，计算得到了不同变量的平均值，包括运行时间、x方向状态估计的均方根误差、y方向状态估计的均方根误差和有效粒子数。一次仿真试验中，x和y单方向的状态估计的均方根误差定义为：

$\mathrm{RMSE}=\sqrt{\frac{1}{T}\underset{k=1}{\overset{T}{\mathrm{\Σ}}}{(x_k-\hat{x})}^2}$

其中T为总迭代步数，x_k为目标真实值，为状态的估计值。仿真时间结果如表2所示。

表2 20次蒙特卡洛仿真试验结果

从表2的试验结果可以看出，在时间上，所提算法比普通单高斯粒子滤波算法稍微长一些，在以后的工作中，可以针对效率进行改进。在x、y单方向的状态估计的均方根误差上，所提算法明显小于普通单高斯粒子滤波算法，在有效粒子数上，所提算法的有效粒子数高于普通单高斯粒子滤波算法，结果表明，所提算法在非线性目标跟踪模型中具有较高的准确性和鲁棒性。

Claims (2)

1.一种采用自适应重采样的高斯混合无迹粒子滤波算法，用于目标跟踪，其特征在于具体实现步骤如下：

步骤一：建立目标跟踪的系统模型：状态方程和观测方程 $\left\{\begin{array}{c}{s}_k=f_k(s_{k-1},w_{k-1})\\ z_k=h_k(s_k,v_k)\end{array}\right.;$

其中k表示的时刻，s_k∈Rⁿ为系统状态向量，在已知初始状态分布p(s₀)的情况下，通过系统状态函数f(·)按时间传播；z_k∈R^m是条件独立的观测向量，在给定状态的情况下，依据观测似然函数p(z_k|s_k)产生；f_k:Rⁿ×R^r→Rⁿ是系统的非线性状态函数；h_k:Rⁿ×R^p→R^m是系统的观测函数；w_k-1∈R^r、v_k∈R^p分别为系统过程噪声和观测噪声；

步骤二：初始化，k＝0，根据先验概率分布p(s₀)建立初始状态样本集其中权值为 ${\left\{w_0^i\right\}}_{i=1}^N=\frac{1}{N};$

步骤三：k＝k+1，根据观测模型，计算本时刻的观测值z_k；

步骤四：利用无迹变换更新每个粒子的状态和方差 $P_{s,k}^{\left(i\right)}=P_{s,k|k-1}^{\left(i\right)}-K_k{P}_{z,k}^{\left(i\right)}{K}_k^T;$

其中，K_k是第k时刻粒子滤波器的增益，P表示的是对应上下标的方差；z_k代表真实的测量值，而表示根据无迹变换得到的测量估计值；

步骤五：根据高斯混合方法，预测粒子集权重 $w_k^i=e^{-\frac{1}{2}{(z_k-{\stackrel{\‾}{z}}_k^{\left(i\right)})}^T\mathrm{inv}\left(R\right)(z_k-{\stackrel{\‾}{z}}_k^{\left(i\right)})}+e^{-\frac{1}{2}{({\stackrel{\‾}{s}}_k^{\left(i\right)}-s_k)}^T\mathrm{inv}\left(Q\right)({\stackrel{\‾}{s}}_k^{\left(i\right)}-s_k)},$ 归一化重要性权重 $w_k^i=w_k^i/\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i,$ 获得粒子集 ${\{s_k^{i^{\′}},w_k^{i^{\′}}\}}_{i=1}^N;$

其中，Q和R分别是系统过程噪声和观测噪声的方差；

步骤六：利用得到的粒子集对后验概率分布进行估计，得到系统状态

步骤七：对原始粒子集采用自适应重采样获得优化后的粒子集，权值为

步骤八：转到步骤三。

2.根据权利要求1所述的采用自适应重采样的高斯混合无迹粒子滤波算法，其特征在于：系统状态模型和观测模型的建立与实现包括如下步骤：

(1)系统状态模型

为验证算法的非线性适应性，系统模型采用了随机游走模型，该模型具有不确定性和深度非线性性；

随机游走动态模型如下：

$s_{k+1}=\left[\begin{array}{cccc}1& 0& \mathrm{\Δt}& 0\\ 0& 1& 0& \mathrm{\Δt}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]s_k+\left[\begin{array}{cc}\frac{{\mathrm{\Δt}}^2}{2}& 0\\ 0& \frac{{\mathrm{\Δt}}^2}{2}\\ \mathrm{\Δt}& 0\\ 0& \mathrm{\Δt}\end{array}\right]\left[\begin{array}{c}{w}_k^x\\ w_k^y\end{array}\right]$

其中和是随着时间k改变的零均值不相关的高斯加速度，Δt是时间间隔，s_k是系统的状态向量，其表示形式如下：

$s_k={[x,y,v_x,v_y]}_k^T$

式中，(x,y)是目标的坐标，v_x和v_y是x轴、y轴方向的速度；

(2)系统观测模型

观测模型中引入距离和距率作为观测向量；距离R表示的是运动目标和观测点之间的距离，距率也被称为多普勒速率或者是径向速度；

为了验证系统方案，在观测模型中采用二个静态的观测点，观测向量定义如下：

$z_k={[R_a,{\stackrel{.}{R}}_a,R_b,{\stackrel{.}{R}}_b]}_k$

其中，R_a和R_b是观测点a或观测点b到运动目标的距离，和表示的是观测点相对于运动目标的距率。