第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 小結(jié)3.1 引 言粒子濾波是指通過尋找一組在狀態(tài)空間中傳播的隨機樣本，對概率密度函數(shù)p(xk|yk)進行近似，以樣本均值代替積分運算，從而獲得狀態(tài)的最小方差估計的一種算法。粒子濾波算法依據(jù)系統(tǒng)狀態(tài)向量的先驗分布在狀態(tài)空間中產(chǎn)生一組隨機樣本，然后根據(jù)觀測量不斷地調(diào)整粒子的權(quán)值和位置，通過調(diào)整后的粒子信息修正最初

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

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

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

5、正先驗概率，以獲得當前狀態(tài)的后驗概率。式(3-3)和式(3-4)是最優(yōu)貝葉斯估計的一般形式。通過以上步驟遞推得到的后驗概率只是一般概念下的表達式，通常情況下難以得到其解析表達式。只有在滿足特定條件時，才可以得到最優(yōu)貝葉斯解。3.3 貝葉斯重要性采樣在貝葉斯重要性采樣中，后驗概率分布由一組離散的樣本集近似得到。根據(jù)大數(shù)定理，隨著樣本粒子數(shù)N的增加，期望Eg(x0: k)可由近似求出。但是，通常很難從后驗概率密度函數(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)上進行計算的期望。通過從概率函數(shù)q(|y1: k)中采樣，期望可以近似表示為(3-9)式中的重要性權(quán)值表示為(3-10)等式(3-9)計算出來的結(jié)果是有偏的。但是，通過以下兩個假設(shè)可以使逐漸收斂，并且接近于真實值。(1) x(i)0: k是從后驗概率分布中采樣得到的一組粒子，Eg(x0: k)存在并且是有限的；(2) 在后驗概率分布上計算出的wk和wkg2(x0: k)的期望存在而且有限。只有g(shù)(x0: k)的方差和重要性權(quán)值有限才能驗證第二個假設(shè)的成立性。隨著N值的無限增大，后驗概率

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

8、先假設(shè)當前狀態(tài)不依賴于將來的觀測，即只進行濾波而不考慮平滑。需要強調(diào)的是，在某些情況下，一些建議分布需要用到過去的狀態(tài)序列。在本書中，不考慮這種情況。假設(shè)狀態(tài)符合馬爾可夫過程，在給定狀態(tài)下，量測值是條件獨立的，則可得(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)提供了一個遞推計算重要性權(quán)值的方法，重要性權(quán)值的計算因此得以簡化。3.4.2 序貫重要性采樣問題及策略1. 選取好的重要性密度函數(shù)選擇合適的重要性密度函數(shù)是重要性采樣算法中的關(guān)鍵步

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

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

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

12、-27)Charlesstein提出了更一般的接受函數(shù)，即(3-28)式中，(x，y)為任意的對稱函數(shù)，只需要滿足對于任何x和y都有r(x，y)1即可。當選用式(3-28)作為接受函數(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)，即由上述過程產(chǎn)生的馬爾可夫鏈是可逆的，且p(x)是馬爾可夫鏈的平穩(wěn)分布。3.5.4 馬爾可夫鏈蒙特卡羅粒子濾波算法步驟MCMC粒子濾波算法5通過構(gòu)造馬爾可夫鏈產(chǎn)生來自目標分布的樣本(粒子)，使樣本更加多樣化，具有很好的收斂性。在粒子濾波中引入MH(MetropolisHastings)采樣的

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

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

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

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

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

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

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

20、濾波算法步驟首先給出MPF算法的一般步驟：Step 1：初始化。初始化粒子，同時置, i = 1，其中N表示粒子數(shù)。 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中引入的噪聲可以減弱粒子退化現(xiàn)象。為了詳細敘述邊緣粒子濾波算法，將系統(tǒng)模型劃分為三類，分別加以介紹。其中，第一類模型是第二類模型的特例，而第二類模型又是第三類模型的特例。3.8.3 Model 1：對角模型此模型闡述了邊緣粒子濾波是怎樣開始實現(xiàn)的。(3-49)(3-50)(3-51)上述方程中的間隙是特意空出來的，從形式上看類似矩陣對角形式，故而形象地稱其為對角模型。該形式

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

