粒子濾波及matlab實現(xiàn)

1、粒子濾波及matlab實現(xiàn)粒子濾波就是指：通過尋找一組在狀態(tài)空間中傳播的隨機樣本來近似的表示概率密度函數(shù)，用樣本均值代替積分運算，進而獲得系統(tǒng)狀態(tài)的最小方差估計的過程，這些樣本被形象的稱為“粒子”，故而叫粒子濾波。粒子濾波通過非參數(shù)化的蒙特卡洛(Monte Carlo)模擬方法來實現(xiàn)遞推貝葉斯濾波，適用于任何能用狀態(tài)空間模型描述的非線性系統(tǒng)，精度可以逼近最優(yōu)估計。粒子濾波器具有簡單、易于實現(xiàn)等特點，它為分析非線性動態(tài)系統(tǒng)提供了一種有效的解決方法，從而引起目標跟蹤、信號處理以及自動控制等領(lǐng)域的廣泛關(guān)注。貝葉斯濾波動態(tài)系統(tǒng)的目標跟蹤問題可以通過下圖所示的狀態(tài)空間模型來描述。在目標跟蹤問題中，動態(tài)系

2、統(tǒng)的狀態(tài)空間模型可描述為其中分別為狀態(tài)轉(zhuǎn)移方程與觀測方程，為系統(tǒng)狀態(tài)，為觀測值，為過程噪聲，為觀測噪聲。為了描述方便，用與分別表示到時刻所有的狀態(tài)與觀測值。在處理目標跟蹤問題時，通常假設(shè)目標的狀態(tài)轉(zhuǎn)移過程服從一階馬爾可夫模型，即當(dāng)前時刻的狀態(tài)只與上一時刻的狀態(tài)有關(guān)。另外一個假設(shè)為觀測值相互獨立，即觀測值只與時刻的狀態(tài)有關(guān)。貝葉斯濾波為非線性系統(tǒng)的狀態(tài)估計問題提供了一種基于概率分布形式的解決方案。貝葉斯濾波將狀態(tài)估計視為一個概率推理過程，即將目標狀態(tài)的估計問題轉(zhuǎn)換為利用貝葉斯公式求解后驗概率密度或濾波概率密度，進而獲得目標狀態(tài)的最優(yōu)估計。貝葉斯濾波包含預(yù)測和更新兩個階段，預(yù)測過程利用系統(tǒng)模型預(yù)測

3、狀態(tài)的先驗概率密度，更新過程則利用最新的測量值對先驗概率密度進行修正，得到后驗概率密度。假設(shè)已知時刻的概率密度函數(shù)為，貝葉斯濾波的具體過程如下：(1) 預(yù)測過程，由得到： 當(dāng)給定時，狀態(tài)與相互獨立，因此上式兩端對積分，可得Chapman-Komolgorov方程 (2) 更新過程，由得到： 獲取時刻的測量后，利用貝葉斯公式對先驗概率密度進行更新，得到后驗概率 假設(shè)只由決定，即因此其中，為歸一化常數(shù)貝葉斯濾波以遞推的形式給出后驗(或濾波)概率密度函數(shù)的最優(yōu)解。目標狀態(tài)的最優(yōu)估計值可由后驗(或濾波)概率密度函數(shù)進行計算。通常根據(jù)極大后驗(MAP)準則或最小均方誤差(MMSE)準則，將具有極大后驗概

4、率密度的狀態(tài)或條件均值作為系統(tǒng)狀態(tài)的估計值，即 貝葉斯濾波需要進行積分運算，除了一些特殊的系統(tǒng)模型（如線性高斯系統(tǒng)，有限狀態(tài)的離散系統(tǒng)）之外，對于一般的非線性、非高斯系統(tǒng)，貝葉斯濾波很難得到后驗概率的封閉解析式。因此，現(xiàn)有的非線性濾波器多采用近似的計算方法解決積分問題，以此來獲取估計的次優(yōu)解。在系統(tǒng)的非線性模型可由在當(dāng)前狀態(tài)展開的線性模型有限近似的前提下，基于一階或二階Taylor級數(shù)展開的擴展Kalman濾波得到廣泛應(yīng)用。在一般情況下，逼近概率密度函數(shù)比逼近非線性函數(shù)容易實現(xiàn)。據(jù)此，Julier與Uhlmann提出一種Unscented Kalman濾波器，通過選定的sigma點來精確估計隨

