第3章-粒子滤波方法课件.ppt

1、第3章 粒子滤波方法第3章 粒子滤波方法3.1 引言3.2 贝叶斯滤波3.3 贝叶斯重要性采样3.4 序贯重要性重采样粒子滤波算法3.5 马尔可夫链蒙特卡罗粒子滤波算法3.6 辅助粒子滤波算法3.7 正则化粒子滤波算法3.8 边缘粒子滤波算法3.9 扩展卡尔曼粒子滤波算法3.10 高斯和粒子滤波算法3.11 小结第3章 粒子滤波方法3.1 引引 言言粒子滤波是指通过寻找一组在状态空间中传播的随机样本，对概率密度函数p(xk|yk)进行近似，以样本均值代替积分运算，从而获得状态的最小方差估计的一种算法。粒子滤波算法依据系统状态向量的先验分布在状态空间中产生一组随机样本，然后根据观测量不断地调整粒

2、子的权值和位置，通过调整后的粒子信息修正最初的后验概率函数。用数学语言可描述为：针对平稳的时变系统，假定k1时刻系统的后验概率密度为p(xk1|yk1)，依据一定规则选取N个随机样本点，在k时刻获得量测信息后, 经过状态更新和时间更新过程, N个粒子的后验概率密度近似为p(xk|yk), 随着粒子数的增加，粒子的概率密度函数就能逼近状态真实的概率密度函数，对状态向量的估计结果与最优贝叶斯估计结果接近。粒子滤波适用于非线性非高斯系统的状态估计，突破了传统卡尔曼滤波理论框架，精度可以逼近最优估计，是一种有效的非线性滤波技术，广泛应用于数字通信、图像视频处理、计算机视觉、语音信号处理、机器学习等领域

3、。第3章 粒子滤波方法3.2 贝贝 叶叶 斯斯 滤滤 波波对于跟踪问题，目标状态序列的状态转移可以用下面的目标状态序列xk,kN的演变方程来描述(3-1)11(,)kkkfxxv其中，是关于状态xk1的非线性函数，是独立同分布的过程噪声序列，nx,nv分别是状态和过程噪声向量的维数。系统量测方程为:xvxnnnf1,kkv(3-2)(,)kkkhzxn第3章 粒子滤波方法从贝叶斯估计观点来看，跟踪问题就是计算k时刻状态xk的某种置信程度。从1到k时刻，由于给定量测数据z1: k的值不同，得出的xk值也不同，因此需要构造概率密度函数p(xk|z1: k)。假定初始概率密度函数p(x0|z0)p(

4、x0)，x0表示初始状态向量，z0表示尚且没有获得量测值，它也是先验概率密度函数。因此从形式上看，通过预测和更新两个步骤就可以递推地得到概率密度函数p(xk|z1: k)的值。假定k1时刻的概率密度函数已知，那么通过ChapmanKolmogorov等式及使用模型(3-1)就可以预测出k时刻状态的先验概率密度函数(3-3)1:1111:11(|)(|) (|)kkkkkkkpppdxzxxxzx第3章 粒子滤波方法在等式(3-3)中，p(xk|xk1)=p(xk|xk1，z1: k1)，且满足等式(3-1)所描述的一阶马尔可夫过程。状态估计p(xk|xk1)的概率模型由系统等式(3-1)和统计

5、值vk1来确定。在k时刻，可以得到量测值z=，然后通过贝叶斯规则更新先验概率密度函数1:11:1:1(|) (|)(|)(|)kkkkkkkkppppzxxzxzzz(3-4)式中的常量(3-5)1:11:1(|)(|) (|)dkkkkkkkpppzzzxxzx是由模型(3-2)定义的似然函数p(zk|xk)和一步预测的统计值p(xk|z1: k1)共同确定的。在更新式子(3-4)中，量测值zk被用来修正先验概率，以获得当前状态的后验概率。第3章 粒子滤波方法式(3-3)和式(3-4)是最优贝叶斯估计的一般形式。通过以上步骤递推得到的后验概率只是一般概念下的表达式，通常情况下难以得到其解析表

6、达式。只有在满足特定条件时，才可以得到最优贝叶斯解。第3章 粒子滤波方法3.3 贝叶斯重要性采样贝叶斯重要性采样在贝叶斯重要性采样中，后验概率分布由一组离散的样本集近似得到。根据大数定理，随着样本粒子数N的增加，期望Eg(x0: k)可由近似求出。但是，通常很难从后验概率密度函数中直接抽样。常规的解决办法是从容易采样的概率分布q(x0:k|y1:k)中采样粒子，由此可以得到0:E ()kg x0:1:0:0:0:1:0:0:1:1:0:0:0:0:1:0:1:0:1:0:0:0:1:0:1:(|)E ()()(|)d(|)(|) ()()(|)d() (|)()()(|)d()kkkkkkkk

