CN102082560A - 一种基于集合卡尔曼滤波的粒子滤波方法 - Google Patents

Abstract

本发明提出一种基于集合卡尔曼滤波的粒子滤波方法，包括初始化采样粒子、对k时刻的背景集合进行预测、计算卡尔曼增益、融合最近观测信息，对背景集合进行更新、重新计算分析集合、构建高斯建议分布函数和权值进行归一化处理等步骤。本发明提出的基于集合卡尔曼滤波的粒子滤波方法无须对非线性系统进行线性化，避免了Jacobian矩阵的计算，使用采样法近似非线性分布，提高了计算精度。且本发明的粒子滤波方法中采样点数是启发式的，可以灵活设定，计算量不会随维数增大发生激增，实时性得到有效控制。

Description

一种基于集合卡尔曼滤波的粒子滤波方法

技术领域

本发明属于非线性滤波技术领域，具体涉及一种基于集合卡尔曼滤波的粒子滤波方法。

背景技术

非线性滤波的基本任务就是要从受噪声污染的观测量中去递推地估计非线性系统不可观测的状态值，它在自动控制、信号处理、目标跟踪、人工智能以及导航制导等诸多领域具有广泛的应用。对于如下非线性离散系统：

x_k＝f(x_k-1，v_k-1)

z_k＝h(x_k，w_k)

其中

为非线性离散系统在k时刻的状态，为非线性离散系统在k时刻的观测向量；v_k-1∈Rⁿ为非线性离散系统过程噪声，w_k∈R^m为k时刻的观测噪声；映射

和

都是有界非线性函数，分别代表非线性离散系统的状态和观测模型。滤波的目的就是要获得非线性离散系统后验分布p(x_k|z_1:k)，继而得到系统状态的统计特性，如均值、最大后验概率和置信区间等。

最著名的非线性滤波方法是扩展卡尔曼滤波(extened kalman filter，EKF)，其基本思想是使用泰勒展开对非线性系统进行线性化，但这种线性化误差较大，而且许多实际问题中很难得到非线性函数的Jacobian矩阵。近年来出现了一些无需计算Jacobian矩阵的非线性滤波方法，如无迹卡尔曼滤波器(unscented kalman filter，UKF)，均差滤波器(divided difference filter，DDF)，中心差分滤波器(central difference filter，CDF)等，这些方法的共同问题是在非线性、非高斯性较强时收敛性急剧下降甚至发散，并且处理高维问题的能力较差。

Hammersley等在20世纪50年代末就提出了基本的SIS方法，并在60年代使其得到了进一步发展，但始终未能解决粒子数匮乏现象和计算量制约等问题，因此未引起人们的重视。根据参考文献1：胡士强，敬忠良，“粒子滤波算法综述”，《控制与决策》，2005，vol20(4)，pp.361-366的记载，直到1993年由Gordon等提出了一种新的基于SIS的Bootstrap 非线性滤波方法，从而奠定了粒子滤波(particle filter，PF)算法的基础。粒子滤波是指：通过寻找一组在状态空间中传播的随机样本对后验概率密度函数进行近似，以样本均值代替积分运算，从而获得状态最小方差估计的过程，这些样本即称为“粒子”。粒子滤波可以完整的反映状态的后验分布，容易的得到如均值、模和方差等统计特征，适用于任何分布的非线性系统。

对于粒子滤波算法而言，粒子数匮乏是其主要缺陷。根据参考文献2：程水英，张剑云，“粒子滤波评述”，《宇航学报》，2008，vol 29(4)，pp.1099-1110的记载，粒子数匮乏是指随着迭代次数增加，粒子丧失多样性的现象。Doucet从理论上证明了SIS算法出现粒子数匮乏现象的必然性。降低该现象影响的方法有两个：选择建议分布函数和采用重采样方法。而其中最有效的就是选择恰当的建议分布函数，使后验概率密度函数和似然函数具有更大的重叠区域。

