第3章---粒子濾波方法課件

1、第3章 粒子濾波方法3.1 引言3.2 貝葉斯濾波3.3 貝葉斯重要性采樣3.4 序貫重要性重采樣粒子濾波算法3.5 馬爾可夫鏈蒙特卡羅粒子濾波算法3.6 輔助粒子濾波算法3.7 正則化粒子濾波算法3.8 邊緣粒子濾波算法3.9 擴(kuò)展卡爾曼粒子濾波算法3.10 高斯和粒子濾波算法3.11 小結(jié)3.1 引 言粒子濾波是指通過(guò)尋找一組在狀態(tài)空間中傳播的隨機(jī)樣本，對(duì)概率密度函數(shù)p(xk|yk)進(jìn)行近似，以樣本均值代替積分運(yùn)算，從而獲得狀態(tài)的最小方差估計(jì)的一種算法。粒子濾波算法依據(jù)系統(tǒng)狀態(tài)向量的先驗(yàn)分布在狀態(tài)空間中產(chǎn)生一組隨機(jī)樣本，然后根據(jù)觀(guān)測(cè)量不斷地調(diào)整粒子的權(quán)值和位置，通過(guò)調(diào)整后的粒子信息修正最初

2、的后驗(yàn)概率函數(shù)。用數(shù)學(xué)語(yǔ)言可描述為：針對(duì)平穩(wěn)的時(shí)變系統(tǒng)，假定k1時(shí)刻系統(tǒng)的后驗(yàn)概率密度為p(xk1|yk1)，依據(jù)一定規(guī)則選取N個(gè)隨機(jī)樣本點(diǎn)，在k時(shí)刻獲得量測(cè)信息后, 經(jīng)過(guò)狀態(tài)更新和時(shí)間更新過(guò)程, N個(gè)粒子的后驗(yàn)概率密度近似為p(xk|yk), 隨著粒子數(shù)的增加，粒子的概率密度函數(shù)就能逼近狀態(tài)真實(shí)的概率密度函數(shù)，對(duì)狀態(tài)向量的估計(jì)結(jié)果與最優(yōu)貝葉斯估計(jì)結(jié)果接近。粒子濾波適用于非線(xiàn)性非高斯系統(tǒng)的狀態(tài)估計(jì)，突破了傳統(tǒng)卡爾曼濾波理論框架，精度可以逼近最優(yōu)估計(jì)，是一種有效的非線(xiàn)性濾波技術(shù)，廣泛應(yīng)用于數(shù)字通信、圖像視頻處理、計(jì)算機(jī)視覺(jué)、語(yǔ)音信號(hào)處理、機(jī)器學(xué)習(xí)等領(lǐng)域。3.2 貝 葉 斯 濾 波對(duì)于跟蹤問(wèn)題，目

3、標(biāo)狀態(tài)序列的狀態(tài)轉(zhuǎn)移可以用下面的目標(biāo)狀態(tài)序列xk,kN的演變方程來(lái)描述(3-1)其中，是關(guān)于狀態(tài)xk1的非線(xiàn)性函數(shù)，是獨(dú)立同分布的過(guò)程噪聲序列，nx,nv分別是狀態(tài)和過(guò)程噪聲向量的維數(shù)。系統(tǒng)量測(cè)方程為(3-2)從貝葉斯估計(jì)觀(guān)點(diǎn)來(lái)看，跟蹤問(wèn)題就是計(jì)算k時(shí)刻狀態(tài)xk的某種置信程度。從1到k時(shí)刻，由于給定量測(cè)數(shù)據(jù)z1: k的值不同，得出的xk值也不同，因此需要構(gòu)造概率密度函數(shù)p(xk|z1: k)。假定初始概率密度函數(shù)p(x0|z0)p(x0)，x0表示初始狀態(tài)向量，z0表示尚且沒(méi)有獲得量測(cè)值，它也是先驗(yàn)概率密度函數(shù)。因此從形式上看，通過(guò)預(yù)測(cè)和更新兩個(gè)步驟就可以遞推地得到概率密度函數(shù)p(xk|z1:

4、 k)的值。假定k1時(shí)刻的概率密度函數(shù)已知，那么通過(guò)ChapmanKolmogorov等式及使用模型(3-1)就可以預(yù)測(cè)出k時(shí)刻狀態(tài)的先驗(yàn)概率密度函數(shù)(3-3)在等式(3-3)中，p(xk|xk1)=p(xk|xk1，z1: k1)，且滿(mǎn)足等式(3-1)所描述的一階馬爾可夫過(guò)程。狀態(tài)估計(jì)p(xk|xk1)的概率模型由系統(tǒng)等式(3-1)和統(tǒng)計(jì)值vk1來(lái)確定。在k時(shí)刻，可以得到量測(cè)值z(mì)=，然后通過(guò)貝葉斯規(guī)則更新先驗(yàn)概率密度函數(shù)(3-4)式中的常量(3-5)是由模型(3-2)定義的似然函數(shù)p(zk|xk)和一步預(yù)測(cè)的統(tǒng)計(jì)值p(xk|z1: k1)共同確定的。在更新式子(3-4)中，量測(cè)值z(mì)k被用來(lái)修

