CN103684350B - 一种粒子滤波方法 - Google Patents

Abstract

本发明提供一种粒子滤波方法，其包括：步骤1，初始化粒子；步骤2，在k时刻获取测量值，然后利用粒子滤波方法由N个粒子滤波过程并行计算均值和方差，然后进行近似处理获得重要性密度函数并抽取采样粒子；步骤3，根据步骤2获得的重要性密度函数，计算每一个采样粒子的重要性权值；步骤4，将步骤3中得到的重要性权值进行归一化处理；步骤5，根据步骤4中归一化处理后得到的权值进行重采样，得到新的粒子序列；步骤6，对步骤5得到的新的粒子序列xik计算后验概率密度，输出滤波结果。本发明的计算过程简单，能在一定程度上改善粒子退化问题，提高了粒子滤波性能。

Description

一种粒子滤波方法

技术领域

本发明属于非线性滤波技术领域，尤其涉及一种粒子滤波方法。

背景技术

滤波是伴随着信号传递产生的一门技术，信号的传递过程不可避免的要受到内、外部干扰的影响，为了获取所需信号，排除干扰，就要对信号进行滤波。对于非线性系统，通过贝叶斯估计得到精确的最优滤波解是很困难的。常用的非线性滤波方法主要有扩展Kalman滤波（EKF）和无迹Kalman滤波（UKF）等，然而这两种非线性滤波方法会受到非线性程度以及噪声类型的限制。于是又有学者提出了应用范围较广的粒子滤波（PF）算法。粒子滤波是一种基于贝叶斯估计原理的序贯MonteCarlo模拟方法，核心是利用一些随机粒子来表示系统随机变量的后验概率密度，以得到物理模型的近似最优数值解，是一种顺序重要性采样法。简单来说，粒子滤波法是指通过寻找一组在状态空间传播的随机样本对概率密度函数进行近似，以样本均值代替积分运算，从而获得状态最小方差分布的过程。但是，粒子滤波随着粒子的不断迭代，会出现粒子退化现象，使滤波性能变差。

发明内容

为解决上述问题，本发明提供一种粒子滤波方法，该方法由粒子滤波产生重要性密度函数，在一定程度上改善了粒子退化问题，提高了粒子滤波性能。

本发明的粒子滤波方法包括以下步骤：

步骤1，初始化粒子且粒子权值为其中，x₀为初始时刻t₀的粒子集合，为初始时刻t₀第i个状态向量，N为产生的粒子数量，p(x₀)为初始概率密度函数；

步骤2，在k时刻获取测量值y_k，根据k-1时刻粒子的集合x_k-1和测量值y_k递推出x_k

x_k-1和测量值y_k利用粒子滤波方法由N个粒子滤波过程并行计算x_k的均值和方差然后由式（1）进行近似处理获得重要性密度函数：

$q\left(x_k^i\right|x_{k-1}^i,y_k)=N({\stackrel{\‾}{x}}_k^i,{{\stackrel{\‾}{P}}_k}^i)---\left(1\right);$

并根据式（2）抽取采样粒子：

$\begin{array}{cc}{\hat{x}}_k^i={\stackrel{\‾}{x}}_k^i+{{\stackrel{\‾}{P}}_k}^{1/2}{\mathrm{\γ}}^i& {\mathrm{\γ}}^i~N(0,I)\end{array}---\left(2\right);$

步骤3，根据步骤2获得的重要性密度函数，利用式（3）计算每一个采样粒子的重要性权值：

${\widetilde{\mathrm{\ω}}}_k^i={\mathrm{\ω}}_{k-1}^i\frac{p\left(y_k\right|x_k^i)p\left(x_k^i\right|x_{k-1}^i)}{q\left(x_k^i\right|x_{k-1}^i,y_k)}---\left(3\right);$

步骤4，将步骤3中得到的重要性权值利用式（4）进行归一化处理：

${\mathrm{\ω}}_k^i=\frac{{\widetilde{\mathrm{\ω}}}_k^i}{\mathrm{sum}{\left\{{\widetilde{\mathrm{\ω}}}_k^i\right\}}_{i=1}^N}---\left(4\right);$

步骤5，根据步骤4中归一化处理后得到的权值进行重采样，得到新的粒子序列

步骤6，按公式对步骤5得到的新的粒子序列计算后验概率密度，输出滤波结果；

进一步的， $x_k=0.5x_{k-1}+\frac{25x_{k-1}}{1+x_{k-1}^2}+8\cos \left(1.2(k-1)\right)+w_k$ 是所用非线性模型的状态方程，是观测方程，其中，w_k,v_k分别为过程噪声和观测噪声，均为零均值高斯白噪声，且w_k，v_k的方差分别假设为1，初始状态为x₀=0.1。