公开号为CN101055563的专利《基于多建议分布的粒子滤波方法》中公开了一种基于多建议分布的粒子滤波方法。该发明在粒子滤波器的框架内使用多种建议分布：先验概率分布、扩展卡尔曼滤波器、unscented卡尔曼滤波器等，采用分治采样策略来处理样本粒子，把总的粒子数分成多个部分，分别从不同的建议分布中提取，这样可以降低粒子滤波器的运行时间，提高运行效率而不损失粒子滤波器的估计精度。用户可以根据自己对时间和精度的需求进行参数设置。公开号为CN101436251的专利《基于拟蒙特卡罗采样的并行高斯粒子滤波方法》公开了一种基于拟蒙特卡罗采样的并行高斯粒子滤波方法。该发明主要解决目前基于蒙特卡罗采样的滤波方法在样本数较少时对非线性动态系统状态估计性能降低的问题。其滤波步骤包括并行拟蒙特卡罗序列产生步骤和并行高斯粒子滤波步骤，即首先采用跳跃拟蒙特卡罗序列产生方法，并行产生后续滤波所需的P条拟蒙特卡罗随机样本序列；然后分别通过P个执行单元，将每条拟蒙特卡罗随机样本序列转换为服从指定高斯分布的拟高斯样本序列；同时对非线性动态系统的状态进行时间更新和状态更新；最后对所有执行单元进行综合处理，完成对非线性动态系统的滤波。Merwe R V和Doucet A在《The Unscented Particle Filter》中提出了一种基于无迹卡尔曼滤波(unscented kalman filter，UKF)的新粒子滤波方法-无迹粒子滤波(unscented particle filter，UPF)，利用无迹卡尔曼滤波的最大后验概率估计产生粒子滤波的重要性密度函数，使重要性密度函数能够融入最新观测信息的同时，更加符合真实状态的后验概率分布，提高了算法性能。然而无迹卡尔曼滤波在选取Sigma点集时，采样点数与状态维数相关，随维数增大计算量上升比较快，如何在提高算法精度的同时保证其实时性，是当前粒子滤波研究亟待解决的问题。

发明内容

针对现有技术中存在的问题，本发明提出一种基于集合卡尔曼滤波的粒子滤波方法，利用集合卡尔曼滤波(简称为EnKF)产生粒子滤波每一时刻的高斯建议分布函数，提供一种提高算法精度并且保证计算效率的粒子滤波方法，其本质是一种基于蒙特卡洛方法的卡尔曼滤波，通过使用集合卡尔曼滤波(ensemble kalman filter，EnKF)产生粒子滤波的高斯建议分布函数，即通过融合每个时刻的最近观测信息对粒子的后验概率密度的均值和方差进行递归高斯估计：

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

其中p(x_k|z_1:k)是k时刻系统的后验分布函数，p_N(x_k|z_1:k)是它的高斯估计，

表示以

为均值以

为方差的高斯分布，式(1)也就是利用高斯分布来近似真实的后验分布。

在粒子滤波的框架中，对每个粒子使用一个独立的集合卡尔曼滤波产生和传递的后验高斯建议分布函数：

$q\left(x_k^{\left(i\right)}\right|x_{1:k-1}^{\left(i\right)},z_{1:k})=N({\hat{x}}_k^{\left(i\right)},{\hat{P}}_k^{\left(i\right)})---\left(2\right)$

其中

为每个粒子的建议分布函数，用来替代真实后验分布函数，

和

分别表示第i个粒子高斯分布的均值和方差，即将集合卡尔曼滤波的高斯估计作为每个粒子的建议分布。本发明提出的基于集合卡尔曼滤波的粒子滤波方法在计算粒子权值前融入了最新观测信息，又避免了对非线性系统的线性化，另外不同于无迹卡尔曼滤波采样点数固定，集合卡尔曼滤波的集合采样点数可以自由选取，所以产生的高斯建议分布函数更接近真实后验概率分布，提高了精度，而且实时性也得到了有效保障。

本发明提出的一种基于集合卡尔曼滤波的粒子滤波方法，主要包括以下几个步骤：