5、正先驗(yàn)概率，以獲得當(dāng)前狀態(tài)的后驗(yàn)概率。式(3-3)和式(3-4)是最優(yōu)貝葉斯估計(jì)的一般形式。通過(guò)以上步驟遞推得到的后驗(yàn)概率只是一般概念下的表達(dá)式，通常情況下難以得到其解析表達(dá)式。只有在滿(mǎn)足特定條件時(shí)，才可以得到最優(yōu)貝葉斯解。3.3 貝葉斯重要性采樣在貝葉斯重要性采樣中，后驗(yàn)概率分布由一組離散的樣本集近似得到。根據(jù)大數(shù)定理，隨著樣本粒子數(shù)N的增加，期望Eg(x0: k)可由近似求出。但是，通常很難從后驗(yàn)概率密度函數(shù)中直接抽樣。常規(guī)的解決辦法是從容易采樣的概率分布q(x0:k|y1:k)中采樣粒子，由此可以得到(3-6)式中w(x0: k)是未歸一化的重要性權(quán)值，可以表示為(3-7)由于p(y1:

6、 k)是未知的，我們可以將式(3-6)表示為(3-8)式中Eq(|y1: k)是指在概率分布q(|y1: k)上進(jìn)行計(jì)算的期望。通過(guò)從概率函數(shù)q(|y1: k)中采樣，期望可以近似表示為(3-9)式中的重要性權(quán)值表示為(3-10)等式(3-9)計(jì)算出來(lái)的結(jié)果是有偏的。但是，通過(guò)以下兩個(gè)假設(shè)可以使逐漸收斂，并且接近于真實(shí)值。(1) x(i)0: k是從后驗(yàn)概率分布中采樣得到的一組粒子，Eg(x0: k)存在并且是有限的；(2) 在后驗(yàn)概率分布上計(jì)算出的wk和wkg2(x0: k)的期望存在而且有限。只有g(shù)(x0: k)的方差和重要性權(quán)值有限才能驗(yàn)證第二個(gè)假設(shè)的成立性。隨著N值的無(wú)限增大，后驗(yàn)概率

7、分布函數(shù)就會(huì)近似于點(diǎn)估計(jì)分布，即(3-11)3.4 序貫重要性重采樣粒子濾波算法3.4.1 序貫重要性采樣貝葉斯重要性采樣(Sequential Importance Sampling， SIS)2, 3是一種簡(jiǎn)單常用的蒙特卡羅積分方法，但是它不能直接用來(lái)做遞推估計(jì)。這主要是因?yàn)楣烙?jì)p(x0: k|y1: k)需要用到所有的觀(guān)測(cè)數(shù)據(jù)y1: k，每次更新觀(guān)測(cè)數(shù)據(jù)yk+1時(shí)，需要重新計(jì)算整個(gè)狀態(tài)序列的重要性權(quán)值，因此它的計(jì)算量隨著時(shí)間的推移而不斷增加。為了解決該問(wèn)題，人們提出了序貫重要性采樣方法。該方法在k時(shí)刻采樣時(shí)不改動(dòng)過(guò)去的狀態(tài)序列x0: k1,而采用如下遞推形式計(jì)算重要性權(quán)值(3-12)這里

8、先假設(shè)當(dāng)前狀態(tài)不依賴(lài)于將來(lái)的觀(guān)測(cè)，即只進(jìn)行濾波而不考慮平滑。需要強(qiáng)調(diào)的是，在某些情況下，一些建議分布需要用到過(guò)去的狀態(tài)序列。在本書(shū)中，不考慮這種情況。假設(shè)狀態(tài)符合馬爾可夫過(guò)程，在給定狀態(tài)下，量測(cè)值是條件獨(dú)立的，則可得(3-13)(3-14)將式(3-12)、式(3-13)和式(3-14)代入式(3-7)中得到權(quán)值遞推公式(3-15)在給定合適的重要性分布函數(shù)q(xk|x0: k1，y1: k)的條件下，式(3-15)提供了一個(gè)遞推計(jì)算重要性權(quán)值的方法，重要性權(quán)值的計(jì)算因此得以簡(jiǎn)化。3.4.2 序貫重要性采樣問(wèn)題及策略1. 選取好的重要性密度函數(shù)選擇合適的重要性密度函數(shù)是重要性采樣算法中的關(guān)鍵步

9、驟。選擇重要性密度函數(shù)的一個(gè)原則是使得權(quán)系數(shù)的方差最小。Doucet等提出了在給定x0: k1和y1: k的條件下權(quán)系數(shù)方差最小的最優(yōu)密度函數(shù)許多學(xué)者也都證明了上式成立。盡管如此，為方便起見(jiàn)，多數(shù)粒子濾波算法中的重要性函數(shù)還是選擇了次優(yōu)的q(xk|x0: k1, y1: k)=p(xk|x0: k1)。由于未能利用最新的量測(cè)信息，以該函數(shù)進(jìn)行抽樣所產(chǎn)生的方差比后驗(yàn)概率p(xk|x0: k1，y1: k)產(chǎn)生的方差大，但由于其容易實(shí)現(xiàn)，在粒子濾波算法中依然得到了廣泛的應(yīng)用。(3-16)2. 粒子退化問(wèn)題序貫重要性采樣方法的一個(gè)嚴(yán)重缺陷是粒子退化問(wèn)題，即經(jīng)過(guò)幾次迭代后，可能只有少數(shù)幾個(gè)粒子有非零權(quán)

10、值，其它粒子的權(quán)值非常小，可以忽略不計(jì)。出現(xiàn)這種現(xiàn)象是由于隨著時(shí)間的增加，重要性權(quán)值的方差也在增加。退化現(xiàn)象使得大量的計(jì)算工作都浪費(fèi)在更新那些對(duì)p(xk|y1: k)的估計(jì)幾乎不起作用的粒子上。1998年，Liu和Chen給出了一種衡量粒子數(shù)匱乏程度的方法3，15，該方法定義“有效粒子數(shù)”Neff為(3-17)式中。Neff越小，表明粒子退化現(xiàn)象越嚴(yán)重。一般很難確切計(jì)算出Neff的值，但可以用下式計(jì)算Neff的估計(jì)值Neff(3-18)式中wik是由式(3-15)定義的歸一化的權(quán)值。由式(3-18)易知NeffN。通過(guò)有效粒子數(shù)可以衡量當(dāng)前粒子群的退化程度，當(dāng)粒子群退化現(xiàn)象比較嚴(yán)重時(shí)，采取增加

