《粒子滤波原理和仿真》由会员分享，可在线阅读，更多相关《粒子滤波原理和仿真（6页珍藏版）》请在金锄头文库上搜索。
1、粒子滤波算法原理和仿真1 引言粒子滤波 (Particle Filter, PF) 是一种基于蒙特卡洛(Monte Carlo, MC) 方法的递推贝叶斯滤波算法。其核心思想是通过从状态空间寻找的一系列随机样本来近似系统变量的概率密度函数，以样本均值代替积分运算，从而获得状态的最小方差估计。其中从状态空间中抽取的样本称为“粒子” 。一般地，随着粒子数目的增加，粒子的概率密度函数就逐渐逼近状态的概率密度函数，从而达到最优贝叶斯估计的效果。2 粒子滤波原理2.1 系统的动态空间对于被观测对象的状态，可以通过以下非线性离散系统来描述：11(,)tttxf xw(1) (,)tttzh x v(2)
2、以上为系统的状态方程和观测方程。其中，f( )为状态函数，h( )为观测函数， xt是系统在时间 t 的状态变量，wt为对应的过程噪声，zt是系统在时间t 的观测值， vt为对应的观测噪声。从贝叶斯估计角度来看，状态估计问题就是根据观测信息z0:t构造状态的概率密度函数p(x0:t|z0:t)，从而估计在系统在任何状态下的滤波值。设系统状态序列函数为gt，则有：0:0:0:0:0:()() ()ttttttxE gxgxp xzdx(3) 根据蒙特卡洛方法，后验概率分布可以用有限的离散样本来近似，由大数定律， 当系统粒子数 N时，期望Egt(x0:t)可近似为：( ) 0:0: 11()()N
3、 i tttt iE g xgxN(4) 式中 ( ) 0:i tx: i=1,2,.N为状态空间中按p(x0:t|z0:t)得到的采样点。2.2 重要性采样在粒子采集过程中，p(x0:t|z0:t)往往是未知且多变的，因此可先从一个已知且容易采样的参考分布q(x0:t|z0:t)中抽样，再通过对抽样粒子集进行加权求和来估计系统的状态值，即：0:0:0:0:0:0:0: 0:0:0:0: 0:0:()() ()()()()()ttttttxtt ttttt ttE gxgxp xz xzgxq xzdxq xz(5) 令 t(x0:t) = p(z0:t|x0:t)p(x0:t) q(x0:t
4、|z0:t)，则式 (5)可表示为：0:0:0:0:0:0:0:0:0:0:()() () () () ()ttttttxttttttxg xwxq xzdx E gx wxq xzdx(6) 按照式 (4)，式 (6)可近似为：( )( ) 0:0: 1 0: ( ) 0: 1( )( ) 0:0: 11()()()1()()()N ii tttt i ttN i tt iN ii tttt igxxNE gx xNgxx(7) 其中( ) 0:()i ttx为( ) 0:()i ttx的归一化权值，( ) 0:i tx是由 q( x0:t z0:t )采样获得的粒子。2.3 序贯重要性抽样
5、贝叶斯估计是一个序列估计问题，通过序贯重要性抽样(Sequential Importance Sampling, SIS)建立采样粒子的序列关系。在t+1 时刻采样时，不改变状态序列过去的样本集，而采用递归的形式计算重要性权值。因此参考分布可表示为：0:0:0:10:10:10:()() (,)tttttttq xzq xzq x xz(8) 同时，假设系统状态是一阶马尔可夫过程，即xt只与 xt-1相关， zt只与 xt相关，因此：0:0:10:10:11()() ()()()tttttttp xp xp x xp xp x x(9) 0:0:0:10:0:10:0:10:1()(,) ()
6、() ()tttttttttttp zxp z zxp zxp z x p zx(10) 再将式 (8) (10)代入 t，可推知：1 1 0:10:() ()(,)tttt tt tttp z xp x xq x xz(11) 一般 q(xt|x0:t-1, z0:t ) = p(xt|xt-1, zt)为参考分布的最优分布，其概率密度仅依赖于xt-1和 zt。但这种选择在实际中往往难以实现，因此多选用q(xt|x0:t-1, z0:t ) = p(xt|xt-1)来近似。此时系统仅记录当前状态粒子( ) i tx和其权重( ) i t，则由式 (11)可知：( )( ) 1()iii tt
7、ttp z x(12) 2.4 粒子滤波重采样粒子退化是序贯重要性采样算法的主要问题，即经过若干次递推后，权值方差会逐渐增大，这样大量权值都集中在少数粒子上，而多数粒子则对系统状态的估计影响甚微，由此严重影响了滤波器的性能。重采样是解决粒子退化的重要方法，其核心思想是减少或剔除权值较小的粒子，而复制权值较大的粒子，最后将所有粒子的权值设为1/N。粒子滤波重采样方法的如下图所示：图 1 粒子滤波重采样示意图中( ) i tx为采样得到的粒子，对应的权值为( ) i t；( ) i tx为重采样所得的粒子，对应权值为 1/N。目前常用的重采样算法包括随机重采样(random resampling)
8、 、多项式重采样(multinomial resampling) 、系统重采样(systematic resampling)和残差重采样(residual resampling)，几种算法的计算步骤分别如下：1. 随机重采样设 uU(0,1，ui : i=1, 2, , N 独立同分布。定义函数D( )，若：1 ( )( )11(,mm jj itt jjumN(13) 则 D(ui) = m，且( )()im ttxx。2. 多项式重采样设uU(0,1，iu: i=1, 2, , N 独立同分布，定义：1111,.,1i iiiN NNuuuiNuu(14) 对函数 D( )，由式 (13)
9、可知： D(ui) = m，且( )()im ttxx。3. 系统重采样4. 残差重采样综上所述，粒子滤波算法的一般流程为：（1） 粒子集初始化， 由先验概率密度p(x0)产生粒子集 ( ) 0ix ， 并使所有粒子权值为1/N；（2）按照( )( )( ) 1(,)iii ttttxq xxz对 t 时刻的系统进行重要性采样，并通过当前观测值zt根据式 (12)计算粒子权值( ) i t；（3）计算归一化权值( ) i t，并按式 (15)计算有效粒子数Neff；eff( )211()Ni tiN(15) （4）若 Neff小于设定阈值Nth，则对粒子进行重采样，得到新的粒子集( )( ),
10、ii ttx，其中( ) i t=1/N，否则( )( )( )( ),iiii ttttxx；（5）由式 (7)推知系统的状态估计值tx为：( )( )1Nii tttixx(16) 再转到步骤（2） ，对 t+1 时刻的系统进行重要性采样。3 粒子滤波仿真首先构造系统的状态模型和观测模型：11 2 1258cos(1.2(1)21kt tt txxxtwx(17) 220t ktxzv(18) 式中， 过程噪声wk N(0, 5)，观测噪声vk N(0,5)。设系统的状态初始值x0 = 0.1，初始分布 p(x0) N(0,2)。观测时长T = 200，每个时刻的采样粒子数N = 500。
11、观测时间内，系统状态值和观测值如图2 所示：图 2 系统状态值和观测值图中的理论状态值是系统在无过程噪声(wt= 0)的情况下， 通过严格时序递推得到的理想值，可以看出， 它与系统的真实状态之间存在差异；理论观测值反映系统真实状态的理论观测情况 (vt= 0)，而真实观测值是系统在观测噪声的干扰下，监测所得的实际数值。粒子滤波将通过观的真实值估计系统的状态，并期望滤波结果接近于真实值。对仿真系统的滤波结果如图 3 所示：图 3 系统的滤波效果由图 3 知，粒子滤波很好地实现了对系统状态的估计。其中， 为了进一步评估滤波的效果，将状态的估计值代入式(18)，并令 vt= 0，得到滤波后的观测值。
12、通过式(19)可以计算当前观测信号Z 的信噪比SNR：TT10lg(z)(z)zzSNRZZ(19) 式中z为观测信号的理论值，代表 Z 中真实有用的成分。表 1 给出几组不同滤波器参数下的观测信号的信噪比：表 1 不同参数滤波效果比较观测信号z 1z2z3z4z采样方差1 2 5 10 SNR(dB) -1.239 3.992 4.052 4.747 3.303 表中 z 为滤波前的观测信号，14zz为按不同方差的分布进行粒子抽样，然后滤波所得的观测信号。 由表 1 知，粒子滤波使信号的信噪比有了明显提升，而滤波效果与采样粒子的方差相关。 根据上表及前文内容可知，当粒子的分布接近过程噪声时，往往可以得到较好的滤波效果。