5、機變量經(jīng)非線性變換后的均值和方差，從而更好的近似狀態(tài)的概率密度函數(shù)，其理論估計精度優(yōu)于擴展Kalman濾波。獲取次優(yōu)解的另外一中方案便是基于蒙特卡洛模擬的粒子濾波器。粒子濾波早在20世紀50年代，Hammersley便采用基于序貫重要性采樣(Sequential importance sampling,SIS)的蒙特卡洛方法解決統(tǒng)計學(xué)問題121。20世紀60年代后期，Handschin與Mayne使用序貫蒙特卡洛方法解決自動控制領(lǐng)域的相關(guān)問題。20世紀70年代，Handschin、Akashi以及Zaritskii等學(xué)者的一系列研究工作使得序貫蒙特卡洛方法得到進一步發(fā)展。限于當(dāng)時的計算能力以及

6、算法本身存在的權(quán)值退化問題，序貫重要性采樣算法沒有受到足夠重視，在隨后較長一段時間內(nèi)進展較為緩慢。直到20世紀80年代末，計算機處理能力的巨大進展使得序貫蒙特卡洛方法重新受到關(guān)注。Tanizaki、Geweke等采用基于重要性采樣的蒙特卡洛方法成功解決了一系列高維積分問題。Smith與Gelfand提出的采樣-重采樣思想為Bayesian推理提供了一種易于實現(xiàn)的計算策略。隨后，Smith與Gordon等人合作，于20世紀90年代初將重采樣(Resampling)步驟引入到粒子濾波中，在一定程度上解決了序貫重要性采樣的權(quán)值退化問題，并由此產(chǎn)生了第一個可實現(xiàn)的SIR(Sampling import

7、ance resampling)粒子濾波算法(Bootstrap濾波)，從而掀起粒子濾波的研究熱潮。美國海軍集成水下監(jiān)控系統(tǒng)中的Nodestar便是粒子濾波應(yīng)用的一個實例。進入21世紀，粒子濾波器成為一個非?；钴S的研究領(lǐng)域，Doucet、Liu、Arulampalam等對粒子濾波的研究作了精彩的總結(jié)，IEEE出版的論文集“Sequential Monte Carlo Methods in Practice”對粒子濾波器進行了詳細介紹。貝葉斯重要性采樣蒙特卡洛模擬是一種利用隨機數(shù)求解物理和數(shù)學(xué)問題的計算方法，又稱為計算機隨機模擬方法。該方法源于第一次世界大戰(zhàn)期間美國研制原子彈的曼哈頓計劃，著名數(shù)

8、學(xué)家馮諾伊曼作為該計劃的主持人之一，用馳名世界的賭城，摩納哥的蒙特卡洛來命名這種方法。蒙特卡洛模擬方法利用所求狀態(tài)空間中大量的樣本點來近似逼近待估計變量的后驗概率分布，如圖2.2所示，從而將積分問題轉(zhuǎn)換為有限樣本點的求和問題。粒子濾波算法的核心思想便是利用一系列隨機樣本的加權(quán)和表示后驗概率密度，通過求和來近似積分操作。假設(shè)可以從后驗概率密度中抽取個獨立同分布的隨機樣本，則有 這里為連續(xù)變量，為單位沖激函數(shù)(狄拉克函數(shù))，即，且。當(dāng)為離散變量時，后驗概率分布可近似逼近為 其中， 。圖2.1 經(jīng)驗概率分布函數(shù)Fig. 2.1 Empirical probability distribution f

9、unction設(shè)為從后驗概率密度函數(shù)中獲取的采樣粒子，則任意函數(shù)的期望估計可以用求和方式逼近，即 蒙特卡洛方法一般可以歸納為以下三個步驟：(1)構(gòu)造概率模型。對于本身具有隨機性質(zhì)的問題，主要工作是正確地描述和模擬這個概率過程。對于確定性問題，比如計算定積分、求解線性方程組、偏微分方程等問題，采用蒙特卡洛方法求解需要事先構(gòu)造一個人為的概率過程，將它的某些參量視為問題的解。(2)從指定概率分布中采樣。產(chǎn)生服從己知概率分布的隨機變量是實現(xiàn)蒙特卡洛方法模擬試驗的關(guān)鍵步驟。(3)建立各種估計量的估計。一般說來，構(gòu)造出概率模型并能從中抽樣后，便可進行現(xiàn)模擬試驗。隨后，就要確定一個隨機變量，將其作為待求解問