11、粒子數(shù)的辦法來(lái)減小粒子群的退化程度，但這種方法的計(jì)算量太大，制約了算法的實(shí)時(shí)性。因此，通常采取另一種方法，即在SIS之后對(duì)重要性樣本進(jìn)行重采樣。3. 重采樣重采樣是抑制粒子退化現(xiàn)象的一種手段。設(shè)定一個(gè)有效樣本數(shù)Nth并將其作為閾值，當(dāng)Neff0時(shí)，T(x，y)0。給定當(dāng)前狀態(tài)xk，MH(MetropolisHastings)算法的步驟如下：Step 1：根據(jù)建議函數(shù)T(xk，y)轉(zhuǎn)移。Step 2：從均勻分布中抽樣得到UUniform0，1，然后更新?tīng)顟B(tài)(3-26)式中，。顯然，當(dāng)T(x，y)對(duì)稱(chēng)時(shí)，上述算法等價(jià)于Metropolis算法。Barker于1965年提出了另外一種接受函數(shù)，即(3

12、-27)Charlesstein提出了更一般的接受函數(shù)，即(3-28)式中，(x，y)為任意的對(duì)稱(chēng)函數(shù)，只需要滿(mǎn)足對(duì)于任何x和y都有r(x，y)1即可。當(dāng)選用式(3-28)作為接受函數(shù)時(shí)，從x轉(zhuǎn)移到y(tǒng)的轉(zhuǎn)移概率可以表示為(3-29)由于(x，y)=(y，x)，故有p(x)A(x，y)=p(y)A(y，x)，即由上述過(guò)程產(chǎn)生的馬爾可夫鏈?zhǔn)强赡娴?，且p(x)是馬爾可夫鏈的平穩(wěn)分布。3.5.4 馬爾可夫鏈蒙特卡羅粒子濾波算法步驟MCMC粒子濾波算法5通過(guò)構(gòu)造馬爾可夫鏈產(chǎn)生來(lái)自目標(biāo)分布的樣本(粒子)，使樣本更加多樣化，具有很好的收斂性。在粒子濾波中引入MH(MetropolisHastings)采樣的

13、具體過(guò)程如下：Step 1：按照均勻分布從區(qū)間0，1中采樣得到門(mén)限值u，即uUniform0，1。Step 2：按照概率p(xk|x(i)k1)采樣得到。Step 3：若，則接受，即 ；否則丟棄，保留重采樣的粒子，即 。MCMC方法的缺陷是, 為了保證收斂所需要的概率轉(zhuǎn)移次數(shù)較大, 算法增加的計(jì)算量較大, 而且其收斂的判斷也是個(gè)問(wèn)題。3.6 輔助粒子濾波算法輔助粒子濾波算法(Auiliary Particle Filter， APF)由Pitt和Shephard提出6，是序貫重要性重采樣濾波器的變形。APF是從聯(lián)合密度函數(shù)p(xk，i|z1: k)中得到一個(gè)抽樣，其中i是輔助變量，表示在k1時(shí)

14、刻可對(duì)xk預(yù)測(cè)的采樣粒子即xik1xk。引入一個(gè)重要性密度函數(shù)q(xk，i|z1: k)，進(jìn)行采樣xjk，ijNj=1，其中ij是k1時(shí)刻可對(duì)xk預(yù)測(cè)的采樣粒子。運(yùn)用貝葉斯準(zhǔn)則可將聯(lián)合密度函數(shù)表示為(3-30)選擇重要性密度函數(shù)，并使該函數(shù)滿(mǎn)足以下比例(3-31)其中，ik是在xik1的條件下關(guān)于狀態(tài)變量xk的某種統(tǒng)計(jì)信息。一般情況下，可以取均值，或者一個(gè)樣本滿(mǎn)足ikp(xk|xik1)。從重要性密度函數(shù)q(xk，i|z1: k)中抽樣的粒子(xjk，ij)權(quán)值滿(mǎn)足(3-32)其中，由式(3-30)和式(3-31)可得出式(3-32)。這些帶權(quán)值的粒子集 的分布近似于p(xk，i|z1: k)

15、，通過(guò)輔助變量ij的替換，即可從p(xk，i|z1: k)中得到想要的抽樣粒子。將重要性函數(shù)因式分解為(3-33)3.7 正則化粒子濾波算法重采樣作為一種減少粒子退化問(wèn)題的方法在粒子濾波中得到廣泛應(yīng)用，但是也帶來(lái)了新的問(wèn)題，最主要的問(wèn)題就是粒子多樣性的消失。這是因?yàn)樵谥夭蓸訒r(shí)，樣本是從離散分布而不是連續(xù)分布中抽樣的。如果此問(wèn)題得不到很好的處理，將可能導(dǎo)致“粒子崩潰”?！傲Ｗ颖罎ⅰ笔且环N很?chē)?yán)峻的粒子匱乏現(xiàn)象，即所有的粒子都占據(jù)狀態(tài)空間上的同一個(gè)點(diǎn)，從而不能很好地反映后驗(yàn)分布。正則化粒子濾波(Regularized Particle Filter， RPF)算法可以在一定程度上解決此問(wèn)題16。RP

16、F算法是從后驗(yàn)密度p(xk|z1: k)的連續(xù)近似中進(jìn)行重采樣的，即(3-34)其中，是重新調(diào)整過(guò)的核密度K()，h0是核的帶寬(標(biāo)量)，nx是狀態(tài)向量x的維數(shù)，wik(i=1，2，N)是經(jīng)過(guò)歸一化后的權(quán)值。核密度是一個(gè)對(duì)稱(chēng)的概率密度函數(shù),且有(3-35)選擇合適的核函數(shù)K()和帶寬h，使得真實(shí)的后驗(yàn)概率密度和相應(yīng)的正則化經(jīng)驗(yàn)表示的積分均方誤差(Mean Integrated Square Error， MISE)的均值最小，該式定義為(3-36)式中，p(|)表示在條件(3-34)下對(duì)p(xk|z1: k)的近似。在特殊情況下，所有的采樣有相同的權(quán)值，核的最優(yōu)選擇是Epanechnikov核