进一步的，所述步骤5中还可以采用精细重采样方法得到新的粒子序列。

本发明的有益效果在于：

本发明提出由粒子滤波产生重要性密度函数的重要性采样方法，与由扩展Kalman滤波、无迹Kalman滤波产生重要性密度函数的重要性采样方法相比较，本发明计算过程简单，精度较高，能在一定程度上改善粒子退化问题，提高了粒子滤波性能。

附图说明

图1为本发明的EKF-PF，UKF-PF以及PF-PF的状态估计示意图；

图2为本发明的EKF-PF，UKF-PF以及PF-PF的RMSE示意图；

图3为本发明的采用精细重采样的EKF-PF，UKF-PF以及PF-PF的状态估计示意图；

图4为本发明的采用精细重采样的EKF-PF，UKF-PF以及PF-PF的RMSE示意图；

图5为本发明的不采用精细重采样的PF-PF，EKF，UKF以及PF的状态估计示意图；

图6为本发明的不采用精细重采样的PF-PF，EKF，UKF以及PF的RMSE示意图。

具体实施方式

重要性采样方法（即获取重要性密度函数的方法）对粒子滤波的效果有较大的影响，若重要性采样方法不合适，会导致严重的粒子退化，影响滤波效果。现有的粒子滤波算法中，产生重要性密度函数的方法主要有EKF和UKF，但是这两种方法计算复杂，精度受限。本发明提供粒子滤波方法，其利用改进的粒子滤波算法，由粒子滤波产生重要性密度函数，得到一种新的滤波方法，称为PF-PF。

该方法步骤如下：

步骤1，初始化粒子，粒子权值其中，x0为初始时刻（t₀时刻）粒子的集合。为t₀时刻第i个状态向量（称为粒子），N为产生的粒子的数量，p(x₀)为初始概率密度函数。

步骤2，重要性采样，在k时刻，获取测量值yk。根据x_k-1和测量值y_k，利用粒子滤波方法，由N个粒子滤波过程并行计算x_k的均值和方差然后重要性密度函数由式（1）近似：

$q\left(x_k^i\right|x_{k-1}^i,y_k)=N({\stackrel{\‾}{x}}_k^i,{{\stackrel{\‾}{P}}_k}^i)---\left(1\right)$

并根据式（2）抽取采样粒子：

$\begin{array}{cc}{\hat{x}}_k^i={\stackrel{\‾}{x}}_k^i+{{\stackrel{\‾}{P}}_k}^{1/2}{\mathrm{\γ}}^i& {\mathrm{\γ}}^i~N(0,I)\end{array}---\left(2\right).$

其中PF，EKF，UKF重要性密度函数都是（1）式，只不过估计和的方式不同，分别利用EKF,UKF和PF来估计和

步骤3，根据步骤2得出的重要性密度函数，计算每一个采样粒子的重要性权值：

${\widetilde{\mathrm{\ω}}}_k^i={\mathrm{\ω}}_{k-1}^i\frac{p\left(y_k\right|x_k^i)p\left(x_k^i\right|x_{k-1}^i)}{q\left(x_k^i\right|x_{k-1}^i,y_k)}$

步骤4，将步骤3中得到的权值归一化：

${\mathrm{\ω}}_k^i=\frac{{\widetilde{\mathrm{\ω}}}_k^i}{\mathrm{sum}{\left\{{\widetilde{\mathrm{\ω}}}_k^i\right\}}_{i=1}^N}$

步骤5，根据得到的权值，进行重采样，得到新的粒子序列

步骤6，按式计算后验概率密度，输出滤波结果，后验概率密度就是最终滤波结果。

其中，x_k-1的获得：由于x₀已知，则可以根据步骤2至步骤6，由x₀以及观测值y₁可递推出x₁，由x₁及y₂递推出x₂。

由于利用粒滤波算法对于粒子均值和方差的估计精度较EKF和UKF更高，因此PF生成的重要性分布与真实的后验概率分布更接近，从而提高采样质量。因此PF-PF算法能够有效防止粒子退化。

此外，步骤5还可以采用精细重采样方法，进一步提高PF-PF的滤波性能。精细重采样方法与传统重采样方法相比，优点是：1、在保留或淘汰粒子上尽可能的保留了权重更大的粒子；2、根据母粒子产生新粒子，不单纯完全复制粒子。这两方面的处理可以有效地改善粒子退化问题。

