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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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; % 觀測(cè)噪聲協(xié)方差measurement noise covariancetf = 50; % 模擬長(zhǎng)度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;%觀測(cè)方程 % 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ù)測(cè) 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;%觀測(cè)和預(yù)測(cè)的差 q(i) = (1 / sqrt(R) / sqrt(2*pi) * exp(-vhat2 / 2 / R); e

28、nd % 歸一化先驗(yàn)概率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 % 粒子濾波的估計(jì)是粒子的平均The particle filter estimate is the mean of the particles. xhatPart = mean(xpart); % 畫出特定點(diǎn)的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實(shí)現(xiàn)粒子濾波是以貝葉斯推理和重要性采樣為基本框架的。因此，想要掌握粒子濾波，對(duì)于上述兩個(gè)基本內(nèi)容必須有一個(gè)初步的了解。貝葉斯公式非常perfect，但是在實(shí)際問題中，由于變量維數(shù)很高，被積函數(shù)很難積分，常常會(huì)給粒子濾波帶來很大的麻煩。為了克服這個(gè)問題，它引入了重要性采樣。即先設(shè)計(jì)一個(gè)重要性密度，根據(jù)重要性密度與實(shí)際分布之間的關(guān)系，給采樣得到的粒子分配權(quán)重。再利用時(shí)變貝葉斯公式，給出粒子權(quán)重的更新公式及重要性密度的演變形式。在實(shí)際問題中，由于直接從重要性密度中采樣非常困難，因此做出了妥協(xié)，重要性密度選為狀態(tài)轉(zhuǎn)移分布，隨之可得權(quán)值更新遵循的規(guī)律與量測(cè)方程有關(guān)。粒子濾波算法源于Monte c

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

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

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

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

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

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

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

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