10、題的解進行估計。在實際計算中，通常無法直接從后驗概率分布中采樣，如何得到服從后驗概率分布的隨機樣本是蒙特卡洛方法中基本的問題之一。重要性采樣法引入一個已知的、容易采樣的重要性概率密度函數(shù)，從中生成采樣粒子，利用這些隨機樣本的加權(quán)和來逼近后驗濾波概率密度，如圖2.3所示。令表示一支撐點集，其中為是時刻第個粒子的狀態(tài)，其相應(yīng)的權(quán)值為，則后驗濾波概率密度可以表示為 其中， 圖2.2 重要性采樣Fig. 2.2 Importance sampling當(dāng)采樣粒子的數(shù)目很大時，式(2.14)便可近似逼近真實的后驗概率密度函數(shù)。任意函數(shù)的期望估計為 序貫重要性采樣算法在基于重要性采樣的蒙特卡洛模擬方法中，估

11、計后驗濾波概率需要利用所有的觀測數(shù)據(jù)，每次新的觀測數(shù)據(jù)來到都需要重新計算整個狀態(tài)序列的重要性權(quán)值。序貫重要性采樣作為粒子濾波的基礎(chǔ)，它將統(tǒng)計學(xué)中的序貫分析方法應(yīng)用到的蒙特卡洛方法中，從而實現(xiàn)后驗濾波概率密度的遞推估計。假設(shè)重要性概率密度函數(shù)可以分解為 設(shè)系統(tǒng)狀態(tài)是一個馬爾可夫過程，且給定系統(tǒng)狀態(tài)下各次觀測獨立，則有 后驗概率密度函數(shù)的遞歸形式可以表示為 粒子權(quán)值的遞歸形式可以表示為 通常，需要對粒子權(quán)值進行歸一化處理，即 序貫重要性采樣算法從重要性概率密度函數(shù)中生成采樣粒子，并隨著測量值的依次到來遞推求得相應(yīng)的權(quán)值，最終以粒子加權(quán)和的形式來描述后驗濾波概率密度，進而得到狀態(tài)估計。序貫重要性采樣

12、算法的流程可以用如下偽代碼描述：For i=1:N(1)時間更新，根據(jù)重要性參考函數(shù)生成采樣粒子；(2)量測更新，根據(jù)最新觀測值計算粒子權(quán)值；End For粒子權(quán)值歸一化，并計算目標狀態(tài)。為了得到正確的狀態(tài)估計，通常希望粒子權(quán)值的方差盡可能趨近于零。然而，序貫蒙特卡洛模擬方法一般都存在權(quán)值退化問題。在實際計算中，經(jīng)過數(shù)次迭代，只有少數(shù)粒子的權(quán)值較大，其余粒子的權(quán)值可忽略不計。粒子權(quán)值的方差隨著時間增大，狀態(tài)空間中的有效粒子數(shù)較少。隨著無效采樣粒子數(shù)目的增加，使得大量的計算浪費在對估計后驗濾波概率分布幾乎不起作用的粒子更新上，使得估計性能下降。通常采用有效粒子數(shù)來衡量粒子權(quán)值的退化程度，即 有效

13、粒子數(shù)越小，表明權(quán)值退化越嚴重。在實際計算中，有效粒子數(shù)可以近似為 在進行序貫重要性采樣時，若小于事先設(shè)定的某一閾值，則應(yīng)當(dāng)采取一些措施加以控制。克服序貫重要性采樣算法權(quán)值退化現(xiàn)象最直接的方法是增加粒子數(shù)，而這會造成計算量的相應(yīng)增加，影響計算的實時性。因此，一般采用以下兩種途徑：(1)選擇合適的重要性概率密度函數(shù)；(2)在序貫重要性采樣之后，采用重采樣方法。重要密度函數(shù)的選擇重要性概率密度函數(shù)的選擇對粒子濾波的性能有很大影響，在設(shè)計與實現(xiàn)粒子濾波器的過程中十分重要。在工程應(yīng)用中，通常選取狀態(tài)變量的轉(zhuǎn)移概率密度函數(shù)作為重要性概率密度函數(shù)。此時，粒子的權(quán)值為 轉(zhuǎn)移概率的形式簡單且易于實現(xiàn)，在觀測精