将由PF产生重要性密度函数，与由EKF、UKF产生重要性密度函数的重要性采样方法进行比较，对采用三种重要性采样方法的粒子滤波算法进行了一系列仿真实验。由EKF产生重要性密度函数的粒子滤波方法（EKF-PF）、由UKF产生重要性密度函数的粒子滤波方法（UKF-PF）、由PF产生重要性密度函数的粒子滤波方法（PF-PF）的状态估计情况如图1、图2所示，验证了采用新的重要性采样方法的粒子滤波的状态估计值更接近真实状态，RMSE更小。

为了进一步提高滤波的精度，设计了由PF、EKF、UKF产生重要性密度函数，同时，重采样阶段分别采用精细重采样方法的一组仿真实验，EKF-PF，UKF-PF，PF-PF的状态估计情况如图3、图4所示，可以看出，PF-PF在三种滤波方法中性能最好，同时从RMSE的大小情况来看，采用更好的重采样方法能够在一定程度上改进滤波性能。

此外，将PF-PF方法与传统的EKF，UKF以及PF进行对比，结果如图5、图6所示，可以明显看出PF-PF滤波性能的优越性，即图中可以看出，PF-PF有更小的RMSE，也就是说明有更好的滤波性能。

仿真的参数设置为：

所用非线性模型的状态方程为 $x_k=0.5x_{k-1}+\frac{25x_{k-1}}{1+x_{k-1}^2}+8\cos \left(1.2(k-1)\right)+w_k.$

观测方程为为 $y_k=\frac{x_k^2}{20}+v_k.$

其中，w_k,v_k分别为过程噪声和观测噪声，均为零均值高斯白噪声，且w_k，v_k的方差分别假设为1，初始状态为x₀=0.1。

当然，本发明还可有其他多种实施例，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (1)

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

步骤1，初始化粒子且粒子权值为其中，x₀为初始时刻t₀的粒子集合，为初始时刻t₀第i个状态向量，N为产生的粒子数量，p(x₀)为初始概率密度函数；

步骤2，在k时刻获取测量值y_k，根据k-1时刻粒子的集合x_k-1和测量值y_k

由N个粒子滤波过程并行计算x_k的均值和方差然后由式(1)进行近似处理获得重要性密度函数：

$q\left(x_k^i\right|x_{k-1}^i,y_k)=N({\stackrel{\‾}{x}}_k^i,{\stackrel{\‾}{P}}_k^i)---\left(1\right);$

其中，即是预测出的k时刻粒子第i个状态向量即第i个粒子的均值，是k时刻粒子第i个状态向量的方差，表示均值为方差为

并根据式(2)抽取采样粒子：

$\begin{array}{cc}{\hat{x}}_k^i={\stackrel{\‾}{x}}_k^i+{\stackrel{\‾}{P}}_k^{1/2}{\mathrm{\γ}}^i& {\mathrm{\γ}}^i~N(0,1)\end{array}---\left(2\right);$

其中，各粒子均值为0，各个粒子方差均为1，即标准正态分布；

步骤3，根据步骤2获得的重要性密度函数，利用式(3)计算每一个采样粒子的重要性权值：

${\widetilde{\mathrm{\ω}}}_k^i={\mathrm{\ω}}_{k-1}^i\frac{p\left(y_k\right|x_k^i)p\left(x_k^i\right|x_{k-1}^i)}{q\left(x_k^i\right|x_{k-1}^i,y_k)}---\left(3\right);$

步骤4，将步骤3中得到的重要性权值利用式(4)进行归一化处理：

${\mathrm{\ω}}_k^i=\frac{{\widetilde{\mathrm{\ω}}}_k^i}{sum{\left\{{\widetilde{\mathrm{\ω}}}_k^i\right\}}_{i=1}^N}---\left(4\right);$

步骤5，根据步骤4中归一化处理后得到的权值进行重采样，得到新的粒子序列

步骤6，按公式对步骤5得到的新的粒子序列计算后验概率密度，输出滤波结果；

其中，x_0:k是从采样时刻0到采样时刻k，各个时刻粒子的集合，用来表示第0次采样到第k次采样的状态值；y_1:k表示1时刻到k时刻各个时刻状态的测量值；而则表示0时刻到k时刻，每一时刻第i个粒子；而δ表示采样时间间隔，即x_k与x_k-1之间的时间差；

进一步的，是所用非线性模型的状态方程，是观测方程，其中，w_k,v_k分别为过程噪声和观测噪声，均为零均值高斯白噪声，且w_k，v_k的方差分别假设为1，初始状态为x₀＝0.1。