粒子滤波算法原理和仿真1 引言粒子滤波(Particle Filter, PF)是一种基于蒙特卡洛(Monte Carlo, MC)方法的递推贝叶斯滤波算法。

其核心思想是通过从状态空间寻找的一系列随机样本来近似系统变量的概率密度函数，以样本均值代替积分运算，从而获得状态的最小方差估计。

其中从状态空间中抽取的样本称为“粒子”。

一般地，随着粒子数目的增加，粒子的概率密度函数就逐渐逼近状态的概率密度函数，从而达到最优贝叶斯估计的效果。

2 粒子滤波原理 2.1 系统的动态空间对于被观测对象的状态，可以通过以下非线性离散系统来描述：11(,)t t t x f x w --= (1)(,)t t t z h x v = (2)以上为系统的状态方程和观测方程。

其中，f ( )为状态函数，h ( )为观测函数，x t 是系统在时间t 的状态变量，w t 为对应的过程噪声，z t 是系统在时间t 的观测值，v t 为对应的观测噪声。

从贝叶斯估计角度来看，状态估计问题就是根据观测信息z 0:t 构造状态的概率密度函数p (x 0:t |z 0:t )，从而估计在系统在任何状态下的滤波值。

设系统状态序列函数为g t ，则有：[]0:0:0:0:0:()()()t t t t t t x E g x g x p x z dx =⎰ (3)根据蒙特卡洛方法，后验概率分布可以用有限的离散样本来近似，由大数定律，当系统粒子数N →∞时，期望E [g t (x 0:t )]可近似为：[]()0:0:11()()Ni t t t ti E g x g xN==∑(4)式中{()0:i t x : i =1,2,...N }为状态空间中按p (x 0:t |z 0:t )得到的采样点。

2.2 重要性采样在粒子采集过程中，p (x 0:t |z 0:t )往往是未知且多变的，因此可先从一个已知且容易采样的参考分布q (x 0:t |z 0:t )中抽样，再通过对抽样粒子集进行加权求和来估计系统的状态值，即：[]0:0:0:0:0:0:0:0:0:0:0:0:0:()()()()()()()t t t t t t xt t t t t t tt t E g x g x p x z dx p x z g x q x z dx q x z ==⎰⎰ (5) 令ωt (x 0:t ) = p (z 0:t |x 0:t )p (x 0:t ) ∕ q (x 0:t |z 0:t )，则式(5)可表示为：[]0:0:0:0:0:0:0:0:0:0:()()()()()()tt tt t t x t tttttxg x w x q x z dx E g x w x q x zdx =⎰⎰ (6)按照式(4)，式(6)可近似为：[]()()0:0:10:()0:1()()0:0:11()()()1()()()Ni i t t t t i t t Ni t ti N i i t t t t i g xx NE g x xNg x x ωωω=====∑∑∑ (7)其中()0:()i t t x ω为()0:()ωi t t x 的归一化权值，()0:i t x 是由q ( x 0:t |z 0:t )采样获得的粒子。

2.3 序贯重要性抽样贝叶斯估计是一个序列估计问题，通过序贯重要性抽样(Sequential Importance Sampling, SIS)建立采样粒子的序列关系。

在t +1时刻采样时，不改变状态序列过去的样本集，而采用递归的形式计算重要性权值。

因此参考分布可表示为：0:0:0:10:10:10:()()(,)---=t t t t t t t q x z q x z q x x z (8)同时，假设系统状态是一阶马尔可夫过程，即x t 只与x t -1相关，z t 只与x t 相关，因此：0:0:10:10:11()()()()()----==t t t t t t t p x p x p x x p x p x x (9)0:0:0:10:0:10:0:10:1()(,)()()()----==t t t t t t t t t t t p z x p z z x p z x p z x p z x (10)再将式(8) ~ (10)代入ωt ，可推知：110:10:()()(,)ωω---=t t t t t t t t t p z x p x x q x x z (11)一般q ( x t |x 0:t -1 , z 0:t ) =p (x t |x t -1, z t )为参考分布的最优分布，其概率密度仅依赖于x t -1和z t 。

但这种选择在实际中往往难以实现，因此多选用q ( x t |x 0:t -1 , z 0:t ) = p (x t |x t -1)来近似。