14、度不高的場合，將其作為重要性概率密度函數(shù)可以取得較好的濾波效果。然而，采用轉(zhuǎn)移概率密度函數(shù)作為重要性概率密度函數(shù)沒有考慮最新觀測數(shù)據(jù)所提供的信息，從中抽取的樣本與真實后驗分布產(chǎn)生的樣本存在一定的偏差，特別是當(dāng)觀測模型具有較高的精度或預(yù)測先驗與似然函數(shù)之間重疊部分較少時，這種偏差尤為明顯。選擇重要性概率密度函數(shù)的一個標準是使得粒子權(quán)值的方差最小。Doucet等給出的最優(yōu)重要性概率密度函數(shù)為 此時，粒子的權(quán)值為 以作為重要性概率密度函數(shù)需要對其直接采樣。此外，只有在為有限離散狀態(tài)或為高斯函數(shù)時，才存在解析解。在實際情況中，構(gòu)造最優(yōu)重要性概率密度函數(shù)的困難程度與直接從后驗概率分布中抽取樣本的困難程度

15、等同。從最優(yōu)重要性概率密度函數(shù)的表達形式來看，產(chǎn)生下一個預(yù)測粒子依賴于已有的粒子和最新的觀測數(shù)據(jù)，這對于設(shè)計重要性概率密度函數(shù)具有重要的指導(dǎo)作用，即應(yīng)該有效利用最新的觀測信息，在易于采樣實現(xiàn)的基礎(chǔ)上，將更多的粒子移動到似然函數(shù)值較高的區(qū)域，如圖2.4所示。圖2.3 移動粒子至高似然區(qū)域Fig. 2.3 Move the samples in the prior to regions of high likelihood輔助粒子濾波算法利用時刻的信息，將時刻最有前途(預(yù)測似然度大)的粒子擴展到時刻137，從而生成采樣粒子。與SIR濾波器相比，當(dāng)粒子的似然函數(shù)位于先驗分布的尾部或似然函數(shù)形狀比較狹

16、窄時，輔助粒子濾波能夠得到更精確的估計結(jié)果。輔助粒子濾波引入輔助變量來表示時刻的粒子列表，應(yīng)用貝葉斯定理，聯(lián)合概率密度函數(shù)可以描述為 生成的重要性概率密度函數(shù)為 其中為由預(yù)測出的與相關(guān)的特征，可以是采樣值或預(yù)測均值。定義，由于 則有 此時，粒子權(quán)值為 采用局部線性化的方法來逼近是另一種提高粒子采樣效率的有效方法。擴展Kalman粒子濾波與Uncented粒子濾波算法在濾波的每一步迭代過程中，首先利用最新觀測值，采用UKF或者EKF對各個粒子進行更新，得到隨機變量經(jīng)非線性變換后的均值和方差，并將它作為重要性概率密度函數(shù)138。另外，利用似然函數(shù)的梯度信息，采用牛頓迭代139或均值漂移140等方法

17、移動粒子至高似然區(qū)域，也是一種可行的方案，如圖2.5所示。以上這些方法的共同特點是將最新的觀測數(shù)據(jù)融入到系統(tǒng)狀態(tài)的轉(zhuǎn)移過程中，引導(dǎo)粒子到高似然區(qū)域，由此產(chǎn)生的預(yù)測粒子可較好地服從狀態(tài)的后驗概率分布，從而有效地減少描述后驗概率密度函數(shù)所需的粒子數(shù)。圖2.4 結(jié)合均值漂移的粒子濾波算法Fig. 2.4 Particle filter combined with mean shift重采樣方法針對序貫重要性采樣算法存在的權(quán)值退化現(xiàn)象，Gordon等提出了一種名為Bootstrap的粒子濾波算法。該算法在每步迭代過程中，根據(jù)粒子權(quán)值對離散粒子進行重采樣，在一定程度上克服了這個問題。重采樣方法舍棄權(quán)值較