步骤一：在k＝0时刻初始化粒子，利用先验概率p(x₀)进行随机采样，获得初始采样粒子

初始化每个采样粒子的权值为1/N，i代表粒子序号，i＝1∶N，N为采样粒子个数，k为非线性系统的运行时刻。对于k＝0时刻每一个采样粒子定义初始的分析集合

为

其中表示k＝0时刻第i个粒子对应的分析集合中第j个样本，j代表分析集合中样本序号，

表示以k＝0时刻第i个粒子状态值为均值，以系统过程噪声协方差Q为方差的高斯分布，即初始分析集合由初始粒子采样得到。

步骤二：(1)在k＝1时刻，对每一个采样粒子分别定义一个背景集合

为和一个分析集合

为

所述的背景集合是由上一时刻(即k＝0时刻)的分析集合通过非线性系统预测得到的，分析集合由背景集合融合最近观测信息更新得到，所述的最近观测信息是指当前时刻(即该k＝1时刻)的观测向量，由非线性系统确定，其中上标a和b分别代表分析集合和背景集合，j代表分析集合或背景集合中样本序号，n表示分析集合或背景集合中样本的个数。表示k时刻背景集合中第j个样本，

表示k时刻分析集合中第j个样本。

(2)利用非线性系统的状态方程由k-1时刻的分析集合对当前背景集合中每个样本进行预测，得到公式(3)

$x_{k,j}^{\left(i\right),b}=f\left(x_{k-1,j}^{\left(i\right),a}\right)---\left(3\right)$

为非线性系统状态方差，

表示k-1时刻分析集合中第j个样本。

计算k时刻背景集合的中所有样本的均值

非线性系统关于状态变量的协方差

以及观测方程的协方差

得到公式(4)、(5)和(6)：

${\hat{x}}_k^{\left(i\right),b}=\frac{1}{n}\underset{j-1}{\overset{n}{\mathrm{\Σ}}}{x}_{k,j}^{\left(i\right),b}---\left(4\right)$

${\hat{P}}_{\mathrm{xh}}^{\left(i\right),k}=\frac{1}{n-1}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}(x_{k,j}^{\left(i\right),b}-{\hat{x}}_k^{\left(i\right),b}){(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right))}^T---\left(5\right)$

${\hat{P}}_{\mathrm{hh}}^{\left(i\right)k}=\frac{1}{n-1}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right)){(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right))}^T---\left(6\right)$

h(·)表示非线性系统的观测函数。

(3)根据公式(7)计算卡尔曼增益

$K_k^{\left(i\right)}={\hat{P}}_{\mathrm{xh}}^{\left(i\right),k}{({\hat{P}}_{\mathrm{hh}}^{\left(i\right),k}+R_k)}^{-1}---\left(7\right)$

其中

为卡尔曼增益，R_k为观测噪声的协方差矩阵。

(4)融合最近观测信息，对k时刻背景集合进行更新，计算得到k时刻的分析集合

(公式(8))；

$x_{k,j}^{\left(i\right),a}=x_{k,j}^{\left(i\right),b}+K_k^{\left(i\right)}(y_{k,j}-h\left(x_{k,j}^{\left(i\right),b}\right))---\left(8\right)$

其中y_k，j是以观测值y_k为均值、R_k为方差的高斯分布的采样。

根据公式(9)和(10)，计算分析集合的均值

和协方差

${\hat{x}}_k^{\left(i\right)a}=\frac{1}{n}\underset{j-1}{\overset{n}{\mathrm{\Σ}}}{x}_{k,j}^{\left(i\right)a}---\left(9\right)$

${\hat{P}}_k^{\left(i\right)a}=\frac{1}{n-1}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}(x_{k,j}^{\left(i\right)a}-{\hat{x}}_k^{\left(i\right)a}){(x_{k,j}^{\left(i\right)a}-{\hat{x}}_k^{\left(i\right)a})}^T---\left(10\right)$