17、，即 (3-37)式中， 是 的單位球的體積。進(jìn)一步說(shuō)，當(dāng)密度函數(shù)是服從單位協(xié)方差矩陣的高斯分布時(shí)，帶寬的最優(yōu)選擇為(3-38)其中(3-39)假設(shè)分布具有單位協(xié)方差矩陣的高斯密度，能夠保證估計(jì)密度的協(xié)方差與樣本的經(jīng)驗(yàn)協(xié)方差矩陣S相等。3.8 邊緣粒子濾波算法3.8.1 問(wèn)題描述對(duì)于非線(xiàn)性非高斯?fàn)顟B(tài)模型，粒子濾波提供了一種通用的計(jì)算方法來(lái)逼近后驗(yàn)密度函數(shù)。然而，雖然粒子濾波很容易實(shí)現(xiàn)，適合處理任意的非線(xiàn)性系統(tǒng)估計(jì)，但是其主要缺點(diǎn)是計(jì)算復(fù)雜度隨著狀態(tài)變量維數(shù)的增加而迅速增大。為了降低粒子濾波的計(jì)算復(fù)雜度，文獻(xiàn)10提出了邊緣粒子濾波算法(Marginalized Particle Filter，

18、MPF)，主要是將狀態(tài)向量的線(xiàn)性部分進(jìn)行邊緣化處理，在粒子濾波過(guò)程中加入卡爾曼濾波算法，即：用卡爾曼濾波處理線(xiàn)性部分，而用常規(guī)粒子濾波處理非線(xiàn)性部分。所討論的非線(xiàn)性非高斯濾波問(wèn)題，可以概括在一個(gè)通用離散時(shí)間狀態(tài)空間模型中，在給定觀(guān)測(cè)值的條件下，遞推地計(jì)算狀態(tài)向量的后驗(yàn)概率密度函數(shù)。模型方程可表述如下xk+1=f(xk，wk) (3-41) yk=h(xk，ek) (3-42)式中，yk是k時(shí)刻的量測(cè)值，xk是狀態(tài)變量，wk是過(guò)程噪聲，ek是量測(cè)噪聲，f，h是兩個(gè)任意的非線(xiàn)性函數(shù)。噪聲密度 和 是不相關(guān)但可知的。在上述通用模型中，只要存在線(xiàn)性子結(jié)構(gòu)，即可利用MPF獲得更好的狀態(tài)估計(jì)，同時(shí)也能有效

19、降低計(jì)算復(fù)雜度。該算法的前提條件是可以將狀態(tài)變量分割為如下形式(3-43)式中，xlk是線(xiàn)性狀態(tài)變量，xnk是非線(xiàn)性狀態(tài)變量，上標(biāo)1和n的含義分別是線(xiàn)性和非線(xiàn)性。對(duì)于線(xiàn)性狀態(tài)變量，利用貝葉斯定理邊緣化處理后用卡爾曼濾波進(jìn)行估計(jì)，而非線(xiàn)性狀態(tài)變量用標(biāo)準(zhǔn)粒子濾波進(jìn)行估計(jì)。邊緣粒子濾波的優(yōu)點(diǎn)：一是從標(biāo)準(zhǔn)粒子濾波算法中得到的估計(jì)方差，可通過(guò)利用模型中的線(xiàn)性子結(jié)構(gòu)來(lái)降低；二是對(duì)相應(yīng)的線(xiàn)性狀態(tài)變量進(jìn)行邊緣化處理，并利用最優(yōu)線(xiàn)性濾波來(lái)估計(jì)，可以減小計(jì)算量。根據(jù)系統(tǒng)式(3-41)和式(3-42)，利用粒子濾波得到后驗(yàn)密度p(xk|Yk)的近似，進(jìn)而由下式可獲得函數(shù)g(|)的估計(jì)(3-44)3.8.2 邊緣粒子

20、濾波算法步驟首先給出MPF算法的一般步驟：Step 1：初始化。初始化粒子，同時(shí)置, i = 1，其中N表示粒子數(shù)。 Step 2：重要性抽樣，即(3-45)歸一化(3-46)Step 3：粒子濾波的量測(cè)更新(重采樣)：(3-47)Step 4：粒子濾波時(shí)間更新和卡爾曼濾波時(shí)間更新：(1) 卡爾曼濾波量測(cè)更新：Mode 1：式(3-60)式(3-63)；Mode 2：式(3-74)式(3-77)；Mode 3：式(3-91)式(3-94)。(2) 粒子濾波的時(shí)間更新，針對(duì)每一個(gè)粒子預(yù)測(cè)新的粒子i=1，N(3-48)(3) 卡爾曼濾波的時(shí)間更新：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)剔除，就可將其等價(jià)于一個(gè)標(biāo)準(zhǔn)粒子濾波。在Step 3中引入的噪聲可以減弱粒子退化現(xiàn)象。為了詳細(xì)敘述邊緣粒子濾波算法，將系統(tǒng)模型劃分為三類(lèi)，分別加以介紹。其中，第一類(lèi)模型是第二類(lèi)模型的特例，而第二類(lèi)模型又是第三類(lèi)模型的特例。3.8.3 Model 1：對(duì)角模型此模型闡述了邊緣粒子濾波是怎樣開(kāi)始實(shí)現(xiàn)的。(3-49)(3-50)(3-51)上述方程中的間隙是特意空出來(lái)的，從形式上看類(lèi)似矩陣對(duì)角形式，故而形象地稱(chēng)其為對(duì)角模型。該形式

