98西安交大粒子滤波课件

PARTICLEFILTERING©CAIYUANLI1(PF)CONTENTS1最优贝叶斯递推滤波.....................................................................................................42完备采样（perfectsampling）......................................................................................73重要性采样...................................................................................................................103.1一般原理............................................................................................................103.2序贯重要性采样................................................................................................183.3SIS算法..............................................................................................................214重采样...........................................................................................................................235基本粒子滤波算法.......................................................................................................285.1基本思想............................................................................................................28©CAIYUANLI25.2PF算法1............................................................................................................335.3PF算法2............................................................................................................355.4改进算法............................................................................................................375.5实现问题与MATLAB代码..............................................................................395.5.1重要性权重计算......................................................................................395.5.2预测粒子计算.........................................................................................405.5.3MATLABcode.........................................................................................405.5.4仿真算例.................................................................................................456一般粒子滤波算法.......................................................................................................506.1滤波算法............................................................................................................516.2重采样选择........................................................................................................546.3算例....................................................................................................................55©CAIYUANLI31最优贝叶斯递推滤波kk(1)1()kk+=+xfxΓw()+xxkmk∈R。~()kkpwwi，,,,01(,,,)kkkv(2)kk=yh其中，，nRk∈)i。m，wRk∈yqR∈，v~(kkpvv记02(kk=Xx)xx，=Yy©CAIYUANLI4yy。那么©CAIYUANLI511(|)(|)(|)dkkkkkkppp++=k∫xYxxxY)(|)dkkkpx(3)k11(|)(|kkkpp++=∫yYyxxYx(4)1111(|(|)kk11)(|)(|)kkkkkpppp++++++=kyxxYxYyY(|kpxY(5)一般情况下，通常无法获得解析解。滤波的目的是实现)k的递推估计。在MonteCarlo仿真中，以若干离散样本（称为粒子）对期望的概率密度进行近似，即)i−(6)()()1(Nikkiwδ=∑xx()ikw≥()11Nikiw==∑{}x并确定()1,2,,{}ikiNw=。ˆ(|)(|)kkkkpp≈=xYxY其中，，而且。0©CAIYUANLI6问题：如果生成粒子()1,2,,ikiN=在粒子滤波中，采用MC技术，近似地给出)k。而需要的)只是)k的边缘分布密度，可以简单地导出。(|kpXY(|x(|kpXYkkYp2完备采样（perfectsampling）对于目标密度函数()px，设(){,,2,,}ix是根据()1iN=p©CAIYUANLI7x采样到的独立同分布粒子，那么©CAIYUANLI8()11ˆ()(NiipNδ==∑xx)−x(7)例如：()11NiiN=≈()dp=∑∫xxxxx(8)T()T11var()()()()d()()NiiipN==−−≈−−∑∫xxxxxxxxxx()x(9)更加一般地()11(NiigN=∑..()asˆ()()NEgEg≈=xx)x(10)理论上ˆlim()NNEg→+∞⎯⎯xEgx(11)→(|kpXY©CAIYUANLI9对于非线性滤波问题，几乎不可能直接从)k进行采样，需要下述重要性采样方法。3重要性采样3.1一般原理通常，目标密度函数)p(x非常复杂，不容易直接产生符合密度函数()px的粒子。设)是较之(qx()p©CAIYUANLI10x容易实现采样的概率分布密度函数，在支撑覆盖条件下，如果)qx，那么粒子属于(){,1,2,,ii=x}~(N()ix()px()iw，，有()()()iqMC求解。相对于(|XY(|)kkqXY是容易进行采用的概率密的概率为称为接受概率()()iipw∝xx[例]定积分的(12)©CAIYUANLI11)kk，设p度函数，称为重要性函数。对于任意的)，其最优估计为(kgX|)d|kY()()((|)()()d(|)kkkkkkkkkkkkEggppgqq==∫∫XXXXXYXXXYYX(13)©CAIYUANLI12注意到©CAIYUANLI13(,)(,)(|)()(,)d(,)(,)(|)d(|)kkkkkkkkkkkkkkkkkkkpppppppqq===∫∫XYXYXYYYXXXYXYXYXXY(14)如果记（称为重要性权重）(,)(|kkkkkpwq=XYXY)(15)那么()()(|kk(|)d)dkkkkkkkgwqEgwq=k∫∫XXYXXXYX(|kqXY()1}iNki=(16)©CAIYUANLI14如果依重要性函数)k采样得N个独立同分布的粒子，那么{X©CAIYUANLI15()11(|)(NikkkkiqNδ=≈∑XYXX)−(17)于是()()11()11()()()()1(NiiNkkNiikNiiNkiwgEgwgw===≈=∑∑∑XX)ikkX(18)式中，(ikw)为重归1化重要性权重。即©CAIYUANLI16()()()1iikkNiki==∑(19)而()()(,)(|iikkikkpwq=XYXY())k(20)因此，重要性采样方法等效于©CAIYUANLI17()()1ˆ(|)(|)(Nikkkkkkkippwδ=≈=∑XYXYXX)i−(21)从式(20)可以看出，每当获得新的量测值时，都要重新计算重要性权重，不是递推计算，无法实现在线（实时）估计。为此，需要下述序贯（递推）重要性采样方法。3.2序贯重要性采样序贯重要性采样（sequentialimportancesampling，SIS)是重要性采样的扩展。由于重要性函数)k亦是一个分布密度，由Beyes’srule有©CAIYUANLI18(|kqXY©CAIYUANLI191111(|)(|,)(|)(|,)(|)kkkkkkkkkkkkqqqqq−−−−1−==XYxXYXYxXYXY()11~(|ikkq(22)表明可以采样)kY，1)k()~(iikkqxx()1|,k−X−−XXY11111,)(,))kkkkp−。而111111(,)(,,,)(|,,)(|(|)(|)(,kkkkkkkkkkkkkkkkkppppppp−−−−−−−−−−−===XYxyXYyxXYxXyxxxXYYXY(23)因此111111(,)(|)(|)(,)(|)(|,)(|)(|)(|)(|,)kkkkkkkkkkkkkkkkkkkkkkkkppppwqqqppwq−−−−−−−−===XYyxxxXYXYxXYXYyxxxxXY111−(24)()()()()(|)(|iii()()()111(|),)ikkkkkiikkkppwq−ikw−©CAIYUANLI20−=yxxxxXY(25)3.3SIS算法更新粒子：}i()()()1{,iikkk−=XxX()，)kY，()|kq−X1,1,2,,iN~(iikkxx(26)=更新重要性权重：()()()1()()(|)((|iiiikkkkiikkppwwq()()11|),)ikkk−−−=yxxxX1,2,,iNxY，=©CAIYUANLI21(27)©CAIYUANLI22()()iikkNww=∑(28)()1ikiw=验后概率密度近似估计：()()1ˆ(|)(Nikkkkkipwδ==∑XYXX)i−(29)SIS算法理论