这个过程利用上一时刻(即k-1时刻，即0时刻)的状态变量值和当前时刻的最新观测信息，通过集合卡尔曼滤波方法对建议分布函数高斯估计的均值和方差进行传递，得到该时刻(即k时刻)的高斯建议分布函数其中

表示k时刻第i个粒子，

表示k-1时刻第i个粒子，z_k表示k时刻观测向量。

步骤三：从步骤二得到的高斯建议分布函数中进行采样，根据公式(11)得到当前时刻(k时刻)的二次采样粒子，

表示k时刻第i个样本的二次采样样本，按照公式(12)计算每个二次采样粒子的权值对权值

进行归一化处理，得到归一化后的权值

${\hat{x}}_k^{\left(i\right)}~q\left(x_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)},z_k)=N({\hat{x}}_k^{\left(i\right),a},{\hat{P}}_k^{\left(i\right),a})---\left(11\right)$

${\mathrm{\ω}}_k^{\left(i\right)}={\mathrm{\ω}}_{k-1}^{\left(i\right)}\frac{p\left(z_k\right|{\hat{x}}_k^{\left(i\right)})p\left({\hat{x}}_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)})}{q\left({\hat{x}}_k^{\left(i\right)}\right|x_{0:k-1}^{\left(i\right)},z_{1:k})}---\left(12\right)$

步骤四：依照二次采样粒子的权值大小对步骤三中得到的二次粒子进行重采样，即抑制小权值粒子，复制大权值粒子，重采样后得到三次采样粒子。

步骤五：待估计非线性离散系统的运行时间为M，若k＝M则输出滤波结果；若k＜M，则迭代过程未完成，进入k+1时刻，重复步骤二至步骤四，直至非线性系统运行结束，使用采样加权平均值E(x_0:k)作为每一时刻算法输出值：

$E\left(x_{0:k}\right)={\∫x}_{0:k}\·p\left(x_{0:k}\right|z_{1:k}){\mathrm{dx}}_{0:k}\≈\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{x}_{0:k}^{\left(i\right)}---\left(13\right)$

输出值E(x_0:k)是对非线性系统进行滤波后的状态变量的估计值，其中0:k表示从0时刻到k时刻；x_0:k表示非线性系统从0到k时刻的状态值；p(x_0:k|z_1:k)表示非线性系统从0到k时刻的后验概率密度；

表示第i个粒子从0到k时刻的状态变量的值。

本发明的优点在于：

1、本发明提出的一种基于集合卡尔曼滤波的粒子滤波方法，在k时刻，使用集合卡尔曼滤波和最近观测信息对每个粒子的高斯建议分布的均值和方差进行更新，在k+1时刻从这个分布中采样得到新的粒子。本发明提出的基于集合卡尔曼滤波的粒子滤波方法无须对非线性系统进行线性化，避免了Jacobian矩阵的计算，使用采样法近似非线性分布，提高了计算精度。

2、本发明提出的一种基于集合卡尔曼滤波的粒子滤波方法，无论随机变量是否服从高斯分布，集合卡尔曼滤波对均值和方差的估计至少可以达到二阶水平。所以集合卡尔曼滤波可以对状态分布进行更好的高斯估计，产生的分布与后验概率分布的重叠区域更大。

3、本发明提出的一种基于集合卡尔曼滤波的粒子滤波方法，集合卡尔曼滤波的采样点数是启发式的，可以灵活设定，计算量不会随维数增大发生激增，实时性得到有效控制。

附图说明

图1：本发明所述集合卡尔曼粒子滤波器系统结构图；

图2：本发明与现有滤波器滤波方法的一次独立实验状态估计结果对比图；

图3：本发明与现有滤波器滤波方法的状态估计均方根误差对比图；

图4：本发明与现有滤波器滤波方法在不同粒子数时状态估计的均方根误差均值情况。

具体实施方式

下面将结合附图对本发明进行详细说明。

本发明提出的一种基于集合卡尔曼滤波的粒子滤波方法，如图1所示，主要包括以下几个步骤：

步骤一：在k＝0时刻初始化粒子，利用先验概率p(x₀)进行随机采样，获得初始采样粒子