22、也是為了易于和Model 3式中(3-82)式(3-84)形成對(duì)比。假定過(guò)程噪聲為高斯白噪聲(3-52)假定量測(cè)噪聲也為高斯白噪聲(3-53)因此，有(3-54)遞歸估計(jì)后驗(yàn)密度p(xk|Yk)可利用標(biāo)準(zhǔn)粒子濾波實(shí)現(xiàn)。然而，受非線(xiàn)性狀態(tài)變量xnk的約束，式(3-50)給出了一個(gè)線(xiàn)性子結(jié)構(gòu)，利用這一事實(shí)可以采用卡爾曼濾波更好地估計(jì)線(xiàn)性狀態(tài)。將后驗(yàn)分布p(xk|Yk)中的線(xiàn)性狀態(tài)變量邊緣化(3-55)式中: p(xlk|Xnk，Yk)是通過(guò)卡爾曼濾波得到的；p(Xnk|Yk)是通過(guò)粒子濾波估計(jì)得到的。如果標(biāo)準(zhǔn)粒子濾波器和邊緣粒子濾波器所用的粒子數(shù)目相同，那么，邊緣粒子濾波能提供更好的估計(jì)。原因是p

23、(Xnk|Yk)的維數(shù)比p(xlk，Xnk|Yk)的低，這意味著粒子處于低維空間；另外一個(gè)原因是利用最優(yōu)算法估計(jì)線(xiàn)性狀態(tài)變量。用 I sN(g(xk)表示利用標(biāo)準(zhǔn)粒子濾波器得到的表示式(3-44)的估計(jì)，而用 ImN(g(xk)表示利用邊緣粒子濾波器得到的相應(yīng)的估計(jì)。在一定的假設(shè)條件下，根據(jù)中心極限定理，有(3-56)(3-57)式中，2s2m。在Model 1下，由卡爾曼濾波算法易知，xlk|k和xlk+1|k的條件概率密度函數(shù)由下列式子得出(3-58)(3-59)(3-60)式中(3-61)(3-62)(3-63)且有(3-64)(3-65)遞歸運(yùn)算初始值：。對(duì)于Model 1，p(yk|

24、Xnk，Yk1)和p(xnk+1|Xnk，Yk)由以下兩式得出(3-66)(3-67)3.8.4 Model 2：三角模型通過(guò)將Model 1擴(kuò)展，將非線(xiàn)性狀態(tài)方程中的項(xiàng)Ank(xnk)xlk包含進(jìn)來(lái)，得到如下三角模型(3-68)(3-69)(3-70)式中符號(hào)定義和假定條件與Model 1相同。由式(3-68)式(3-70)可知，包含有線(xiàn)性狀態(tài)變量的信息，意味著非線(xiàn)性狀態(tài)變量的預(yù)測(cè)中含有線(xiàn)性狀態(tài)變量x1k的信息。為理解這種改變對(duì)推導(dǎo)過(guò)程的影響，假定MPF算法中Step 4(2)已實(shí)現(xiàn)，即預(yù)測(cè)值是有效的，模型可以寫(xiě)為(3-71)(3-72)式中(3-73)zk可以理解為量測(cè)值，wnk為相應(yīng)的量

25、測(cè)噪聲。式(3-71)式(3-73)是一個(gè)線(xiàn)性狀態(tài)空間模型，噪聲服從高斯分布，最優(yōu)狀態(tài)估計(jì)由卡爾曼濾波器通過(guò)下式得到(3-74)(3-75)(3-76)(3-77)式中用“*”將第二個(gè)量測(cè)更新與第一個(gè)量測(cè)更新區(qū)別開(kāi)來(lái)，而且 和Pk|k分別由式(3-60)與式(3-61)得出。最后一步是將第二個(gè)量測(cè)更新和時(shí)間更新合并為狀態(tài)預(yù)測(cè)值，結(jié)果如下 (3-78) (3-79) (3-80) (3-81)為了使MPF算法對(duì)于較為通用的Model 2有效，用式(3-78)式(3-81)代替卡爾曼濾波中的時(shí)間更新方程(3-64)(3-65)。3.8.5 Model 3：一般模型上述部分已經(jīng)說(shuō)明了邊緣粒子濾波的相

26、關(guān)機(jī)制，這里給出邊緣粒子濾波最一般的模型： (3-82) (3-83) (3-84)式中，狀態(tài)噪聲假定為高斯白噪聲 (3-85)量測(cè)噪聲也假定為高斯白噪聲 (3-86)因此，xl0服從高斯分布 (3-87)xn0的密度函數(shù)是任意的，但是已知的。由貝葉斯準(zhǔn)則，濾波分布p(xk|Yk)分解為(3-88)線(xiàn)性狀態(tài)變量用卡爾曼濾波進(jìn)行估計(jì)。為了估計(jì)線(xiàn)性狀態(tài)變量，仍需要執(zhí)行三個(gè)步驟：第一步是用yk中的有效信息進(jìn)行量測(cè)更新；第二步是用xnk+1|k中的有效信息進(jìn)行量測(cè)更新；最后一步是時(shí)間更新。通過(guò)下面的定理來(lái)闡述如何估計(jì)線(xiàn)性狀態(tài)變量。定理3.1 對(duì)于Model 3，xlk和xlk+1的條件概率密度函數(shù)由下

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)證明：為了簡(jiǎn)潔，式(3-82)式(3-84)可寫(xiě)為(3-102)(3-103)(3-104)式中，zak和zbk的定義如下(3-105)(3-106)從數(shù)學(xué)結(jié)構(gòu)上看，式(3-103)和式(3-104)可看做量測(cè)方程，而zak和zbk可看做量測(cè)值。事實(shí)上，考慮到式(3-85)中Qlnk0，所以wlk和wnk兩個(gè)噪聲過(guò)程是相關(guān)的，可以利用GramSchmidt過(guò)程實(shí)現(xiàn)噪聲去相關(guān)。通過(guò)下式(3-107)替換wlk，得(3-108

28、)(3-109)假定Gnk是可逆的, 由式(3-103)和式(3-107), 式(3-102)可改寫(xiě)為(3-110)式中(3-111)(3-112)去相關(guān)系統(tǒng)為該系統(tǒng)是一個(gè)具有高斯噪聲的線(xiàn)性系統(tǒng)，而且由式(3-105)和式(3-106)可知：如果xnk+1和yk可知，則可得到zak，zbk。下面利用歸納法證明：在0時(shí)刻， 。假定在任意時(shí)刻是高斯分布。遞推分為三步。首先，實(shí)際量測(cè)值yk中的有效信息zbk是已知的。當(dāng)進(jìn)行量測(cè)更新時(shí)，估計(jì)值和Pk|k是已知的，可用這些估計(jì)值計(jì)算非線(xiàn)性狀態(tài)變量的預(yù)測(cè)值，而這些預(yù)測(cè)值提供了有關(guān)系統(tǒng)的新信息。其次，利用zak進(jìn)行第二次量測(cè)更新時(shí)，合并新信息。最后，利用第二

29、步的結(jié)果進(jìn)行時(shí)間更新。Step 1：假定和zbk已知，可得(3-113)由量測(cè)噪聲和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：時(shí)間更新，計(jì)算(3-122)由于狀態(tài)噪聲服從高斯分布，相當(dāng)于是由卡爾曼濾波進(jìn)行時(shí)間更新，因此 ，式中(3-123)(3-124)(3-125)對(duì)于Model 3，p(yk|Xnk，Yk-1)和p(xnk+1|Xnk，Yk)由下式得出(3-126)(3-127)(3-128)式(3-127)式

30、(3-128)的證明過(guò)程省略。證畢。至此，Model 3中狀態(tài)估計(jì)的細(xì)節(jié)已推導(dǎo)完畢。正如前面指出，此算法和標(biāo)準(zhǔn)粒子濾波的不同點(diǎn)在于預(yù)測(cè)步驟的區(qū)別。如果去掉MPF算法中Step 4(1)和Step 4(3)，就得到了標(biāo)準(zhǔn)粒子濾波算法。線(xiàn)性狀態(tài)變量的均值和協(xié)方差如下(3-129)(3-130)是歸一化重要性權(quán)值，由MPF算法中的Step 2給出。由MPF算法過(guò)程可以看出，該算法通過(guò)兩種措施解決了粒子濾波的退化現(xiàn)象： 降低了粒子濾波處理狀態(tài)變量的維數(shù)； 利用卡爾曼濾波改善了粒子濾波的重要性密度函數(shù)，從而提高了粒子濾波的估計(jì)精度，減少了粒子濾波的計(jì)算復(fù)雜度。3.9 擴(kuò)展卡爾曼粒子濾波算法3.9.1 局

31、部線(xiàn)性化該方法是將最新的觀(guān)測(cè)值與狀態(tài)的最優(yōu)高斯近似結(jié)合起來(lái)，以此逼近最優(yōu)重要性分布，通常采用非線(xiàn)性系統(tǒng)的一階泰勒級(jí)數(shù)展開(kāi)。在此框架下，擴(kuò)展卡爾曼濾波器在給出所有觀(guān)測(cè)值的條件下，通過(guò)計(jì)算狀態(tài)的均值來(lái)近似系統(tǒng)狀態(tài)的最優(yōu)最小均方根誤差估計(jì)。這是在遞推的框架下進(jìn)行的，通過(guò)在時(shí)間上傳播后驗(yàn)分布的高斯近似。擴(kuò)展卡爾曼濾波器在計(jì)算下面的真實(shí)后驗(yàn)濾波密度時(shí)將其近似為 。在粒子濾波算法框架下，用一個(gè)單獨(dú)的擴(kuò)展卡爾曼濾波器為每個(gè)粒子生成一個(gè)高斯分布并進(jìn)行傳播，如下式(3-131)即在k1時(shí)刻，先用擴(kuò)展卡爾曼濾波方程加上新的數(shù)據(jù)，計(jì)算重要性分布的每個(gè)粒子的均值和方差。然后，從該分布抽取第i個(gè)粒子。該方法要求傳播協(xié)方

32、差 P(i)，并指定擴(kuò)展卡爾曼濾波過(guò)程和量測(cè)噪聲協(xié)方差。這種方法被稱(chēng)為擴(kuò)展卡爾曼粒子濾波7,12 (Extended Kalman Particle Filter， EKPF)。3.9.2 擴(kuò)展卡爾曼粒子濾波算法步驟EKPF算法過(guò)程如下：首先初始化粒子。根據(jù)p(x0)抽取N個(gè)粒子x(i)0，i=1，N；令k=1，進(jìn)行下列步驟：Step 1：重要性采樣。對(duì)于i=1，N：(1) 計(jì)算動(dòng)態(tài)模型和量測(cè)模型的雅可比矩陣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) 設(shè)置和。Step 2：對(duì)

