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

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

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

3、标状态序列的状态转移可以用下面的目标状态序列xk,kN的演变方程来描述(3-1)其中，是关于状态xk1的非线性函数，是独立同分布的过程噪声序列，nx,nv分别是状态和过程噪声向量的维数。系统量测方程为(3-2)从贝叶斯估计观点来看，跟踪问题就是计算k时刻状态xk的某种置信程度。从1到k时刻，由于给定量测数据z1: k的值不同，得出的xk值也不同，因此需要构造概率密度函数p(xk|z1: k)。假定初始概率密度函数p(x0|z0)p(x0)，x0表示初始状态向量，z0表示尚且没有获得量测值，它也是先验概率密度函数。因此从形式上看，通过预测和更新两个步骤就可以递推地得到概率密度函数p(xk|z1:

4、 k)的值。假定k1时刻的概率密度函数已知，那么通过ChapmanKolmogorov等式及使用模型(3-1)就可以预测出k时刻状态的先验概率密度函数(3-3)在等式(3-3)中，p(xk|xk1)=p(xk|xk1，z1: k1)，且满足等式(3-1)所描述的一阶马尔可夫过程。状态估计p(xk|xk1)的概率模型由系统等式(3-1)和统计值vk1来确定。在k时刻，可以得到量测值z=，然后通过贝叶斯规则更新先验概率密度函数(3-4)式中的常量(3-5)是由模型(3-2)定义的似然函数p(zk|xk)和一步预测的统计值p(xk|z1: k1)共同确定的。在更新式子(3-4)中，量测值zk被用来修

5、正先验概率，以获得当前状态的后验概率。式(3-3)和式(3-4)是最优贝叶斯估计的一般形式。通过以上步骤递推得到的后验概率只是一般概念下的表达式，通常情况下难以得到其解析表达式。只有在满足特定条件时，才可以得到最优贝叶斯解。3.3 贝叶斯重要性采样在贝叶斯重要性采样中，后验概率分布由一组离散的样本集近似得到。根据大数定理，随着样本粒子数N的增加，期望Eg(x0: k)可由近似求出。但是，通常很难从后验概率密度函数中直接抽样。常规的解决办法是从容易采样的概率分布q(x0:k|y1:k)中采样粒子，由此可以得到(3-6)式中w(x0: k)是未归一化的重要性权值，可以表示为(3-7)由于p(y1:

6、 k)是未知的，我们可以将式(3-6)表示为(3-8)式中Eq(|y1: k)是指在概率分布q(|y1: k)上进行计算的期望。通过从概率函数q(|y1: k)中采样，期望可以近似表示为(3-9)式中的重要性权值表示为(3-10)等式(3-9)计算出来的结果是有偏的。但是，通过以下两个假设可以使逐渐收敛，并且接近于真实值。(1) x(i)0: k是从后验概率分布中采样得到的一组粒子，Eg(x0: k)存在并且是有限的；(2) 在后验概率分布上计算出的wk和wkg2(x0: k)的期望存在而且有限。只有g(x0: k)的方差和重要性权值有限才能验证第二个假设的成立性。随着N值的无限增大，后验概率

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

8、先假设当前状态不依赖于将来的观测，即只进行滤波而不考虑平滑。需要强调的是，在某些情况下，一些建议分布需要用到过去的状态序列。在本书中，不考虑这种情况。假设状态符合马尔可夫过程，在给定状态下，量测值是条件独立的，则可得(3-13)(3-14)将式(3-12)、式(3-13)和式(3-14)代入式(3-7)中得到权值递推公式(3-15)在给定合适的重要性分布函数q(xk|x0: k1，y1: k)的条件下，式(3-15)提供了一个递推计算重要性权值的方法，重要性权值的计算因此得以简化。3.4.2 序贯重要性采样问题及策略1. 选取好的重要性密度函数选择合适的重要性密度函数是重要性采样算法中的关键步

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