初始化每个采样粒子的权值为1/N，i代表粒子序号，i＝1∶N，N为采样粒子个数，k为非线性系统的运行时刻，为k＝0时刻第i个采样粒子的状态值。对于k＝0时刻每一个采样粒子定义初始的分析集合

为

其中

表示k＝0时刻第i个粒子对应的分析集合中第j个样本，j代表分析集合中样本序号，

表示以k＝0时刻第i个粒子状态值为均值，以系统过程噪声协方差Q为方差的高斯分布，即初始分析集合由初始粒子采样得到。

步骤二：(1)在k＝1时刻，对每一个采样粒子分别定义一个背景集合

为

和一个分析集合

为

所述的背景集合是由上一时刻(即k＝0时刻)的分析集合通过非线性系统预测得到的，分析集合由背景集合融合最近观测信息更新得到，所述的最近观测信息是指当前时刻(即该k＝1时刻)的观测向量，由非线性系统确定，其中上标a和b分别代表分析集合和背景集合，j代表分析集合或背景集合中样本序号，n表示分析集合或背景集合中样本的个数。表示k时刻背景集合中第j个样本，

表示k时刻分析集合中第j个样本。

(2)利用非线性系统的状态方程由k-1时刻的分析集合对当前背景集合中每个样本进行预测，得到公式(3)

$x_{k,j}^{\left(i\right),b}=f\left(x_{k-1,j}^{\left(i\right),a}\right)---\left(3\right)$

为非线性系统状态方差，表示k-1时刻分析集合中第j个样本。

计算k时刻背景集合的中所有样本的均值

非线性系统关于状态变量的协方差以及观测方程的协方差

得到公式(4)、(5)和(6)：

${\hat{x}}_k^{\left(i\right),b}=\frac{1}{n}\underset{j-1}{\overset{n}{\mathrm{\Σ}}}{x}_{k,j}^{\left(i\right),b}---\left(4\right)$

${\hat{P}}_{\mathrm{xh}}^{\left(i\right),k}=\frac{1}{n-1}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}(x_{k,j}^{\left(i\right),b}-{\hat{x}}_k^{\left(i\right),b}){(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right))}^T---\left(5\right)$

${\hat{P}}_{\mathrm{hh}}^{\left(i\right)k}=\frac{1}{n-1}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right)){(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right))}^T---\left(6\right)$

h(·)表示非线性系统的观测函数。

(3)根据公式(7)计算卡尔曼增益

$K_k^{\left(i\right)}={\hat{P}}_{\mathrm{xh}}^{\left(i\right),k}{({\hat{P}}_{\mathrm{hh}}^{\left(i\right),k}+R_k)}^{-1}---\left(7\right)$

其中

为卡尔曼增益，R_k为观测噪声的协方差矩阵。

(4)融合最近观测信息，对k时刻背景集合进行更新，计算得到k时刻的分析集合

(公式(8))；

$x_{k,j}^{\left(i\right),a}=x_{k,j}^{\left(i\right),b}+K_k^{\left(i\right)}(y_{k,j}-h\left(x_{k,j}^{\left(i\right),b}\right))---\left(8\right)$

其中y_k，j是以观测值y_k为均值、R_k为方差的高斯分布的采样。

根据公式(9)和(10)，计算分析集合的均值和协方差

${\hat{x}}_k^{\left(i\right)a}=\frac{1}{n}\underset{j-1}{\overset{n}{\mathrm{\Σ}}}{x}_{k,j}^{\left(i\right)a}---\left(9\right)$

${\hat{P}}_k^{\left(i\right)a}=\frac{1}{n-1}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}(x_{k,j}^{\left(i\right)a}-{\hat{x}}_k^{\left(i\right)a}){(x_{k,j}^{\left(i\right)a}-{\hat{x}}_k^{\left(i\right)a})}^T---\left(10\right)$