33、于i=1，N，計(jì)算重要性權(quán)值 (3-137)然后對(duì)重要性權(quán)值進(jìn)行歸一化(3-138)Step 3：重采樣。用高/低重要性權(quán)值來(lái)增加/抑制粒子，分別獲得N個(gè)隨機(jī)粒子，該過(guò)程可以通過(guò)常用的重采樣算法實(shí)現(xiàn)。Step 4：MCMC階段(該步驟可選)。使用由確定的具有不變分布的馬爾可夫轉(zhuǎn)移核來(lái)獲得；Step 5：利用與標(biāo)準(zhǔn)粒子濾波同樣的方法計(jì)算狀態(tài)估計(jì)及其協(xié)方差，并輸出；k=k+1，轉(zhuǎn)向Step 2。在上述Step 4中，可選的MCMC步驟包括了一個(gè)用EKF產(chǎn)生的提議分布函數(shù)的MH算法，該算法在此處的實(shí)現(xiàn)步驟如下：Step4-1：從均勻分布U0，1中采樣；Step4-2：計(jì)算過(guò)程轉(zhuǎn)移模型和量測(cè)模型的雅克

34、比矩陣和；Step4-3：用EKF更新粒子狀態(tài) (3-139)(3-140)(3-141)(3-142)(3-143)Step 4-4：采樣獲得候選粒子(3-144)若，則接受移動(dòng)，即(3-145)否則拒絕移動(dòng)，即(3-146)3.10 高斯和粒子濾波算法3.10.1 問(wèn)題描述假設(shè)動(dòng)態(tài)模型和量測(cè)模型如下xn=f(xn1)+un (3-147)yn=h(xn)+vn (3-148)其中：un和vn是非高斯噪聲；為狀態(tài)向量，是一個(gè)具有p(xn|xn1)分布的馬爾可夫過(guò)程，p(x0)為其初始分布。觀(guān)測(cè)獨(dú)立于給出的xn，可用分布p(yn|xn)表示。我們的目標(biāo)是在時(shí)間上遞推估計(jì)濾波分布p(xn|y0:

35、n)和預(yù)測(cè)分布p(xn+1|y0:n)。3.10.2 高斯噪聲條件下的高斯和粒子濾波算法1. 高斯和濾波器首先簡(jiǎn)要地回顧高斯和濾波器的原理9，17。對(duì)于式(3-147)和式(3-148)中的動(dòng)態(tài)模型，假設(shè)分布p(x0)是高斯混合形式。狀態(tài)的濾波和預(yù)測(cè)結(jié)果的分布也可近似地表達(dá)為高斯混合形式。(1) 量測(cè)更新。假設(shè)在n時(shí)刻有預(yù)測(cè)分布(3-149)得到n時(shí)刻的觀(guān)測(cè)值yn后，根據(jù)預(yù)測(cè)分布獲得濾波分布(3-150)其中Cn是歸一化常數(shù)。利用式(3-149)，可以將式(3-150)寫(xiě)成(3-151)在系統(tǒng)方程(3-147)和(3-148)中，對(duì)于一個(gè)加性高斯噪聲模型，即yn=h(xn)+vn，其中vnN(

36、0，Rn)，式(3-149)給出了和p(xn|y0:n1)，分布p(xn|y0:n)可用下列高斯和形式逼近(3-152)其中，ni和ni用下面的式子計(jì)算(3-153)(3-154)(3-155)(3-156)(3-157)式中(3-158)(2) 時(shí)間更新。由于將p(xn|y0:n)表達(dá)為一個(gè)高斯混合形式，因此下面將預(yù)測(cè)分布p(xn+1|y0:n)近似為一個(gè)高斯混合，通過(guò)如下推導(dǎo)來(lái)實(shí)現(xiàn)(3-159)上述方程右邊的每個(gè)積分都可近似為一個(gè)高斯形式。對(duì)于在系統(tǒng)方程(3-147)和(3-148)中的加性高斯噪聲模型,即xn=f(xn1)+un，其中unN(0，Qn)，式(3-152)給出了和p(xn|

37、y0:n)，更新預(yù)測(cè)分布p(xn+1|y0:n)并用高斯和形式逼近(3-160)其中和下式進(jìn)行更新(3-161)(3-162)(3-163)(3-164)(3) 濾波器實(shí)現(xiàn)。采用加權(quán)高斯混合模型對(duì)p(x0)進(jìn)行初始化逼近。在量測(cè)和時(shí)間更新方程中，對(duì)EKF方程產(chǎn)生的每個(gè)高斯項(xiàng)的均值和協(xié)方差進(jìn)行更新。通過(guò)G個(gè)并行的擴(kuò)展卡爾曼濾波器來(lái)實(shí)現(xiàn)GSFI，高斯混合項(xiàng)權(quán)值的調(diào)整根據(jù)給定的更新方程計(jì)算。需要指出的是，采用高斯混合方式進(jìn)行近似對(duì)于“小”協(xié)方差是有效的，協(xié)方差增大后會(huì)導(dǎo)致一些問(wèn)題。2. 高斯和粒子濾波對(duì)于具有加性高斯噪聲的非線(xiàn)性系統(tǒng)，高斯和濾波器使用一組并行的擴(kuò)展卡爾曼濾波器將后驗(yàn)分布近似為高斯混合