18、小的粒子，代之以權(quán)值較大的粒子。重采樣過程在滿足條件下，將粒子集合更新為。重采樣策略包括固定時間間隔重采樣與根據(jù)粒子權(quán)值進行的動態(tài)重采樣。動態(tài)重采樣通常根據(jù)當(dāng)前的有效粒子數(shù)或最大與最小權(quán)值比來判斷是否需要進行重采樣。常用的重采樣方法包括多項式(Multinomial resampling)重采樣、殘差重采樣(Residual resampling)、分層重采樣(Stratified resampling)與系統(tǒng)重采樣(Systematic resampling)等。殘余重采樣法具有效率高、實現(xiàn)方便的特點。設(shè)，其中為取整操作。殘余重采樣采用新的權(quán)值選擇余下的個粒子，如圖2.6所示。殘余重采樣的主

19、要過程為(1) 計算剩余粒子的權(quán)值累計量 。(2) 生成在個0，1區(qū)間均勻分布的隨機數(shù) ；(3) 對于每個，尋找歸一化權(quán)值累計量大于或等于的最小標號，即。當(dāng)落在區(qū)間時，被復(fù)制一次。這樣，每個粒子經(jīng)重采樣后的個數(shù)為步驟(3)中被選擇的若干粒子數(shù)目與之和。 圖2.5 殘差重采樣Fig. 2.5 Residual Resampling重采樣并沒有從根本上解決權(quán)值退化問題。重采樣后的粒子之間不再是統(tǒng)計獨立關(guān)系，給估計結(jié)果帶來額外的方差。重采樣破壞了序貫重要性采樣算法的并行性，不利于VLSI硬件實現(xiàn)。另外，頻繁的重采樣會降低對測量數(shù)據(jù)中野值的魯棒性。由于重采樣后的粒子集中包含了多個重復(fù)的粒子，重采樣過程

20、可能導(dǎo)致粒子多樣性的喪失，此類問題在噪聲較小的環(huán)境下更加嚴重。因此，一個好的重采樣算法應(yīng)該在增加粒子多樣性和減少權(quán)值較小的粒子數(shù)目之間進行有效折衷。圖2.7為粒子濾波算法的示意圖，該圖描述了粒子濾波算法包含的時間更新、觀測更新和重采樣三個步驟。時刻的先驗概率由個權(quán)值為的粒子近似表示。在時間更新過程中，通過系統(tǒng)狀態(tài)轉(zhuǎn)移方程預(yù)測每個粒子在時刻的狀態(tài)。經(jīng)過觀測值后，更新粒子權(quán)值。重采樣過程舍棄權(quán)值較小的粒子，代之以權(quán)值較大的粒子，粒子的權(quán)值被重新設(shè)置為。圖2.6 SIR算法示意圖Fig. 2.6 SIR algorithm標準的粒子濾波算法流程為：(1) 粒子集初始化，：對于，由先驗生成采樣粒子(2

21、) 對于，循環(huán)執(zhí)行以下步驟： 重要性采樣：對于，從重要性概率密度中生成采樣粒子，計算粒子權(quán)值，并進行歸一化； 重采樣：對粒子集進行重采樣，重采樣后的粒子集為； 輸出：計算時刻的狀態(tài)估計值：。粒子濾波中的權(quán)值退化問題是不可避免的。雖然重采樣方法可以在一定程度上緩解權(quán)值退化現(xiàn)象，但重采樣方法也會帶來一些其它的問題。重采樣需要綜合所有的粒子才能實現(xiàn)，限制了粒子濾波的并行計算。另外，根據(jù)重采樣的原則，粒子權(quán)值較大的粒子必然會更多的被選中復(fù)制，經(jīng)過若干步迭代后，必然導(dǎo)致相同的粒子越來越多，粒子將缺乏多樣性，可能出現(xiàn)粒子退化現(xiàn)象，從而使狀態(tài)估計產(chǎn)生較大偏差。針對粒子退化問題，一個有效的解決方法是增加馬爾可

22、夫蒙特卡洛(Markov chain monte carlo,MCMC)移動步驟141。馬爾可夫鏈蒙特卡洛方法(如Gibbs采樣、Metropolis-Hastings采樣等)利用不可約馬爾可夫過程可逆平穩(wěn)分布的性質(zhì)，將馬爾可夫過程的平穩(wěn)分布視為目標分布，通過構(gòu)造馬爾可夫鏈產(chǎn)生來自目標分布的粒子。粒子退化問題是由于重采樣使得粒子過分集中在某些狀態(tài)上而導(dǎo)致的，對重采樣后的粒子進行馬爾可夫跳轉(zhuǎn)可以提高粒子群的多樣性，同時保證跳轉(zhuǎn)后的粒子同樣能夠準確的描述既定的后驗分布。設(shè)粒子分布服從后驗概率，實施核為的馬爾科夫鏈變換之后，若滿足 便可以得到一組滿足既定后驗概率分布的粒子集合，且這組新的粒子可能移動