这个过程利用上一时刻(即k-1时刻，即0时刻)的状态变量值和当前时刻的最新观测信息，通过集合卡尔曼滤波方法对建议分布函数高斯估计的均值和方差进行传递，得到该时刻(即k时刻)的高斯建议分布函数

其中

表示k时刻第i个粒子，

表示k-1时刻第i个粒子，z_k表示k时刻观测向量。

步骤三：从步骤二得到的高斯建议分布函数中进行采样，根据公式(11)得到当前时刻(k时刻)的二次采样粒子，

表示k时刻第i个样本的二次采样样本，按照公式(12)计算每个二次采样粒子的权值

对权值进行归一化处理，得到归一化后的权值

${\hat{x}}_k^{\left(i\right)}~q\left(x_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)},z_k)=N({\hat{x}}_k^{\left(i\right),a},{\hat{P}}_k^{\left(i\right),a})---\left(11\right)$

${\mathrm{\ω}}_k^{\left(i\right)}={\mathrm{\ω}}_{k-1}^{\left(i\right)}\frac{p\left(z_k\right|{\hat{x}}_k^{\left(i\right)})p\left({\hat{x}}_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)})}{q\left({\hat{x}}_k^{\left(i\right)}\right|x_{0:k-1}^{\left(i\right)},z_{1:k})}---\left(12\right)$

步骤四：依照二次采样粒子的权值大小对步骤三中得到的二次粒子进行重采样，即抑制小权值粒子，复制大权值粒子，重采样后得到三次采样粒子。

步骤五：待估计非线性离散系统的运行时间为M，若k＝M则输出滤波结果；若k＜M，则迭代过程未完成，进入k+1时刻，重复步骤二至步骤四，直至非线性系统运行结束，使用采样加权平均值E(x_0:k)作为每一时刻算法输出值；

$E\left(x_{0:k}\right)={\∫x}_{0:k}\·p\left(x_{0:k}\right|z_{1:k}){\mathrm{dx}}_{0:k}\≈\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{x}_{0:k}^{\left(i\right)}---\left(13\right)$

输出值E(x_0:k)是对非线性系统进行滤波后的状态变量的估计值，其中0:k表示从0时刻到k时刻；x_0:k表示非线性系统从0到k时刻的状态值；p(x_0:k|z_1:k)表示非线性系统从0到k时刻的后验概率密度；

表示第i个粒子从0到k时刻的状态变量的值。

所述的非线性系统的模型的标准算例如公式(14)所示：

x_k＝1+sin(ωπk)+0.5x_k-1+v_k-1