10、值，其它粒子的权值非常小，可以忽略不计。出现这种现象是由于随着时间的增加，重要性权值的方差也在增加。退化现象使得大量的计算工作都浪费在更新那些对p(xk|y1: k)的估计几乎不起作用的粒子上。1998年，Liu和Chen给出了一种衡量粒子数匮乏程度的方法3，15，该方法定义“有效粒子数”Neff为(3-17)式中。Neff越小，表明粒子退化现象越严重。一般很难确切计算出Neff的值，但可以用下式计算Neff的估计值Neff(3-18)式中wik是由式(3-15)定义的归一化的权值。由式(3-18)易知NeffN。通过有效粒子数可以衡量当前粒子群的退化程度，当粒子群退化现象比较严重时，采取增加

11、粒子数的办法来减小粒子群的退化程度，但这种方法的计算量太大，制约了算法的实时性。因此，通常采取另一种方法，即在SIS之后对重要性样本进行重采样。3. 重采样重采样是抑制粒子退化现象的一种手段。设定一个有效样本数Nth并将其作为阈值，当Neff0时，T(x，y)0。给定当前状态xk，MH(MetropolisHastings)算法的步骤如下：Step 1：根据建议函数T(xk，y)转移。Step 2：从均匀分布中抽样得到UUniform0，1，然后更新状态(3-26)式中，。显然，当T(x，y)对称时，上述算法等价于Metropolis算法。Barker于1965年提出了另外一种接受函数，即(3

12、-27)Charlesstein提出了更一般的接受函数，即(3-28)式中，(x，y)为任意的对称函数，只需要满足对于任何x和y都有r(x，y)1即可。当选用式(3-28)作为接受函数时，从x转移到y的转移概率可以表示为(3-29)由于(x，y)=(y，x)，故有p(x)A(x，y)=p(y)A(y，x)，即由上述过程产生的马尔可夫链是可逆的，且p(x)是马尔可夫链的平稳分布。3.5.4 马尔可夫链蒙特卡罗粒子滤波算法步骤MCMC粒子滤波算法5通过构造马尔可夫链产生来自目标分布的样本(粒子)，使样本更加多样化，具有很好的收敛性。在粒子滤波中引入MH(MetropolisHastings)采样的

13、具体过程如下：Step 1：按照均匀分布从区间0，1中采样得到门限值u，即uUniform0，1。Step 2：按照概率p(xk|x(i)k1)采样得到。Step 3：若，则接受，即 ；否则丢弃，保留重采样的粒子，即 。MCMC方法的缺陷是, 为了保证收敛所需要的概率转移次数较大, 算法增加的计算量较大, 而且其收敛的判断也是个问题。3.6 辅助粒子滤波算法辅助粒子滤波算法(Auiliary Particle Filter， APF)由Pitt和Shephard提出6，是序贯重要性重采样滤波器的变形。APF是从联合密度函数p(xk，i|z1: k)中得到一个抽样，其中i是辅助变量，表示在k1时

14、刻可对xk预测的采样粒子即xik1xk。引入一个重要性密度函数q(xk，i|z1: k)，进行采样xjk，ijNj=1，其中ij是k1时刻可对xk预测的采样粒子。运用贝叶斯准则可将联合密度函数表示为(3-30)选择重要性密度函数，并使该函数满足以下比例(3-31)其中，ik是在xik1的条件下关于状态变量xk的某种统计信息。一般情况下，可以取均值，或者一个样本满足ikp(xk|xik1)。从重要性密度函数q(xk，i|z1: k)中抽样的粒子(xjk，ij)权值满足(3-32)其中，由式(3-30)和式(3-31)可得出式(3-32)。这些带权值的粒子集 的分布近似于p(xk，i|z1: k)

15、，通过辅助变量ij的替换，即可从p(xk，i|z1: k)中得到想要的抽样粒子。将重要性函数因式分解为(3-33)3.7 正则化粒子滤波算法重采样作为一种减少粒子退化问题的方法在粒子滤波中得到广泛应用，但是也带来了新的问题，最主要的问题就是粒子多样性的消失。这是因为在重采样时，样本是从离散分布而不是连续分布中抽样的。如果此问题得不到很好的处理，将可能导致“粒子崩溃”。“粒子崩溃”是一种很严峻的粒子匮乏现象，即所有的粒子都占据状态空间上的同一个点，从而不能很好地反映后验分布。正则化粒子滤波(Regularized Particle Filter， RPF)算法可以在一定程度上解决此问题16。RP