38、形式。類(lèi)似地，下面使用一組并行的高斯粒子濾波器來(lái)更新?tīng)顟B(tài)向量的后驗(yàn)分布。(1) 量測(cè)更新。假設(shè)在n時(shí)刻，有預(yù)測(cè)分布(3-165)當(dāng)獲得量測(cè)值yn后，濾波分布更新方程如式(3-153)所示。正如高斯和濾波器一樣，由給出的式(3-151)右側(cè)的每一項(xiàng)都被近似為高斯分布。更新濾波分布現(xiàn)在可表達(dá)為 (3-166)(2) 時(shí)間更新。假設(shè)在n時(shí)刻，有 (3-172)由式(3-161)，預(yù)測(cè)分布為 (3-173)用高斯粒子濾波器的時(shí)間更新算法將右側(cè)的積分近似為一個(gè)高斯形式。此時(shí)，時(shí)間更新分布近似為 (3-174)3.10.3 非高斯噪聲與高斯混合模型較高斯噪聲而言，非高斯噪聲通常更加難以處理。在高斯噪聲模型

39、假定下的許多性質(zhì)都不再適用于非高斯噪聲問(wèn)題。高斯混合模型(GMMs)可以對(duì)非高斯分布密度進(jìn)行近似，而高斯分布項(xiàng)則可以繼續(xù)使用高斯模型中的性質(zhì)。下面提供一個(gè)通用框架來(lái)處理動(dòng)態(tài)模型中的非高斯噪聲。在式(3-147)和式(3-148)中具有加性噪聲的動(dòng)態(tài)模型，過(guò)程噪聲為為簡(jiǎn)化表達(dá)，在下面進(jìn)一步的討論中假設(shè)觀(guān)測(cè)噪聲vn是高斯的。在0時(shí)刻，p(x0)表示已知的先驗(yàn)知識(shí)。x1的預(yù)測(cè)分布可以寫(xiě)為(3-178)當(dāng)獲得量測(cè)值y1后，后驗(yàn)分布可表示為(3-179)其中，C1是一個(gè)比例常數(shù)(3-180)常數(shù)1k是歸一化權(quán)值，并且(3-181)在得到第n時(shí)刻的觀(guān)測(cè)值后，有(3-182)和(3-183)1. 并行動(dòng)態(tài)模