$y_k=\left\{\begin{array}{cc}0.2x_k^2+w_k,& k\≤30\\ 0.5x_k-2+w_k,& k>30\end{array}\right.---\left(14\right)$

其中x_k为非线性系统样本状态变量，y_k为非线性系统观测变量，系数ω＝6e-2，过程噪声v_k服从Gamma分布Gamma(4，3)，观测噪声w_k服从正态分布N(0，1e-4)。粒子滤波框架中的采样粒子样本个数为N＝100，使用残差采样法，将采样粒子的加权平均值作为每一时刻算法输出值。令集合卡尔曼滤波的集合采样个数(即背景集合或分析集合中的样本数)为5，运行时间为M＝60(即运行步骤二至五的次数为60次)，每次随机起始并进行100次独立重复实验，滤波结束。

本发明提出的基于集合卡尔曼滤波的粒子滤波方法基于Matlab仿真实验，与现有滤波器滤波方法如粒子滤波(PF)、卡尔曼粒子滤波(PF-EKF)和无迹粒子滤波(UPF)等的非线性系统的估计性能进行比较。仿真硬件环境均为Intel(R)Core2 Duo CPU T7250@2.00GHz，2G RAM，Windows XP操作系统。如图2所示的一次独立实验条件下得到的四种滤波方法的状态估计情况，可以看出卡尔曼粒子滤波PF-EKF、无迹粒子滤波UPF和本发明中基于集合卡尔曼滤波的粒子滤波都是用最大后验估计产生粒子滤波的建议分布函数，通过融入最新观测信息将先验分布向似然分布移动，估计性能优于标准粒子滤波方法。但由于卡尔曼粒子滤波使用一阶泰勒展开，忽略高阶项，这种线性化过程产生了较大误差，所以卡尔曼粒子滤波的估计性能低于无迹粒子滤波UPF和本发明的基于集合卡尔曼滤波的粒子滤波。同时，无迹粒子滤波和本发明的基于集合卡尔曼滤波的粒子滤波都是通过近似非线性概率密度分布代替非线性函数的方法，本发明的基于集合卡尔曼滤波的粒子滤波的估计性能略优于无迹粒子滤波。

如图3所示的四种粒子滤波方法的均方根估计误差，标准粒子滤波和卡尔曼粒子滤波会出现不稳定的情况，且估计误差较大，而无迹粒子滤波和本发明的基于集合卡尔曼滤波的粒子滤波方法由于建议分布函数更加接近真实后验分布，其状态估计的均方根误差明显降低，并且本发明的估计效果最好。

如图4所示的四种粒子滤波方法在不同粒子数时状态估计的均方根估计误差均值，可以看出，在粒子数较少时本发明提出的基于集合卡尔曼滤波的粒子滤波方法仍然具有较高的估计性能。

Claims (3)

1.一种基于集合卡尔曼滤波的粒子滤波方法，其特征在于：包括以下几个步骤：

步骤一：在k＝0时刻初始化粒子，利用先验概率p(x₀)进行随机采样，获得初始采样粒子

初始化每个采样粒子的权值为1/N，i代表粒子序号，i＝1∶N，N为采样粒子个数，k为非线性系统的运行时刻，

为k＝0时刻第i个采样粒子的状态值；对于k＝0时刻每一个采样粒子定义初始的分析集合

为

其中

表示k＝0时刻第i个粒子对应的分析集合中第j个样本，j代表分析集合中样本序号，

表示以k＝0时刻第i个粒子状态值为均值，以系统过程噪声协方差Q为方差的高斯分布；

步骤二：(1)在k＝1时刻，对每一个采样粒子分别定义一个背景集合

为

和一个分析集合

为

其中上标a和b分别代表分析集合和背景集合，j代表分析集合或背景集合中样本序号，n表示分析集合或背景集合中样本的个数，表示k时刻背景集合中第j个样本，

表示k时刻分析集合中第j个样本；

(2)利用非线性系统的状态方程由k-1时刻的分析集合对当前背景集合中每个样本进行预测，得到公式(3)

$x_{k,j}^{\left(i\right),b}=f\left(x_{k-1,j}^{\left(i\right),a}\right)---\left(3\right)$

为非线性系统状态方差，

表示k-1时刻分析集合中第j个样本。

计算k时刻背景集合的中所有样本的均值

非线性系统关于状态变量的协方差

以及观测方程的协方差

得到公式(4)、(5)和(6)：

${\hat{x}}_k^{\left(i\right),b}=\frac{1}{n}\underset{j-1}{\overset{n}{\mathrm{\Σ}}}{x}_{k,j}^{\left(i\right),b}---\left(4\right)$

${\hat{P}}_{\mathrm{xh}}^{\left(i\right),k}=\frac{1}{n-1}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}(x_{k,j}^{\left(i\right),b}-{\hat{x}}_k^{\left(i\right),b}){(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right))}^T---\left(5\right)$

${\hat{P}}_{\mathrm{hh}}^{\left(i\right)k}=\frac{1}{n-1}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right)){(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right))}^T---\left(6\right)$

h(·)表示非线性系统的观测方程函数；

(3)根据公式(7)计算卡尔曼增益

$K_k^{\left(i\right)}={\hat{P}}_{\mathrm{xh}}^{\left(i\right),k}{({\hat{P}}_{\mathrm{hh}}^{\left(i\right),k}+R_k)}^{-1}---\left(7\right)$

其中

为卡尔曼增益，R_k为观测噪声的协方差矩阵；

(4)融合最近观测信息，对k时刻背景集合进行更新，计算得到k时刻的分析集合：