16、F算法是从后验密度p(xk|z1: k)的连续近似中进行重采样的，即(3-34)其中，是重新调整过的核密度K()，h0是核的带宽(标量)，nx是状态向量x的维数，wik(i=1，2，N)是经过归一化后的权值。核密度是一个对称的概率密度函数,且有(3-35)选择合适的核函数K()和带宽h，使得真实的后验概率密度和相应的正则化经验表示的积分均方误差(Mean Integrated Square Error， MISE)的均值最小，该式定义为(3-36)式中，p(|)表示在条件(3-34)下对p(xk|z1: k)的近似。在特殊情况下，所有的采样有相同的权值，核的最优选择是Epanechnikov核

17、，即 (3-37)式中， 是 的单位球的体积。进一步说，当密度函数是服从单位协方差矩阵的高斯分布时，带宽的最优选择为(3-38)其中(3-39)假设分布具有单位协方差矩阵的高斯密度，能够保证估计密度的协方差与样本的经验协方差矩阵S相等。3.8 边缘粒子滤波算法3.8.1 问题描述对于非线性非高斯状态模型，粒子滤波提供了一种通用的计算方法来逼近后验密度函数。然而，虽然粒子滤波很容易实现，适合处理任意的非线性系统估计，但是其主要缺点是计算复杂度随着状态变量维数的增加而迅速增大。为了降低粒子滤波的计算复杂度，文献10提出了边缘粒子滤波算法(Marginalized Particle Filter，

18、MPF)，主要是将状态向量的线性部分进行边缘化处理，在粒子滤波过程中加入卡尔曼滤波算法，即：用卡尔曼滤波处理线性部分，而用常规粒子滤波处理非线性部分。所讨论的非线性非高斯滤波问题，可以概括在一个通用离散时间状态空间模型中，在给定观测值的条件下，递推地计算状态向量的后验概率密度函数。模型方程可表述如下xk+1=f(xk，wk) (3-41) yk=h(xk，ek) (3-42)式中，yk是k时刻的量测值，xk是状态变量，wk是过程噪声，ek是量测噪声，f，h是两个任意的非线性函数。噪声密度 和 是不相关但可知的。在上述通用模型中，只要存在线性子结构，即可利用MPF获得更好的状态估计，同时也能有效

19、降低计算复杂度。该算法的前提条件是可以将状态变量分割为如下形式(3-43)式中，xlk是线性状态变量，xnk是非线性状态变量，上标1和n的含义分别是线性和非线性。对于线性状态变量，利用贝叶斯定理边缘化处理后用卡尔曼滤波进行估计，而非线性状态变量用标准粒子滤波进行估计。边缘粒子滤波的优点：一是从标准粒子滤波算法中得到的估计方差，可通过利用模型中的线性子结构来降低；二是对相应的线性状态变量进行边缘化处理，并利用最优线性滤波来估计，可以减小计算量。根据系统式(3-41)和式(3-42)，利用粒子滤波得到后验密度p(xk|Yk)的近似，进而由下式可获得函数g(|)的估计(3-44)3.8.2 边缘粒子

20、滤波算法步骤首先给出MPF算法的一般步骤：Step 1：初始化。初始化粒子，同时置, i = 1，其中N表示粒子数。 Step 2：重要性抽样，即(3-45)归一化(3-46)Step 3：粒子滤波的量测更新(重采样)：(3-47)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)(3) 卡尔曼滤波的时间更新：Mode 1：式(3-64)式(3-65)；Mod

21、e 2：式(3-78)式(3-81)；Mode 3：式(3-95)式(3-98)；Step 5：k=k+1，返回至Step 2。只要将上述算法Step 4中的(1)和(3)剔除，就可将其等价于一个标准粒子滤波。在Step 3中引入的噪声可以减弱粒子退化现象。为了详细叙述边缘粒子滤波算法，将系统模型划分为三类，分别加以介绍。其中，第一类模型是第二类模型的特例，而第二类模型又是第三类模型的特例。3.8.3 Model 1：对角模型此模型阐述了边缘粒子滤波是怎样开始实现的。(3-49)(3-50)(3-51)上述方程中的间隙是特意空出来的，从形式上看类似矩阵对角形式，故而形象地称其为对角模型。该形式