23、到狀態(tài)空間中更為有利的位置。采用Metropolis-Hastings算法，從概率密度中生成粒子的具體步驟為：(1) 從0,1之間的均勻分布中生成隨機數(shù)；(2) 從重要性概率密度函數(shù)中生成采樣粒子；(3) 如果，接受，否則，拒絕 另一種解決粒子退化問題的方法是正則化粒子濾波142。傳統(tǒng)的重采樣方法是在離散分布中采樣實現(xiàn)，即 在過程噪聲比較小的情況下，傳統(tǒng)的粒子濾波方法(如SIR方法)的粒子退化現(xiàn)象比較嚴重；正則化粒子濾波首先采用密度估計理論計算后驗密度的連續(xù)分布，然后從連續(xù)分布中采樣來生成采樣粒子，以提高粒子集的多樣性，即 其中，為帶寬，為核函數(shù)。在實際計算中，為了減少計算量，通常采用高斯核函

24、數(shù)。粒子濾波程序clear;clc;% function ParticleEx1% Particle filter example, adapted from Gordon, Salmond, and Smith paper.x = 0.1; % 初始狀態(tài)Q = 1; % 過程噪聲協(xié)方差process noise covarianceR = 1; % 觀測噪聲協(xié)方差measurement noise covariancetf = 50; % 模擬長度simulation lengthN = 100; % 粒子數(shù)目number of particles in the particle filte