$x_{k,j}^{\left(i\right),a}=x_{k,j}^{\left(i\right),b}+K_k^{\left(i\right)}(y_{k,j}-h\left(x_{k,j}^{\left(i\right),b}\right))---\left(8\right)$

其中y_k，j是以观测值y_k为均值、R_k为方差的高斯分布的采样；

根据公式(9)和(10)，计算分析集合的均值和协方差

${\hat{x}}_k^{\left(i\right)a}=\frac{1}{n}\underset{j-1}{\overset{n}{\mathrm{\Σ}}}{x}_{k,j}^{\left(i\right)a}---\left(9\right)$

${\hat{P}}_k^{\left(i\right)a}=\frac{1}{n-1}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}(x_{k,j}^{\left(i\right)a}-{\hat{x}}_k^{\left(i\right)a}){(x_{k,j}^{\left(i\right)a}-{\hat{x}}_k^{\left(i\right)a})}^T---\left(10\right)$

这个过程利用上一时刻的状态变量的值和当前时刻的最近观测信息，通过集合卡尔曼滤波方法对建议分布更新后的分析集合均值和方差进行传递，得到该时刻的高斯建议分布函数

其中

表示k时刻第i个粒子，表示k-1时刻第i个粒子，z_k表示k时刻观测向量；

步骤三：从步骤二得到的高斯建议分布函数中进行采样，根据公式(11)得到当前时刻的二次采样粒子，

表示k时刻第i个样本的二次采样样本，按照公式(12)计算每个二次采样粒子的权值对权值

进行归一化处理，得到归一化后的权值

${\hat{x}}_k^{\left(i\right)}~q\left(x_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)},z_k)=N({\hat{x}}_k^{\left(i\right),a},{\hat{P}}_k^{\left(i\right),a})$

${\mathrm{\ω}}_k^{\left(i\right)}={\mathrm{\ω}}_{k-1}^{\left(i\right)}\frac{p\left(z_k\right|{\hat{x}}_k^{\left(i\right)})p\left({\hat{x}}_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)})}{q\left({\hat{x}}_k^{\left(i\right)}\right|x_{0:k-1}^{\left(i\right)},z_{1:k})}---\left(12\right)$

步骤四：依照二次采样粒子的权值大小对步骤三中得到的二次粒子进行重采样，得到三次采样粒子；

步骤五：待估计非线性离散系统的运行时间为M，若k＝M则输出滤波结果，若k＜M，则迭代过程未完成，进入k+1时刻，重复步骤二至步骤四，直至非线性系统运行结束，使用采样加权平均值E(x_0:k)作为每一时刻算法输出值：

$E\left(x_{0:k}\right)={\∫x}_{0:k}\·p\left(x_{0:k}\right|z_{1:k}){\mathrm{dx}}_{0:k}\≈\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{x}_{0:k}^{\left(i\right)}---\left(13\right)$

输出值E(x_0:k)是对非线性系统进行滤波后的状态变量的估计值，其中0:k表示从0时刻到k时刻；x_0:k表示非线性系统从0到k时刻的状态值；p(x_0:k|z_1:k)表示非线性系统从0到k时刻的后验概率密度；

表示第i个样本从0到k时刻的状态变量的值。

2.根据权利要求1所述的一种基于集合卡尔曼滤波的粒子滤波方法，其特征在于：所述的步骤二(1)中的背景集合是由上一时刻的分析集合通过非线性系统预测得到的，分析集合由背景集合融合该时刻最近观测信息后更新得到。

3.根据权利要求1所述的一种基于集合卡尔曼滤波的粒子滤波方法，其特征在于：所述的非线性系统的模型的标准算例为：

x_k＝1+sin(ωπk)+0.5x_k-1+v_k-1，

其中x_k为非线性系统样本状态变量，y_k为非线性系统观测变量，系数ω＝6e-2，过程噪声v_k服从Gamma分布Gamma(4，3)，观测噪声w_k服从正态分布N(0，1e-4)。

Images