7、kkkkkkkkkkkkkkkkkpggqqppgqpqwgqpxyxxxyxxyyxxxxyxyxyxxxyxy(3-6)第3章 粒子滤波方法式中w(x0: k)是未归一化的重要性权值，可以表示为(3-7)1:0:0:0:1:(|) ()(|)kkkkkkppwqyxxxy由于p(y1: k)是未知的，我们可以将式(3-6)表示为0000101000100110000100010001011E ()() () ()d()() () ()d()() ()d()() () ()d() ()dE (:k:k:k:k:k:k:k:k:k:k:k:k:k:k:k:k:k:k:k:k:k:k:k:k:k

8、:k:k:k:kq:ggwq|pgwq|q|p|pq|gwq|wq|xxxxyxyxxxyxxyyxxxxyxxxyxxxyxy0010)( ()()E ()( ()k:k:kq:k:kwg|wxxyx(3-8)第3章 粒子滤波方法式中Eq(|y1: k)是指在概率分布q(|y1: k)上进行计算的期望。通过从概率函数q(|y1: k)中采样，期望可以近似表示为(3-9)( )( )0:0:( )( )10:0:0:( )10:11g() ()E ()() ()1()NiikkNiiikkkNiikiwNggwwNxxxxxx式中的重要性权值表示为( ) ikw (3-10)( )( )( )

9、1iikkNjkjwww等式(3-9)计算出来的结果是有偏的。但是，通过以下两个假设可以使逐渐收敛，并且接近于真实值。(1) x(i)0: k是从后验概率分布中采样得到的一组粒子，Eg(x0: k)存在并且是有限的；0:E ()kg x第3章 粒子滤波方法(2) 在后验概率分布上计算出的wk和wkg2(x0: k)的期望存在而且有限。只有g(x0: k)的方差和重要性权值有限才能验证第二个假设的成立性。随着N值的无限增大，后验概率分布函数就会近似于点估计分布，即(3-11)( )0:( )0:1:0:1(|)()ikNikkkkipwdxxyx第3章 粒子滤波方法3.4 序贯重要性重采样粒子滤

10、波算法序贯重要性重采样粒子滤波算法3.4.1 序贯重要性采样序贯重要性采样贝叶斯重要性采样(Sequential Importance Sampling， SIS)2, 3是一种简单常用的蒙特卡罗积分方法，但是它不能直接用来做递推估计。这主要是因为估计p(x0: k|y1: k)需要用到所有的观测数据y1: k，每次更新观测数据yk+1时，需要重新计算整个状态序列的重要性权值，因此它的计算量随着时间的推移而不断增加。为了解决该问题，人们提出了序贯重要性采样方法。该方法在k时刻采样时不改动过去的状态序列x0: k1,而采用如下递推形式计算重要性权值(3-12)0:1:0:11:10:11:(|)

11、(|) (|,)kkkkkkkqqqxyxyxxy第3章 粒子滤波方法这里先假设当前状态不依赖于将来的观测，即只进行滤波而不考虑平滑。需要强调的是，在某些情况下，一些建议分布需要用到过去的状态序列。在本书中，不考虑这种情况。假设状态符合马尔可夫过程，在给定状态下，量测值是条件独立的，则可得(3-13)0:011()()(|)kkjjjpppxxxx1:0:1(|)(|)kkkjjjppyxyx(3-14)将式(3-12)、式(3-13)和式(3-14)代入式(3-7)中得到权值递推公式(3-15)1:0:0:0:11:10:11:1:0:0:11:10:10:10:11:110:11:(|)

12、()(|) (|,)(|) ()1(|) ()(|,)(|) (|)(|,)kkkkkkkkkkkkkkkkkkkkkkkkkkkppwqqppwppqppwqyxxxyxxyyxxyxxxxyyxxxxxy第3章 粒子滤波方法在给定合适的重要性分布函数q(xk|x0: k1，y1: k)的条件下，式(3-15)提供了一个递推计算重要性权值的方法，重要性权值的计算因此得以简化。第3章 粒子滤波方法3.4.2 序贯重要性采样问题及策略序贯重要性采样问题及策略1. 选取好的重要性密度函数选取好的重要性密度函数选择合适的重要性密度函数是重要性采样算法中的关键步骤。选择重要性密度函数的一个原则是使得权

13、系数的方差最小。Doucet等提出了在给定x0: k1和y1: k的条件下权系数方差最小的最优密度函数许多学者也都证明了上式成立。尽管如此，为方便起见，多数粒子滤波算法中的重要性函数还是选择了次优的q(xk|x0: k1, y1: k)=p(xk|x0: k1)。由于未能利用最新的量测信息，以该函数进行抽样所产生的方差比后验概率p(xk|x0: k1，y1: k)产生的方差大，但由于其容易实现，在粒子滤波算法中依然得到了广泛的应用。(3-16)0:11:0:11:(|,)(|,)kkkkkkqpxxyxxy第3章 粒子滤波方法2. 粒子退化问题粒子退化问题序贯重要性采样方法的一个严重缺陷是粒子

14、退化问题，即经过几次迭代后，可能只有少数几个粒子有非零权值，其它粒子的权值非常小，可以忽略不计。出现这种现象是由于随着时间的增加，重要性权值的方差也在增加。退化现象使得大量的计算工作都浪费在更新那些对p(xk|y1: k)的估计几乎不起作用的粒子上。1998年，Liu和Chen给出了一种衡量粒子数匮乏程度的方法3，15，该方法定义“有效粒子数”Neff为(3-17)eff1var()ikNNw式中。Neff越小，表明粒子退化现象越严重。一般很难确切计算出Neff的值，但可以用下式计算Neff的估计值Neff1:1(|) / (|,)iiiikkkkkkwpqxzxxz第3章 粒子滤波方法(3-

15、18)eff211()NikiNw式中wik是由式(3-15)定义的归一化的权值。由式(3-18)易知NeffN。通过有效粒子数可以衡量当前粒子群的退化程度，当粒子群退化现象比较严重时，采取增加粒子数的办法来减小粒子群的退化程度，但这种方法的计算量太大，制约了算法的实时性。因此，通常采取另一种方法，即在SIS之后对重要性样本进行重采样。第3章 粒子滤波方法3. 重采样重采样重采样是抑制粒子退化现象的一种手段。设定一个有效样本数Nth并将其作为阈值，当Neff0时，T(x，y)0。给定当前状态xk，MH(MetropolisHastings)算法的步骤如下：Step 1：根据建议函数T(xk，y

16、)转移。Step 2：从均匀分布中抽样得到UUniform0，1，然后更新状态(3-26)1,(, ),otherwisekkkyUr xyxx( ) ( , )( , )min 1,( ) ( , )p y T y xr x yp x T x y式中，。第3章 粒子滤波方法显然，当T(x，y)对称时，上述算法等价于Metropolis算法。Barker于1965年提出了另外一种接受函数，即( ) ( , )( , )( ) ( , )( ) ( , )Bp y T y xrx yp y T y xp x T x y(3-27)Charlesstein提出了更一般的接受函数，即(3-28)(

17、, )( , )( ) ( , )x yr x yp x T x y式中，(x，y)为任意的对称函数，只需要满足对于任何x和y都有r(x，y)1即可。当选用式(3-28)作为接受函数时，从x转移到y的转移概率可以表示为(3-29)1( , )( , ) ( , )( )( , )A x yT x y r x yp xx y第3章 粒子滤波方法由于(x，y)=(y，x)，故有p(x)A(x，y)=p(y)A(y，x)，即由上述过程产生的马尔可夫链是可逆的，且p(x)是马尔可夫链的平稳分布。第3章 粒子滤波方法3.5.4 马尔可夫链蒙特卡罗粒子滤波算法步骤马尔可夫链蒙特卡罗粒子滤波算法步骤MCMC

18、粒子滤波算法5通过构造马尔可夫链产生来自目标分布的样本(粒子)，使样本更加多样化，具有很好的收敛性。在粒子滤波中引入MH(MetropolisHastings)采样的具体过程如下：Step 1：按照均匀分布从区间0，1中采样得到门限值u，即uUniform0，1。Step 2：按照概率p(xk|x(i)k1)采样得到。( )( )1(|)iikkkxp xxStep 3：若，则接受，即 ；否则丢弃，保留重采样的粒子，即 。MCMC方法的缺陷是, 为了保证收敛所需要的概率转移次数较大, 算法增加的计算量较大, 而且其收敛的判断也是个问题。( )( )(|)min 1,(|)ikkikkp zxu

19、p zx( ) ikx( )( )iikkxx( ) ikx( )ikx( )( )iikkxx第3章 粒子滤波方法3.6 辅助粒子滤波算法辅助粒子滤波算法辅助粒子滤波算法(Auiliary Particle Filter， APF)由Pitt和Shephard提出6，是序贯重要性重采样滤波器的变形。APF是从联合密度函数p(xk，i|z1: k)中得到一个抽样，其中i是辅助变量，表示在k1时刻可对xk预测的采样粒子即xik1xk。引入一个重要性密度函数q(xk，i|z1: k)，进行采样xjk，ijNj=1，其中ij是k1时刻可对xk预测的采样粒子。运用贝叶斯准则可将联合密度函数表示为(3-

20、30)1:1:11:11:111(, |)(|, ,) (, |) ( |)(|) (|)kkkkkkkkiikkkkkpipipip ippwxzzxzxzzzxxx选择重要性密度函数，并使该函数满足以下比例(3-31)1:11(, |)(|) (|)iiikkkkkkkqippwxzzxx第3章 粒子滤波方法其中，ik是在xik1的条件下关于状态变量xk的某种统计信息。一般情况下，可以取均值，或者一个样本满足ikp(xk|xik1)。从重要性密度函数q(xk，i|z1: k)中抽样的粒子(xjk，ij)权值满足(3-32)111:(|) (|)(|)(,|)(|)jjjjjijiikkkk

21、kkkkjjikkkkpppwwqipzxxxzxxzz其中，由式(3-30)和式(3-31)可得出式(3-32)。这些带权值的粒子集 的分布近似于p(xk，i|z1: k)，通过辅助变量ij的替换，即可从p(xk，i|z1: k)中得到想要的抽样粒子。将重要性函数因式分解为1,pNjjjkkjiwx(3-33)1:1:1:11(, |)( |) (| ,)( (|) (|)iiikkkkkkkkkkqiq iqipwpxzzxzzxx第3章 粒子滤波方法3.7 正则化粒子滤波算法正则化粒子滤波算法重采样作为一种减少粒子退化问题的方法在粒子滤波中得到广泛应用，但是也带来了新的问题，最主要的问题

22、就是粒子多样性的消失。这是因为在重采样时，样本是从离散分布而不是连续分布中抽样的。如果此问题得不到很好的处理，将可能导致“粒子崩溃”。“粒子崩溃”是一种很严峻的粒子匮乏现象，即所有的粒子都占据状态空间上的同一个点，从而不能很好地反映后验分布。正则化粒子滤波(Regularized Particle Filter， RPF)算法可以在一定程度上解决此问题16。RPF算法是从后验密度p(xk|z1: k)的连续近似中进行重采样的，即(3-34)1:1(|)()sNiikkkhkkipw Kxzxx第3章 粒子滤波方法其中，是重新调整过的核密度K()，h0是核的带宽(标量)，nx是状态向量x的维数，

23、wik(i=1，2，N)是经过归一化后的权值。核密度是一个对称的概率密度函数,且有1( )( )hnKKhhxxx( )d0,Kxxx2( )dK xxx(3-35)选择合适的核函数K()和带宽h，使得真实的后验概率密度和相应的正则化经验表示的积分均方误差(Mean Integrated Square Error， MISE)的均值最小，该式定义为(3-36)21:1:MISE( )E(|)(|)dkkkkkpppxzxzx式中，p(|)表示在条件(3-34)下对p(xk|z1: k)的近似。在特殊情况下，所有的采样有相同的权值，核的最优选择是Epanechnikov核，即第3章 粒子滤波方法

24、 (3-37)2opt2(1x ),if120,xxnnxcK其它式中， 是 的单位球的体积。进一步说，当密度函数是服从单位协方差矩阵的高斯分布时，带宽的最优选择为ncxnx1/(4)optnhANx(3-38)其中1/(4)18(4)(2 ) nnnAcnxxxx(3-39)假设分布具有单位协方差矩阵的高斯密度，能够保证估计密度的协方差与样本的经验协方差矩阵S相等。第3章 粒子滤波方法3.8 边缘粒子滤波算法边缘粒子滤波算法3.8.1 问题描述问题描述对于非线性非高斯状态模型，粒子滤波提供了一种通用的计算方法来逼近后验密度函数。然而，虽然粒子滤波很容易实现，适合处理任意的非线性系统估计，但是

25、其主要缺点是计算复杂度随着状态变量维数的增加而迅速增大。为了降低粒子滤波的计算复杂度，文献10提出了边缘粒子滤波算法(Marginalized Particle Filter， MPF)，主要是将状态向量的线性部分进行边缘化处理，在粒子滤波过程中加入卡尔曼滤波算法，即：用卡尔曼滤波处理线性部分，而用常规粒子滤波处理非线性部分。第3章 粒子滤波方法所讨论的非线性非高斯滤波问题，可以概括在一个通用离散时间状态空间模型中，在给定观测值的条件下，递推地计算状态向量的后验概率密度函数。模型方程可表述如下xk+1=f(xk，wk) (3-41) yk=h(xk，ek) (3-42)式中，yk是k时刻的量测

26、值，xk是状态变量，wk是过程噪声，ek是量测噪声，f，h是两个任意的非线性函数。噪声密度 和 是不相关但可知的。在上述通用模型中，只要存在线性子结构，即可利用MPF获得更好的状态估计，同时也能有效降低计算复杂度。该算法的前提条件是可以将状态变量分割为如下形式kwpkep(3-43)lnkkkxxx第3章 粒子滤波方法式中，xlk是线性状态变量，xnk是非线性状态变量，上标1和n的含义分别是线性和非线性。对于线性状态变量，利用贝叶斯定理边缘化处理后用卡尔曼滤波进行估计，而非线性状态变量用标准粒子滤波进行估计。边缘粒子滤波的优点：一是从标准粒子滤波算法中得到的估计方差，可通过利用模型中的线性子结

27、构来降低；二是对相应的线性状态变量进行边缘化处理，并利用最优线性滤波来估计，可以减小计算量。根据系统式(3-41)和式(3-42)，利用粒子滤波得到后验密度p(xk|Yk)的近似，进而由下式可获得函数g(|)的估计(3-44)(|)( ()E ()() (|)dkkkpkkkkkI gggpx YxxxxYx第3章 粒子滤波方法3.8.2 边缘粒子滤波算法步骤边缘粒子滤波算法步骤首先给出MPF算法的一般步骤：Step 1：初始化。初始化粒子，同时置, i = 1，其中N表示粒子数。 Step 2：重要性抽样，即0n,( )n00()nipxxxl,( )l0| 1000,iixpxp( )n,

28、( )1(,)iikkkkqpy | XY(3-45)归一化(3-46)( )( )( )1NiijkkkjqqqStep 3：粒子滤波的量测更新(重采样)：(3-47)n,( )n,( )( )|1Pr()ijjk kk kkqxx第3章 粒子滤波方法Step 4：粒子滤波时间更新和卡尔曼滤波时间更新：(1) 卡尔曼滤波量测更新：Mode 1：式(3-60)式(3-63)；Mode 2：式(3-74)式(3-77)；Mode 3：式(3-91)式(3-94)。(2) 粒子滤波的时间更新，针对每一个粒子预测新的粒子i=1，N(3-48)n,( )nn,( )1|1|(|,)iikkkkkkpx

29、xXY(3) 卡尔曼滤波的时间更新：Mode 1：式(3-64)式(3-65)；Mode 2：式(3-78)式(3-81)；Mode 3：式(3-95)式(3-98)；Step 5：k=k+1，返回至Step 2。第3章 粒子滤波方法只要将上述算法Step 4中的(1)和(3)剔除，就可将其等价于一个标准粒子滤波。在Step 3中引入的噪声可以减弱粒子退化现象。为了详细叙述边缘粒子滤波算法，将系统模型划分为三类，分别加以介绍。其中，第一类模型是第二类模型的特例，而第二类模型又是第三类模型的特例。第3章 粒子滤波方法3.8.3 Model 1：对角模型：对角模型此模型阐述了边缘粒子滤波是怎样开始

30、实现的。(3-49)nnnn1()kkkkxfxwllnll1()kkkkkxA xxwnnl()()kkkkkkkyhxCxxe(3-50)(3-51)上述方程中的间隙是特意空出来的，从形式上看类似矩阵对角形式，故而形象地称其为对角模型。该形式也是为了易于和Model 3式中(3-82)式(3-84)形成对比。假定过程噪声为高斯白噪声(3-52)lnN( ,),0kkkkwwQwlnkkkQQQ00假定量测噪声也为高斯白噪声(3-53)N( ,)kkeR0第3章 粒子滤波方法因此，有l000N(,)xxP(3-54)递归估计后验密度p(xk|Yk)可利用标准粒子滤波实现。然而，受非线性状态变

31、量xnk的约束，式(3-50)给出了一个线性子结构，利用这一事实可以采用卡尔曼滤波更好地估计线性状态。将后验分布p(xk|Yk)中的线性状态变量边缘化(3-55)lnlnn(,|)(,|) (|)kkkkkkkkpppxXYxXYXY式中: p(xlk|Xnk，Yk)是通过卡尔曼滤波得到的；p(Xnk|Yk)是通过粒子滤波估计得到的。如果标准粒子滤波器和边缘粒子滤波器所用的粒子数目相同，那么，边缘粒子滤波能提供更好的估计。原因是p(Xnk|Yk)的维数比p(xlk，Xnk|Yk)的低，这意味着粒子处于低维空间；另外一个原因是利用最优算法估计线性状态变量。第3章 粒子滤波方法用 I sN(g(x

32、k)表示利用标准粒子滤波器得到的表示式(3-44)的估计，而用 ImN(g(xk)表示利用边缘粒子滤波器得到的相应的估计。在一定的假设条件下，根据中心极限定理，有(3-56)(3-57)2( ()( ()N(0,),sNkksN IgI gN xx 2( ()( ()N(0,),mNkkmN IgI gN xx 式中，2s2m。在Model 1下，由卡尔曼滤波算法易知，xlk|k和xlk+1|k的条件概率密度函数由下列式子得出(3-58)(3-59)(3-60)lnl|(,|)N(,)kkkk kk kpxXYxPlnl111|1|(,|)N(,)kkkkkkkpxXYxP式中lll|1|1(

33、)k kk kkkkkk kxxKyhC x第3章 粒子滤波方法(3-61)|1|1k kk kkkk kPPK C PT|1+kkk kkkSC PCRT1|1kk kkkKPC S(3-62)(3-63)且有(3-64)(3-65)lll1|kkk kkxA xllTl1|()kkkk kkkPA PAQ递归运算初始值：。l0| 10,xx0| 10PP对于Model 1，p(yk|Xnk，Yk1)和p(xnk+1|Xnk，Yk)由以下两式得出(3-66)(3-67)nlT1|1|1(|,)N(,)kkkkkk kkk kkkpxyXYhCC PCRnnnnn1(|,)N(,)kkkkkk

34、pxxXYfQ第3章 粒子滤波方法3.8.4 Model 2：三角模型：三角模型通过将Model 1扩展，将非线性状态方程中的项Ank(xnk)xlk包含进来，得到如下三角模型(3-68)(3-69)(3-70)nnnnnln1()()kkkkkkkxfxAxxwllnll1()kkkkkxAxxwnnl()()kkkkkkkyhxCxxe式中符号定义和假定条件与Model 1相同。由式(3-68)式(3-70)可知，包含有线性状态变量的信息，意味着非线性状态变量的预测中含有线性状态变量x1k的信息。为理解这种改变对推导过程的影响，假定MPF算法中Step 4(2)已实现，即预测值是有效的，模

35、型可以写为n1|kkxn1|kkxn1|kkx(3-71)llll1kkkkxA xw第3章 粒子滤波方法(3-72)nlnkkkkzA xw式中(3-73)nn1kkkzxfzk可以理解为量测值，wnk为相应的量测噪声。式(3-71)式(3-73)是一个线性状态空间模型，噪声服从高斯分布，最优状态估计由卡尔曼滤波器通过下式得到(3-74)(3-75)(3-76)(3-77)*llnl|=+()k kk kkkkk kxxLzA x*T|k kk kkkkPPL N LnT1|()kk kkkLPANnnTn|()kkk kkkNA PAQ第3章 粒子滤波方法式中用“*”将第二个量测更新与第一

36、个量测更新区别开来，而且 和Pk|k分别由式(3-60)与式(3-61)得出。最后一步是将第二个量测更新和时间更新合并为状态预测值，结果如下l|k kx (3-78)lllnl1|=+()kkkk kkkkk kxxALzA xllTlT1|()kkkk kkkkkkPA PAQL N LnnT1|()kkk kkkLA PANnnTn|()kkk kkkNA PAQ (3-79) (3-80) (3-81)为了使MPF算法对于较为通用的Model 2有效，用式(3-78)式(3-81)代替卡尔曼滤波中的时间更新方程(3-64)(3-65)。第3章 粒子滤波方法3.8.5 Model 3：一般

37、模型：一般模型上述部分已经说明了边缘粒子滤波的相关机制，这里给出边缘粒子滤波最一般的模型： (3-82)nnnnnlnnn1()()()kkkkkkkkkxfxAxxGxwllnlnllnl1()()()kkkkkkkkkxfxA xxGxwnnl()()kkkkkkkyh xCxxe (3-83) (3-84)式中，状态噪声假定为高斯白噪声 (3-85)lnN( ,),0kkkkwwQwllnlnTn()kkkkkQQQQQ量测噪声也假定为高斯白噪声 (3-86)N( ,)kkeR0因此，xl0服从高斯分布 (3-87)l000N(,)xxP第3章 粒子滤波方法xn0的密度函数是任意的，但是

38、已知的。由贝叶斯准则，滤波分布p(xk|Yk)分解为(3-88)lnlnn(,|)(,|) (|)kkkkkkkkpppxXYxXYXY线性状态变量用卡尔曼滤波进行估计。为了估计线性状态变量，仍需要执行三个步骤：第一步是用yk中的有效信息进行量测更新；第二步是用xnk+1|k中的有效信息进行量测更新；最后一步是时间更新。通过下面的定理来阐述如何估计线性状态变量。定理定理3.1 对于Model 3，xlk和xlk+1的条件概率密度函数由下式得出(3-89)(3-90)(3-91)lnl|(|,)N(,)kkkk kk kpxXYxPlnl111|1|(|,)N(,)kkkkkkkpxXYxPll

39、l|1|1()k kk kkkkkk kxxKyhC xT|1k kk kkkkPPK M K(3-92)第3章 粒子滤波方法(3-93)(3-94)T|1kkk kkkMC PCRT1|1kk kkkKPC M并且lllllnTnn1lnl1|() ()()kkkk kkkkkkkkkkk kxA xGQG QzfLzA xllTlnlTT1|()()kkkk kkkkkkkkPA PAG QGL N LnnTnnnT|()()kkk kkkkkNA PAG QGlnT1|()kkk kkkLA PAN(3-95)(3-96)(3-97)(3-98)式中nn1kkkzxfllllnTnn1n

40、() ()kkkkkkkAAG QG QAlllnTn1ln() ()kkkkkQQQQQ(3-99)(3-100)(3-101)证明：为了简洁，式(3-82)式(3-84)可写为(3-102)(3-103)llllll1kkkkkkxfA xG wanlnnkkkkkzA xG w第3章 粒子滤波方法(3-104)blkkkkzC xe式中，zak和zbk的定义如下(3-105)(3-106)ann1kkkzxfbkkkzyh从数学结构上看，式(3-103)和式(3-104)可看做量测方程，而zak和zbk可看做量测值。事实上，考虑到式(3-85)中Qlnk0，所以wlk和wnk两个噪声过程

41、是相关的，可以利用GramSchmidt过程实现噪声去相关。通过下式(3-107)lllnTnnT1nllnn1nE() (E() )()kkkkkkkkkkkwwwwwwwwQQw替换wlk，得(3-108)lnTE() kkww01llTlln1lnE() ()nkkkkkkkQw wQQQQ(3-109)第3章 粒子滤波方法假定Gnk是可逆的, 由式(3-103)和式(3-107), 式(3-102)可改写为(3-110)llllllnn1n1anll1llllllnnn1al() ()()()kkkkkkkkkkkkkkkkkkkkkkxA xGwQQGzA xfA xG wG QG

42、Qzf式中(3-111)llllnnn1n()kkkkkkkAAG QG QA(3-112)llllllnnn1all1()kkkkkkkkkkkxfA xG QG QzG w去相关系统为该系统是一个具有高斯噪声的线性系统，而且由式(3-105)和式(3-106)可知：如果xnk+1和yk可知，则可得到zak，zbk。第3章 粒子滤波方法下面利用归纳法证明：在0时刻， 。假定在任意时刻是高斯分布。递推分为三步。首先，实际量测值yk中的有效信息zbk是已知的。当进行量测更新时，估计值和Pk|k是已知的，可用这些估计值计算非线性状态变量的预测值，而这些预测值提供了有关系统的新信息。其次，利用zak

43、进行第二次量测更新时，合并新信息。最后，利用第二步的结果进行时间更新。lnlnl0010000(|,)(|)N(,)ppxXYxxxPln1(|,)kkkpxXYl|k kxl|k kxn1|kkx第3章 粒子滤波方法Step 1：假定和zbk已知，可得lnl1|1|1(|,)N(,)kkkk kk kpxXYxPnllnln1nllnl1(|,) (|,)(|,)(|,) (|,)dkkkkkkkkkkkkkkkkpppppyxxxXYxXYyxxxXYx(3-113)由量测噪声和p(yk|xnk，xlk)服从高斯分布，知,式中lnl|(|,)N(,)kkkk kk kpxXYxPlnl|(

44、|,)N(,)kkkk kk kpxXYxP(3-114)(3-115)(3-116)(3-117)llbl|1|1()k kk kkkkk kxxKzC xT|1k kk kkkkPPK M KT-1|1kk kkkKPC MT|1kkk kkkMC PCRStep 2：在该步，zak是已知的。用nnllnln11nnllnl1(|,) (|,)(|,)(|,) (|,)dkkkkkkkkkkkkkkkkpppppxxxxXYx XYxxxxXYx第3章 粒子滤波方法近似，式中lnl*1|1|(|,)N(,)kkkk kk kpxXYxP(3-118)l*lanl|()k kk kkkkk

45、kxxL zA x*T|k kk kkkkPPL N LnT*1|() ()kk kktLPAN*nnTnnnT|()()kkk kkkkkNA PAG QG(3-119)(3-120)(3-121)Step 3：时间更新，计算(3-122)lnlnnllnl11111(|,)(|,) (|,)dkkkkkkkkkkkpppxXYxxxxxXYx由于状态噪声服从高斯分布，相当于是由卡尔曼滤波进行时间更新，因此 ，式中ln11(|,)kkkpxXYl1|1|N(,)kkkkxP(3-123)(3-124)(3-125)lllllnTnn1alanl1|() ()()kkkk kkkkkkkkkk

46、k kLxA xG QG QzfzA xllTlllTT1|()()kkkk kkkkkkkkPA PAG Q GL N LlnT1|()kkk kkkLA PAN第3章 粒子滤波方法对于Model 3，p(yk|Xnk，Yk-1)和p(xnk+1|Xnk，Yk)由下式得出nnTnnnT|()()kkk kkkkkNA PAG QG(3-126)(3-127)(3-128)nlT1|1|1(|,)N(,)kkkkkk kkk kkkpyXYhC xC PCRnnlnlnnTnnnT1|(|,)N,()()kkkkkk kkk kkkkkpxXYfA xA PAG QG式(3-127)式(3-1

47、28)的证明过程省略。证毕。至此，Model 3中状态估计的细节已推导完毕。正如前面指出，此算法和标准粒子滤波的不同点在于预测步骤的区别。如果去掉MPF算法中Step 4(1)和Step 4(3)，就得到了标准粒子滤波算法。第3章 粒子滤波方法线性状态变量的均值和协方差如下(3-129)ll( )l,( )l|(|)1EkkNiik kkk kkpiqx Yxxxll( )( )l,( )ll,( )lT|k|1l2l22(|)(|)()() )E()E() )kkkkNiiiik kkkk kk kk kk kikkppqx Yx YPPxxxxxx(3-130)是归一化重要性权值，由MPF

48、算法中的Step 2给出。由MPF算法过程可以看出，该算法通过两种措施解决了粒子滤波的退化现象： 降低了粒子滤波处理状态变量的维数； 利用卡尔曼滤波改善了粒子滤波的重要性密度函数，从而提高了粒子滤波的估计精度，减少了粒子滤波的计算复杂度。( ) ikq 第3章 粒子滤波方法3.9 扩展卡尔曼粒子滤波算法扩展卡尔曼粒子滤波算法3.9.1 局部线性化局部线性化该方法是将最新的观测值与状态的最优高斯近似结合起来，以此逼近最优重要性分布，通常采用非线性系统的一阶泰勒级数展开。在此框架下，扩展卡尔曼滤波器在给出所有观测值的条件下，通过计算状态的均值来近似系统状态的最优最小均方根误差估计。这是在递推的框架

49、下进行的，通过在时间上传播后验分布的高斯近似。扩展卡尔曼滤波器在计算下面的真实后验滤波密度时将其近似为 。1:()N(,)kkkkpxyxP第3章 粒子滤波方法在粒子滤波算法框架下，用一个单独的扩展卡尔曼滤波器为每个粒子生成一个高斯分布并进行传播，如下式( )( )( )( )0:11:(,)N(,)1,.,iiiikkkkkqiNxxyxP(3-131)即在k1时刻，先用扩展卡尔曼滤波方程加上新的数据，计算重要性分布的每个粒子的均值和方差。然后，从该分布抽取第i个粒子。该方法要求传播协方差 P(i)，并指定扩展卡尔曼滤波过程和量测噪声协方差。这种方法被称为扩展卡尔曼粒子滤波7,12 (Ext

50、ended Kalman Particle Filter， EKPF)。第3章 粒子滤波方法3.9.2 扩展卡尔曼粒子滤波算法步骤扩展卡尔曼粒子滤波算法步骤EKPF算法过程如下：首先初始化粒子。根据p(x0)抽取N个粒子x(i)0，i=1，N；令k=1，进行下列步骤：Step 1：重要性采样。对于i=1，N：(1) 计算动态模型和量测模型的雅可比矩阵F(i)k、G(i)k和H(i)k、U(i)k；(2) 用EKF更新粒子：(3-132)(3-133)(3-134)(3-135)( )( )11()iikk kfxx( )( )( )T( )( )T( )11iiiiiikkkkkkk kPFP