23、(Xnk|Yk)的維數(shù)比p(xlk，Xnk|Yk)的低，這意味著粒子處于低維空間；另外一個原因是利用最優(yōu)算法估計線性狀態(tài)變量。用 I sN(g(xk)表示利用標準粒子濾波器得到的表示式(3-44)的估計，而用 ImN(g(xk)表示利用邊緣粒子濾波器得到的相應(yīng)的估計。在一定的假設(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)遞歸運算初始值：。對于Model 1，p(yk|

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

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

26、關(guān)機制，這里給出邊緣粒子濾波最一般的模型： (3-82) (3-83) (3-84)式中，狀態(tài)噪聲假定為高斯白噪聲 (3-85)量測噪聲也假定為高斯白噪聲 (3-86)因此，xl0服從高斯分布 (3-87)xn0的密度函數(shù)是任意的，但是已知的。由貝葉斯準則，濾波分布p(xk|Yk)分解為(3-88)線性狀態(tài)變量用卡爾曼濾波進行估計。為了估計線性狀態(tài)變量，仍需要執(zhí)行三個步驟：第一步是用yk中的有效信息進行量測更新；第二步是用xnk+1|k中的有效信息進行量測更新；最后一步是時間更新。通過下面的定理來闡述如何估計線性狀態(tài)變量。定理3.1 對于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)證明：為了簡潔，式(3-82)式(3-84)可寫為(3-102)(3-103)(3-104)式中，zak和zbk的定義如下(3-105)(3-106)從數(shù)學結(jié)構(gòu)上看，式(3-103)和式(3-104)可看做量測方程，而zak和zbk可看做量測值。事實上，考慮到式(3-85)中Qlnk0，所以wlk和wnk兩個噪聲過程是相關(guān)的，可以利用GramSchmidt過程實現(xiàn)噪聲去相關(guān)。通過下式(3-107)替換wlk，得(3-108

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

29、步的結(jié)果進行時間更新。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)由于狀態(tài)噪聲服從高斯分布，相當于是由卡爾曼濾波進行時間更新，因此 ，式中(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中狀態(tài)估計的細節(jié)已推導完畢。正如前面指出，此算法和標準粒子濾波的不同點在于預測步驟的區(qū)別。如果去掉MPF算法中Step 4(1)和Step 4(3)，就得到了標準粒子濾波算法。線性狀態(tài)變量的均值和協(xié)方差如下(3-129)(3-130)是歸一化重要性權(quán)值，由MPF算法中的Step 2給出。由MPF算法過程可以看出，該算法通過兩種措施解決了粒子濾波的退化現(xiàn)象： 降低了粒子濾波處理狀態(tài)變量的維數(shù)； 利用卡爾曼濾波改善了粒子濾波的重要性密度函數(shù)，從而提高了粒子濾波的估計精度，減少了粒子濾波的計算復雜度。3.9 擴展卡爾曼粒子濾波算法3.9.1 局

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

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

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

34、比矩陣和；Step4-3：用EKF更新粒子狀態(tài) (3-139)(3-140)(3-141)(3-142)(3-143)Step 4-4：采樣獲得候選粒子(3-144)若，則接受移動，即(3-145)否則拒絕移動，即(3-146)3.10 高斯和粒子濾波算法3.10.1 問題描述假設(shè)動態(tài)模型和量測模型如下xn=f(xn1)+un (3-147)yn=h(xn)+vn (3-148)其中：un和vn是非高斯噪聲；為狀態(tài)向量，是一個具有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)中的動態(tài)模型，假設(shè)分布p(x0)是高斯混合形式。狀態(tài)的濾波和預測結(jié)果的分布也可近似地表達為高斯混合形式。(1) 量測更新。假設(shè)在n時刻有預測分布(3-149)得到n時刻的觀測值yn后，根據(jù)預測分布獲得濾波分布(3-150)其中Cn是歸一化常數(shù)。利用式(3-149)，可以將式(3-150)寫成(3-151)在系統(tǒng)方程(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)近似為一個高斯混合，通過如下推導來實現(xiàn)(3-159)上述方程右邊的每個積分都可近似為一個高斯形式。對于在系統(tǒng)方程(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) 濾波器實現(xiàn)。采用加權(quán)高斯混合模型對p(x0)進行初始化逼近。在量測和時間更新方程中，對EKF方程產(chǎn)生的每個高斯項的均值和協(xié)方差進行更新。通過G個并行的擴展卡爾曼濾波器來實現(xiàn)GSFI，高斯混合項權(quán)值的調(diào)整根據(jù)給定的更新方程計算。需要指出的是，采用高斯混合方式進行近似對于“小”協(xié)方差是有效的，協(xié)方差增大后會導致一些問題。2. 高斯和粒子濾波對于具有加性高斯噪聲的非線性系統(tǒng)，高斯和濾波器使用一組并行的擴展卡爾曼濾波器將后驗分布近似為高斯混合

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

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

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

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

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

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