40、型和重采樣通過(guò)觀(guān)察動(dòng)態(tài)模型方程(3-147)和(3-148)后發(fā)現(xiàn)，我們可以使用K個(gè)具有概率密度k的高斯噪聲近似動(dòng)態(tài)模型過(guò)程。因此，非線(xiàn)性非高斯噪聲動(dòng)態(tài)模型等價(jià)于非線(xiàn)性高斯噪聲動(dòng)態(tài)模型的加權(quán)和。所以，由式(3-183)可知，在時(shí)刻n，Kn個(gè)并行非線(xiàn)性高斯噪聲動(dòng)態(tài)模型各自有權(quán)值nj。然而，對(duì)于一個(gè)給定的觀(guān)測(cè)集y1:n，這些并行模型中只有少數(shù)幾個(gè)具有大的權(quán)值。從觀(guān)測(cè)更新式(3-179)可以看到，根據(jù)后驗(yàn)概率密度函數(shù)p(yn|xn)可以對(duì)高斯項(xiàng)的權(quán)值進(jìn)行調(diào)整。因此，在實(shí)際應(yīng)用中將后驗(yàn)分布p(xn|y1:n)近似為G個(gè)加權(quán)混合高斯項(xiàng)之和，并直接丟棄那些權(quán)值很小的高斯項(xiàng)。即有(3-184)顯然，對(duì)于一個(gè)

41、給定的誤差代價(jià)函數(shù)，G取決于模型方程和權(quán)值k。假設(shè)在時(shí)刻n，通過(guò)上述近似給出了p(xn|y1:n)，當(dāng)?shù)玫叫碌挠^(guān)測(cè)值yn+1后，p(xn|y1:n)有GK個(gè)高斯項(xiàng)，即(3-185)和(3-186)為了讓每個(gè)時(shí)刻都保持相同數(shù)目的高斯項(xiàng)，要利用粒子濾波方法中的重采樣。重采樣就是丟掉那些權(quán)值很小的高斯項(xiàng)，并且復(fù)制那些權(quán)值大的高斯項(xiàng)。下面給出了一種簡(jiǎn)單的重采樣方法，當(dāng)然，也可以采用其它的重采樣方法。Step 1：根據(jù)權(quán)值nj的大小將GK個(gè)高斯項(xiàng)按降序排列。僅保留前G個(gè)高斯項(xiàng)，并把權(quán)值和高斯項(xiàng)分別表示為nj和pg(xn|yn)，g=1，G。若最小的權(quán)值Gthreshhold，則執(zhí)行Step 2和Step

42、 3；否則轉(zhuǎn)到Step 3。Step 2：對(duì)于g=1，G，抽取一個(gè)與n1，nG成比例的數(shù)j1，G，令ng，ng=nj，nj并設(shè)置nG=1/G。Step 3：對(duì)于g=1，G，根據(jù)重新初始化權(quán)值。threshhold值的選取取決于實(shí)際問(wèn)題，一般情況下小于0.05。將重采樣過(guò)程應(yīng)用于式(3-186)，保留G個(gè)高斯項(xiàng)。重采樣后的近似式為 (3-187)重采樣過(guò)程保證高斯項(xiàng)的數(shù)目在任意時(shí)刻都等于G，因此，計(jì)算代價(jià)就不會(huì)浪費(fèi)到那些權(quán)值很小的高斯項(xiàng)上。此外，根據(jù)權(quán)值大小復(fù)制不同數(shù)目的高斯項(xiàng)，可保證那些權(quán)值較大的高斯項(xiàng)能參與進(jìn)一步的計(jì)算，并起到更多的作用。但因?yàn)橹夭蓸訒?huì)丟棄高斯項(xiàng)，所以應(yīng)該謹(jǐn)慎使用。綜上所述，

43、非線(xiàn)性非高斯噪聲動(dòng)態(tài)模型問(wèn)題已經(jīng)被轉(zhuǎn)換為一個(gè)非線(xiàn)性高斯噪聲動(dòng)態(tài)模型的加權(quán)和，為了保持高斯項(xiàng)的數(shù)目不變，加入了重采樣過(guò)程。2. 加性非高斯噪聲動(dòng)態(tài)狀態(tài)模型的非線(xiàn)性濾波對(duì)于非線(xiàn)性系統(tǒng)，在時(shí)間更新步獲得和在觀(guān)測(cè)更新步獲得pj(xn|y1:n)以及權(quán)值nj的積分依然是一個(gè)問(wèn)題。換句話(huà)說(shuō)，它需要為每個(gè)并行的非線(xiàn)性高斯噪聲動(dòng)態(tài)模型獲得預(yù)測(cè)和濾波分布及其權(quán)值。該問(wèn)題可以通過(guò)EKF、GPF或GSPF加以解決。一類(lèi)方法可以將預(yù)測(cè)和濾波分布的混合項(xiàng)近似為高斯分布。該近似可以通過(guò)EKF(或者不敏卡爾曼濾波)和GPF來(lái)實(shí)現(xiàn)，產(chǎn)生一個(gè)高斯濾波并行組合。同時(shí)，和pj(xn|y1:n)近似為高斯分布，后驗(yàn)密度p(xn|y1:n1)和p(xn|y1:n)為高斯混合形式。當(dāng)分別使用EKF和GPF時(shí)，由此確定的濾波器被稱(chēng)為GSF和GSPF。該問(wèn)題分解為如下步驟：更新一個(gè)高斯混合模型；使用每一個(gè)