此时系统仅记录当前状态粒子()i t x 和其权重()i t ω，则由式(11)可知：()()1()i i it t t t p z x ωω-= (12)2.4 粒子滤波重采样粒子退化是序贯重要性采样算法的主要问题，即经过若干次递推后，权值方差会逐渐增大，这样大量权值都集中在少数粒子上，而多数粒子则对系统状态的估计影响甚微，由此严重影响了滤波器的性能。

重采样是解决粒子退化的重要方法，其核心思想是减少或剔除权值较小的粒子，而复制权值较大的粒子，最后将所有粒子的权值设为1/N 。

粒子滤波重采样方法的如下图所示：图1 粒子滤波重采样示意图中()i t x 为采样得到的粒子，对应的权值为()i t ω；()i t x 为重采样所得的粒子，对应权值为1/N 。

目前常用的重采样算法包括随机重采样(random resampling)、多项式重采样(multinomial resampling)、系统重采样(systematic resampling)和残差重采样(residual resampling)，几种算法的计算步骤分别如下： 1. 随机重采样设u ~U (0,1]，{u i : i =1, 2, .., N }独立同分布。

定义函数D ( )，若：1()()11(,]m mj j i t t j j u m N ωω-==∈≤∑∑ (13)则D (u i ) = m ，且()()i m t t x x =。

2. 多项式重采样设u ~U (0,1]，{i u : i =1, 2, .., N }独立同分布，定义：1111,...,1i i i i NN Nu u u i N u u +⎧=⎪=-⎨⎪=⎩ (14)对函数D ( )，由式(13)可知：D (u i ) = m ，且()()i m t t x x =。

3. 系统重采样 4. 残差重采样综上所述，粒子滤波算法的一般流程为：（1）粒子集初始化，由先验概率密度p (x 0)产生粒子集{()0i x }，并使所有粒子权值为1/N ；（2）按照()()()1~(,)i i i t t t t x q x x z -对t 时刻的系统进行重要性采样，并通过当前观测值z t根据式(12)计算粒子权值()i t ω；（3）计算归一化权值()i t ω，并按式(15)计算有效粒子数N eff ；eff ()211()Ni ti N ω==∑ (15)（4）若N eff 小于设定阈值N th ，则对粒子进行重采样，得到新的粒子集()(){,}i i t t x ω，其中()i t ω=1/N ，否则()()()(){,}{,}i i i i t t t t x x ωω=； （5）由式(7)推知系统的状态估计值t x 为：()()1Ni i t t t i x x ω==∑ (16)再转到步骤（2），对t +1时刻的系统进行重要性采样。

3 粒子滤波仿真首先构造系统的状态模型和观测模型：1121258cos(1.2(1))21k t t t t x x x t w x ---=++-++ (17) 220t k t x z v =+ (18)式中，过程噪声w k ~ N (0, 5)，观测噪声v k ~ N (0,5)。

设系统的状态初始值x 0 = 0.1，初始分布p (x 0) ~ N (0,2)。

观测时长T = 200，每个时刻的采样粒子数N = 500。

观测时间内，系统状态值和观测值如图2所示：图2 系统状态值和观测值图中的理论状态值是系统在无过程噪声(w t= 0)的情况下，通过严格时序递推得到的理想值，可以看出，它与系统的真实状态之间存在差异；理论观测值反映系统真实状态的理论观测情况(v t= 0)，而真实观测值是系统在观测噪声的干扰下，监测所得的实际数值。

粒子滤波将通过观的真实值估计系统的状态，并期望滤波结果接近于真实值。

对仿真系统的滤波结果如图3所示：图3 系统的滤波效果由图3知，粒子滤波很好地实现了对系统状态的估计。

其中，为了进一步评估滤波的效果，将状态的估计值代入式(18)，并令v t= 0，得到滤波后的观测值。

通过式(19)可以计算当前观测信号Z的信噪比SNR：TT10lg (z )(z )zzSNR Z Z =⨯-- (19) 式中z 为观测信号的理论值，代表Z 中真实有用的成分。

表1给出几组不同滤波器参数下的观测信号的信噪比：表1 不同参数滤波效果比较1z2z 3z 4z1 2 5 103.9924.052 4.747 3.303表中z 为滤波前的观测信号，14~z z 为按不同方差的分布进行粒子抽样，然后滤波所得的观测信号。

由表1知，粒子滤波使信号的信噪比有了明显提升，而滤波效果与采样粒子的方差相关。

根据上表及前文内容可知，当粒子的分布接近过程噪声时，往往可以得到较好的滤波效果。