22、也是为了易于和Model 3式中(3-82)式(3-84)形成对比。假定过程噪声为高斯白噪声(3-52)假定量测噪声也为高斯白噪声(3-53)因此，有(3-54)递归估计后验密度p(xk|Yk)可利用标准粒子滤波实现。然而，受非线性状态变量xnk的约束，式(3-50)给出了一个线性子结构，利用这一事实可以采用卡尔曼滤波更好地估计线性状态。将后验分布p(xk|Yk)中的线性状态变量边缘化(3-55)式中: p(xlk|Xnk，Yk)是通过卡尔曼滤波得到的；p(Xnk|Yk)是通过粒子滤波估计得到的。如果标准粒子滤波器和边缘粒子滤波器所用的粒子数目相同，那么，边缘粒子滤波能提供更好的估计。原因是p

23、(Xnk|Yk)的维数比p(xlk，Xnk|Yk)的低，这意味着粒子处于低维空间；另外一个原因是利用最优算法估计线性状态变量。用 I sN(g(xk)表示利用标准粒子滤波器得到的表示式(3-44)的估计，而用 ImN(g(xk)表示利用边缘粒子滤波器得到的相应的估计。在一定的假设条件下，根据中心极限定理，有(3-56)(3-57)式中，2s2m。在Model 1下，由卡尔曼滤波算法易知，xlk|k和xlk+1|k的条件概率密度函数由下列式子得出(3-58)(3-59)(3-60)式中(3-61)(3-62)(3-63)且有(3-64)(3-65)递归运算初始值：。对于Model 1，p(yk|

24、Xnk，Yk1)和p(xnk+1|Xnk，Yk)由以下两式得出(3-66)(3-67)3.8.4 Model 2：三角模型通过将Model 1扩展，将非线性状态方程中的项Ank(xnk)xlk包含进来，得到如下三角模型(3-68)(3-69)(3-70)式中符号定义和假定条件与Model 1相同。由式(3-68)式(3-70)可知，包含有线性状态变量的信息，意味着非线性状态变量的预测中含有线性状态变量x1k的信息。为理解这种改变对推导过程的影响，假定MPF算法中Step 4(2)已实现，即预测值是有效的，模型可以写为(3-71)(3-72)式中(3-73)zk可以理解为量测值，wnk为相应的量

25、测噪声。式(3-71)式(3-73)是一个线性状态空间模型，噪声服从高斯分布，最优状态估计由卡尔曼滤波器通过下式得到(3-74)(3-75)(3-76)(3-77)式中用“*”将第二个量测更新与第一个量测更新区别开来，而且 和Pk|k分别由式(3-60)与式(3-61)得出。最后一步是将第二个量测更新和时间更新合并为状态预测值，结果如下 (3-78) (3-79) (3-80) (3-81)为了使MPF算法对于较为通用的Model 2有效，用式(3-78)式(3-81)代替卡尔曼滤波中的时间更新方程(3-64)(3-65)。3.8.5 Model 3：一般模型上述部分已经说明了边缘粒子滤波的相

26、关机制，这里给出边缘粒子滤波最一般的模型： (3-82) (3-83) (3-84)式中，状态噪声假定为高斯白噪声 (3-85)量测噪声也假定为高斯白噪声 (3-86)因此，xl0服从高斯分布 (3-87)xn0的密度函数是任意的，但是已知的。由贝叶斯准则，滤波分布p(xk|Yk)分解为(3-88)线性状态变量用卡尔曼滤波进行估计。为了估计线性状态变量，仍需要执行三个步骤：第一步是用yk中的有效信息进行量测更新；第二步是用xnk+1|k中的有效信息进行量测更新；最后一步是时间更新。通过下面的定理来阐述如何估计线性状态变量。定理3.1 对于Model 3，xlk和xlk+1的条件概率密度函数由下

27、式得出(3-89)(3-90)(3-91)(3-92)(3-93)(3-94)并且(3-95)(3-96)(3-97)(3-98)式中(3-99)(3-100)(3-101)证明：为了简洁，式(3-82)式(3-84)可写为(3-102)(3-103)(3-104)式中，zak和zbk的定义如下(3-105)(3-106)从数学结构上看，式(3-103)和式(3-104)可看做量测方程，而zak和zbk可看做量测值。事实上，考虑到式(3-85)中Qlnk0，所以wlk和wnk两个噪声过程是相关的，可以利用GramSchmidt过程实现噪声去相关。通过下式(3-107)替换wlk，得(3-108

28、)(3-109)假定Gnk是可逆的, 由式(3-103)和式(3-107), 式(3-102)可改写为(3-110)式中(3-111)(3-112)去相关系统为该系统是一个具有高斯噪声的线性系统，而且由式(3-105)和式(3-106)可知：如果xnk+1和yk可知，则可得到zak，zbk。下面利用归纳法证明：在0时刻， 。假定在任意时刻是高斯分布。递推分为三步。首先，实际量测值yk中的有效信息zbk是已知的。当进行量测更新时，估计值和Pk|k是已知的，可用这些估计值计算非线性状态变量的预测值，而这些预测值提供了有关系统的新信息。其次，利用zak进行第二次量测更新时，合并新信息。最后，利用第二

29、步的结果进行时间更新。Step 1：假定和zbk已知，可得(3-113)由量测噪声和p(yk|xnk，xlk)服从高斯分布，知,式中(3-114)(3-115)(3-116)(3-117)Step 2：在该步，zak是已知的。用近似，式中(3-118)(3-119)(3-120)(3-121)Step 3：时间更新，计算(3-122)由于状态噪声服从高斯分布，相当于是由卡尔曼滤波进行时间更新，因此 ，式中(3-123)(3-124)(3-125)对于Model 3，p(yk|Xnk，Yk-1)和p(xnk+1|Xnk，Yk)由下式得出(3-126)(3-127)(3-128)式(3-127)式

30、(3-128)的证明过程省略。证毕。至此，Model 3中状态估计的细节已推导完毕。正如前面指出，此算法和标准粒子滤波的不同点在于预测步骤的区别。如果去掉MPF算法中Step 4(1)和Step 4(3)，就得到了标准粒子滤波算法。线性状态变量的均值和协方差如下(3-129)(3-130)是归一化重要性权值，由MPF算法中的Step 2给出。由MPF算法过程可以看出，该算法通过两种措施解决了粒子滤波的退化现象： 降低了粒子滤波处理状态变量的维数； 利用卡尔曼滤波改善了粒子滤波的重要性密度函数，从而提高了粒子滤波的估计精度，减少了粒子滤波的计算复杂度。3.9 扩展卡尔曼粒子滤波算法3.9.1 局

31、部线性化该方法是将最新的观测值与状态的最优高斯近似结合起来，以此逼近最优重要性分布，通常采用非线性系统的一阶泰勒级数展开。在此框架下，扩展卡尔曼滤波器在给出所有观测值的条件下，通过计算状态的均值来近似系统状态的最优最小均方根误差估计。这是在递推的框架下进行的，通过在时间上传播后验分布的高斯近似。扩展卡尔曼滤波器在计算下面的真实后验滤波密度时将其近似为 。在粒子滤波算法框架下，用一个单独的扩展卡尔曼滤波器为每个粒子生成一个高斯分布并进行传播，如下式(3-131)即在k1时刻，先用扩展卡尔曼滤波方程加上新的数据，计算重要性分布的每个粒子的均值和方差。然后，从该分布抽取第i个粒子。该方法要求传播协方

32、差 P(i)，并指定扩展卡尔曼滤波过程和量测噪声协方差。这种方法被称为扩展卡尔曼粒子滤波7,12 (Extended Kalman Particle Filter， EKPF)。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)(3-136)(3) 采样(4) 设置和。Step 2：对

33、于i=1，N，计算重要性权值 (3-137)然后对重要性权值进行归一化(3-138)Step 3：重采样。用高/低重要性权值来增加/抑制粒子，分别获得N个随机粒子，该过程可以通过常用的重采样算法实现。Step 4：MCMC阶段(该步骤可选)。使用由确定的具有不变分布的马尔可夫转移核来获得；Step 5：利用与标准粒子滤波同样的方法计算状态估计及其协方差，并输出；k=k+1，转向Step 2。在上述Step 4中，可选的MCMC步骤包括了一个用EKF产生的提议分布函数的MH算法，该算法在此处的实现步骤如下：Step4-1：从均匀分布U0，1中采样；Step4-2：计算过程转移模型和量测模型的雅克

34、比矩阵和；Step4-3：用EKF更新粒子状态 (3-139)(3-140)(3-141)(3-142)(3-143)Step 4-4：采样获得候选粒子(3-144)若，则接受移动，即(3-145)否则拒绝移动，即(3-146)3.10 高斯和粒子滤波算法3.10.1 问题描述假设动态模型和量测模型如下xn=f(xn1)+un (3-147)yn=h(xn)+vn (3-148)其中：un和vn是非高斯噪声；为状态向量，是一个具有p(xn|xn1)分布的马尔可夫过程，p(x0)为其初始分布。观测独立于给出的xn，可用分布p(yn|xn)表示。我们的目标是在时间上递推估计滤波分布p(xn|y0:

35、n)和预测分布p(xn+1|y0:n)。3.10.2 高斯噪声条件下的高斯和粒子滤波算法1. 高斯和滤波器首先简要地回顾高斯和滤波器的原理9，17。对于式(3-147)和式(3-148)中的动态模型，假设分布p(x0)是高斯混合形式。状态的滤波和预测结果的分布也可近似地表达为高斯混合形式。(1) 量测更新。假设在n时刻有预测分布(3-149)得到n时刻的观测值yn后，根据预测分布获得滤波分布(3-150)其中Cn是归一化常数。利用式(3-149)，可以将式(3-150)写成(3-151)在系统方程(3-147)和(3-148)中，对于一个加性高斯噪声模型，即yn=h(xn)+vn，其中vnN(

36、0，Rn)，式(3-149)给出了和p(xn|y0:n1)，分布p(xn|y0:n)可用下列高斯和形式逼近(3-152)其中，ni和ni用下面的式子计算(3-153)(3-154)(3-155)(3-156)(3-157)式中(3-158)(2) 时间更新。由于将p(xn|y0:n)表达为一个高斯混合形式，因此下面将预测分布p(xn+1|y0:n)近似为一个高斯混合，通过如下推导来实现(3-159)上述方程右边的每个积分都可近似为一个高斯形式。对于在系统方程(3-147)和(3-148)中的加性高斯噪声模型,即xn=f(xn1)+un，其中unN(0，Qn)，式(3-152)给出了和p(xn|

37、y0:n)，更新预测分布p(xn+1|y0:n)并用高斯和形式逼近(3-160)其中和下式进行更新(3-161)(3-162)(3-163)(3-164)(3) 滤波器实现。采用加权高斯混合模型对p(x0)进行初始化逼近。在量测和时间更新方程中，对EKF方程产生的每个高斯项的均值和协方差进行更新。通过G个并行的扩展卡尔曼滤波器来实现GSFI，高斯混合项权值的调整根据给定的更新方程计算。需要指出的是，采用高斯混合方式进行近似对于“小”协方差是有效的，协方差增大后会导致一些问题。2. 高斯和粒子滤波对于具有加性高斯噪声的非线性系统，高斯和滤波器使用一组并行的扩展卡尔曼滤波器将后验分布近似为高斯混合

38、形式。类似地，下面使用一组并行的高斯粒子滤波器来更新状态向量的后验分布。(1) 量测更新。假设在n时刻，有预测分布(3-165)当获得量测值yn后，滤波分布更新方程如式(3-153)所示。正如高斯和滤波器一样，由给出的式(3-151)右侧的每一项都被近似为高斯分布。更新滤波分布现在可表达为 (3-166)(2) 时间更新。假设在n时刻，有 (3-172)由式(3-161)，预测分布为 (3-173)用高斯粒子滤波器的时间更新算法将右侧的积分近似为一个高斯形式。此时，时间更新分布近似为 (3-174)3.10.3 非高斯噪声与高斯混合模型较高斯噪声而言，非高斯噪声通常更加难以处理。在高斯噪声模型

39、假定下的许多性质都不再适用于非高斯噪声问题。高斯混合模型(GMMs)可以对非高斯分布密度进行近似，而高斯分布项则可以继续使用高斯模型中的性质。下面提供一个通用框架来处理动态模型中的非高斯噪声。在式(3-147)和式(3-148)中具有加性噪声的动态模型，过程噪声为为简化表达，在下面进一步的讨论中假设观测噪声vn是高斯的。在0时刻，p(x0)表示已知的先验知识。x1的预测分布可以写为(3-178)当获得量测值y1后，后验分布可表示为(3-179)其中，C1是一个比例常数(3-180)常数1k是归一化权值，并且(3-181)在得到第n时刻的观测值后，有(3-182)和(3-183)1. 并行动态模

40、型和重采样通过观察动态模型方程(3-147)和(3-148)后发现，我们可以使用K个具有概率密度k的高斯噪声近似动态模型过程。因此，非线性非高斯噪声动态模型等价于非线性高斯噪声动态模型的加权和。所以，由式(3-183)可知，在时刻n，Kn个并行非线性高斯噪声动态模型各自有权值nj。然而，对于一个给定的观测集y1:n，这些并行模型中只有少数几个具有大的权值。从观测更新式(3-179)可以看到，根据后验概率密度函数p(yn|xn)可以对高斯项的权值进行调整。因此，在实际应用中将后验分布p(xn|y1:n)近似为G个加权混合高斯项之和，并直接丢弃那些权值很小的高斯项。即有(3-184)显然，对于一个

41、给定的误差代价函数，G取决于模型方程和权值k。假设在时刻n，通过上述近似给出了p(xn|y1:n)，当得到新的观测值yn+1后，p(xn|y1:n)有GK个高斯项，即(3-185)和(3-186)为了让每个时刻都保持相同数目的高斯项，要利用粒子滤波方法中的重采样。重采样就是丢掉那些权值很小的高斯项，并且复制那些权值大的高斯项。下面给出了一种简单的重采样方法，当然，也可以采用其它的重采样方法。Step 1：根据权值nj的大小将GK个高斯项按降序排列。仅保留前G个高斯项，并把权值和高斯项分别表示为nj和pg(xn|yn)，g=1，G。若最小的权值Gthreshhold，则执行Step 2和Step

42、 3；否则转到Step 3。Step 2：对于g=1，G，抽取一个与n1，nG成比例的数j1，G，令ng，ng=nj，nj并设置nG=1/G。Step 3：对于g=1，G，根据重新初始化权值。threshhold值的选取取决于实际问题，一般情况下小于0.05。将重采样过程应用于式(3-186)，保留G个高斯项。重采样后的近似式为 (3-187)重采样过程保证高斯项的数目在任意时刻都等于G，因此，计算代价就不会浪费到那些权值很小的高斯项上。此外，根据权值大小复制不同数目的高斯项，可保证那些权值较大的高斯项能参与进一步的计算，并起到更多的作用。但因为重采样会丢弃高斯项，所以应该谨慎使用。综上所述，

43、非线性非高斯噪声动态模型问题已经被转换为一个非线性高斯噪声动态模型的加权和，为了保持高斯项的数目不变，加入了重采样过程。2. 加性非高斯噪声动态状态模型的非线性滤波对于非线性系统，在时间更新步获得和在观测更新步获得pj(xn|y1:n)以及权值nj的积分依然是一个问题。换句话说，它需要为每个并行的非线性高斯噪声动态模型获得预测和滤波分布及其权值。该问题可以通过EKF、GPF或GSPF加以解决。一类方法可以将预测和滤波分布的混合项近似为高斯分布。该近似可以通过EKF(或者不敏卡尔曼滤波)和GPF来实现，产生一个高斯滤波并行组合。同时，和pj(xn|y1:n)近似为高斯分布，后验密度p(xn|y1:n1)和p(xn|y1:n)为高斯混合形式。当分别使用EKF和GPF时，由此确定的滤波器被称为GSF和GSPF。该问题分解为如下步骤：更新一个高斯混合模型；使用每一个