25、rxhat = x;P = 2;xhatPart = x;% 粒子濾波初始化Initialize the particle filter.for i = 1 : N xpart(i) = x + sqrt(P) * randn;endxArr = x;yArr = x2 / 20 + sqrt(R) * randn;xhatArr = x;PArr = P;xhatPartArr = xhatPart;close all;for k = 1 : tf % 模擬系統(tǒng)System simulation x = 0.5 * x + 25 * x / (1 + x2) + 8 * cos(1.2*(k

26、-1) + sqrt(Q) * randn;%狀態(tài)方程 y = x2 / 20 + sqrt(R) * randn;%觀測方程 % EKF濾波器Extended Kalman filter F = 0.5 + 25 * (1 - xhat2) / (1 + xhat2)2; P = F * P * F' + Q; H = xhat / 10; K = P * H' * (H * P * H' + R)(-1); xhat = 0.5 * xhat + 25 * xhat / (1 + xhat2) + 8 * cos(1.2*(k-1);%預(yù)測 xhat = xhat

27、+ K * (y - xhat2 / 20);%更新 P = (1 - K * H) * P; % 粒子濾波Particle filter for i = 1 : N xpartminus(i) = 0.5 * xpart(i) + 25 * xpart(i) / (1 + xpart(i)2) + 8 * cos(1.2*(k-1) + sqrt(Q) * randn; ypart = xpartminus(i)2 / 20; vhat = y - ypart;%觀測和預(yù)測的差 q(i) = (1 / sqrt(R) / sqrt(2*pi) * exp(-vhat2 / 2 / R); e

28、nd % 歸一化先驗概率Normalize the likelihood of each a priori estimate. qsum = sum(q); for i = 1 : N q(i) = q(i) / qsum;%歸一化權(quán)重 end % 重采樣Resample. for i = 1 : N u = rand; % uniform random number between 0 and 1 qtempsum = 0; for j = 1 : N qtempsum = qtempsum + q(j); if qtempsum >= u xpart(i) = xpartminus(

29、j); break; end end end % 粒子濾波的估計是粒子的平均The particle filter estimate is the mean of the particles. xhatPart = mean(xpart); % 畫出特定點的pdf Plot the estimated pdf's at a specific time. if k = 20 % Particle filter pdf pdf = zeros(81,1); for m = -40 : 40 for i = 1 : N if (m <= xpart(i) && (xpa

30、rt(i) < m+1) pdf(m+41) = pdf(m+41) + 1; end end end figure; m = -40 : 40; plot(m, pdf / N, 'r'); hold; title('Estimated pdf at k=20'); disp('min, max xpart(i) at k = 20: ', num2str(min(xpart), ', ', num2str(max(xpart); % Kalman filter pdf pdf = (1 / sqrt(P) / sqrt(

31、2*pi) .* exp(-(m - xhat).2 / 2 / P); plot(m, pdf, 'b'); legend('Particle filter', 'Kalman filter'); end % 保存數(shù)據(jù)Save data in arrays for later plotting xArr = xArr x; yArr = yArr y; xhatArr = xhatArr xhat; PArr = PArr P; xhatPartArr = xhatPartArr xhatPart;endt = 0 : tf;%figure;%

32、plot(t, xArr);%ylabel('true state');figure;plot(t, xArr, 'b.', t, xhatArr, 'k-', t, xhatArr-2*sqrt(PArr), 'r:', t, xhatArr+2*sqrt(PArr), 'r:');axis(0 tf -40 40);set(gca,'FontSize',12); set(gcf,'Color','White'); xlabel('time step'

33、;); ylabel('state');legend('True state', 'EKF estimate', '95% confidence region'); figure;plot(t, xArr, 'b.', t, xhatPartArr, 'k-');set(gca,'FontSize',12); set(gcf,'Color','White'); xlabel('time step'); ylabel('state

34、');legend('True state', 'Particle filter estimate'); xhatRMS = sqrt(norm(xArr - xhatArr)2 / tf);xhatPartRMS = sqrt(norm(xArr - xhatPartArr)2 / tf);disp('Kalman filter RMS error = ', num2str(xhatRMS);disp('Particle filter RMS error = ', num2str(xhatPartRMS);粒子濾波的ma

35、tlab實現(xiàn)粒子濾波是以貝葉斯推理和重要性采樣為基本框架的。因此，想要掌握粒子濾波，對于上述兩個基本內(nèi)容必須有一個初步的了解。貝葉斯公式非常perfect，但是在實際問題中，由于變量維數(shù)很高，被積函數(shù)很難積分，常常會給粒子濾波帶來很大的麻煩。為了克服這個問題，它引入了重要性采樣。即先設(shè)計一個重要性密度，根據(jù)重要性密度與實際分布之間的關(guān)系，給采樣得到的粒子分配權(quán)重。再利用時變貝葉斯公式，給出粒子權(quán)重的更新公式及重要性密度的演變形式。在實際問題中，由于直接從重要性密度中采樣非常困難，因此做出了妥協(xié)，重要性密度選為狀態(tài)轉(zhuǎn)移分布，隨之可得權(quán)值更新遵循的規(guī)律與量測方程有關(guān)。粒子濾波算法源于Monte c

36、arlo思想，即以某事件出現(xiàn)的頻率來指代該事件的概率。因此在濾波過程中，需要用到概率如P(x)的地方，一概對變量x采樣，以大量采樣及其相應(yīng)的權(quán)值來近似表示P(x)。因此，采用此思想，在濾波過程中粒子濾波可以處理任意形式的概率，而不像Kalman濾波只能處理線性高斯分布的概率問題。粒子濾波的一大優(yōu)勢也在于此。下來看看對任意如下的狀態(tài)方程：                      

37、;          x(t)=f(x(t-1),u(t),w(t)                                  y(t)=h(x(t),e(t)其

38、中的x(t)為t時刻狀態(tài)，u(t)為控制量，w(t)和e(t)分別為狀態(tài)噪聲和觀測噪聲。前一個方程描述是狀態(tài)轉(zhuǎn)移，后一個是觀測方程。對于這么一個問題粒子濾波怎么來從觀測y(t)，和x(t-1),u(t) 濾出真實狀態(tài)x(t)呢？預(yù)測階段：粒子濾波首先根據(jù)x(t-1) 的概率分布生成大量的采樣，這些采樣就稱之為粒子。那么這些采樣在狀態(tài)空間中的分布實際上就是x(t-1) 的概率分布了。好，接下來依據(jù)狀態(tài)轉(zhuǎn)移方程加上控制量可以對每一粒子得到一個預(yù)測粒子。校正階段：觀測值y到達后，利用觀測方程即條件概率P(y|xi ),對所有的粒子進行評價，直白的說，這個條件概率代表了假設(shè)真實狀態(tài)x(t)取

39、第i個粒子xi時獲得觀測y的概率。令這個條件概率為第i個粒子的權(quán)重。如此這般下來，對所有粒子都進行這么一個評價，那么越有可能獲得觀測y的粒子，當(dāng)然獲得的權(quán)重越高。重采樣算法：去除低權(quán)值的粒子，復(fù)制高權(quán)值的粒子。所得到的當(dāng)然就是我們說需要的真實狀態(tài)x(t)了，而這些重采樣后的粒子，就代表了真實狀態(tài)的概率分布了。下一輪濾波，再將重采樣過后的粒子集輸入到狀態(tài)轉(zhuǎn)移方程中，直接就能夠獲得預(yù)測粒子了。初始狀態(tài)的問題： 由于開始對x(0)一無所知，所有我們可以認為x(0)在全狀態(tài)空間內(nèi)平均分布。于是初始的采樣就平均分布在整個狀態(tài)空間中。然后將所有采樣輸入狀態(tài)轉(zhuǎn)移方程，得到預(yù)測粒子。然后再評價下所有預(yù)測粒子的

40、權(quán)重，當(dāng)然我們在整個狀態(tài)空間中只有部分粒子能夠獲的高權(quán)值。最后進行重采樣，去除低權(quán)值的，將下一輪濾波的考慮重點縮小到了高權(quán)值粒子附近。具體過程：1）初始化階段-提取跟蹤目標特征該階段要人工指定跟蹤目標，程序計算跟蹤目標的特征，比如可以采用目標的顏色特征。具體到Rob Hess的代碼，開始時需要人工用鼠標拖動出一個跟蹤區(qū)域，然后程序自動計算該區(qū)域色調(diào)(Hue)空間的直方圖，即為目標的特征。直方圖可以用一個向量來表示，所以目標特征就是一個N*1的向量V。2）搜索階段-放狗好，我們已經(jīng)掌握了目標的特征，下面放出很多條狗，去搜索目標對象，這里的狗就是粒子particle。狗有很多種放法。比如，a)均勻

41、的放：即在整個圖像平面均勻的撒粒子(uniform distribution)；b)在上一幀得到的目標附近按照高斯分布來放，可以理解成，靠近目標的地方多放，遠離目標的地方少放。Rob Hess的代碼用的是后一種方法。狗放出去后，每條狗怎么搜索目標呢？就是按照初始化階段得到的目標特征(色調(diào)直方圖，向量V)。每條狗計算它所處的位置處圖像的顏色特征，得到一個色調(diào)直方圖，向量Vi，計算該直方圖與目標直方圖的相似性。相似性有多種度量，最簡單的一種是計算sum(abs(Vi-V).每條狗算出相似度后再做一次歸一化，使得所有的狗得到的相似度加起來等于1.3）決策階段我們放出去的一條條聰明的狗向我們發(fā)回報告，

42、“一號狗處圖像與目標的相似度是0.3”,“二號狗處圖像與目標的相似度是0.02”,“三號狗處圖像與目標的相似度是0.0003”,“N號狗處圖像與目標的相似度是0.013”.那么目標究竟最可能在哪里呢？我們做次加權(quán)平均吧。設(shè)N號狗的圖像像素坐標是(Xn,Yn),它報告的相似度是Wn,于是目標最可能的像素坐標X = sum(Xn*Wn),Y = sum(Yn*Wn).4）重采樣階段Resampling既然我們是在做目標跟蹤，一般說來，目標是跑來跑去亂動的。在新的一幀圖像里，目標可能在哪里呢？還是讓我們放狗搜索吧。但現(xiàn)在應(yīng)該怎樣放狗呢？讓我們重溫下狗狗們的報告吧。“一號狗處圖像與目標的相似度是0.3

43、”,“二號狗處圖像與目標的相似度是0.02”,“三號狗處圖像與目標的相似度是0.0003”,“N號狗處圖像與目標的相似度是0.013”.綜合所有狗的報告，一號狗處的相似度最高，三號狗處的相似度最低，于是我們要重新分布警力，正所謂好鋼用在刀刃上，我們在相似度最高的狗那里放更多條狗，在相似度最低的狗那里少放狗，甚至把原來那條狗也撤回來。這就是Sampling Importance Resampling，根據(jù)重要性重采樣(更具重要性重新放狗)。(2)->(3)->(4)->(2)如是反復(fù)循環(huán)，即完成了目標的動態(tài)跟蹤。程序： clc;clear all;close all;x = 0; %初始值R = 1;Q = 1;tf = 10