粒子濾波及matlab實現(xiàn)粒子濾波就是指：通過尋找一組在狀態(tài)空間中傳播的隨機(jī)樣本來近似的表示概率密度函數(shù)，用樣本均值代替積分運(yùn)算，進(jìn)而獲得系統(tǒng)狀態(tài)的最小方差估計的過程，這些樣本被形象的稱為“粒子”，故而叫粒子濾波。粒子濾波通過非參數(shù)化的蒙特卡洛(MonteCarlo)模擬方法來實現(xiàn)遞推貝葉斯濾波，適用于任何能用狀態(tài)空間模型描述的非線性系統(tǒng)，精度可以逼近最優(yōu)估計。粒子濾波器具有簡單、易于實現(xiàn)等特點(diǎn)，它為分析非線性動態(tài)系統(tǒng)提供了一種有效的解決方法，從而引起目標(biāo)跟蹤、信號處理以及自動控制等領(lǐng)域的廣泛關(guān)注。貝葉斯濾波動態(tài)系統(tǒng)的目標(biāo)跟蹤問題可以通過下圖所示的狀態(tài)空間模型來描述。在目標(biāo)跟蹤問題中，動態(tài)系統(tǒng)的狀態(tài)空間模型可描述為其中分別為狀態(tài)轉(zhuǎn)移方程與觀測方程，為系統(tǒng)狀態(tài)，為觀測值，為過程噪聲，為觀測噪聲。為了描述方便，用與分別表示到時刻所有的狀態(tài)與觀測值。在處理目標(biāo)跟蹤問題時，通常假設(shè)目標(biāo)的狀態(tài)轉(zhuǎn)移過程服從一階馬爾可夫模型，即當(dāng)前時刻的狀態(tài)只與上一時刻的狀態(tài)有關(guān)。另外一個假設(shè)為觀測值相互獨(dú)立，即觀測值只與時刻的狀態(tài)有關(guān)。貝葉斯濾波為非線性系統(tǒng)的狀態(tài)估計問題提供了一種基于概率分布形式的解決方案。貝葉斯濾波將狀態(tài)估計視為一個概率推理過程，即將目標(biāo)狀態(tài)的估計問題轉(zhuǎn)換為利用貝葉斯公式求解后驗概率密度或濾波概率密度，進(jìn)而獲得目標(biāo)狀態(tài)的最優(yōu)估計。貝葉斯濾波包含預(yù)測和更新兩個階段，預(yù)測過程利用系統(tǒng)模型預(yù)測狀態(tài)的先驗概率密度，更新過程則利用最新的測量值對先驗概率密度進(jìn)行修正，得到后驗概率密度。假設(shè)已知時刻的概率密度函數(shù)為，貝葉斯濾波的具體過程如下：(1)預(yù)測過程，由得到：當(dāng)給定時，狀態(tài)與相互獨(dú)立，因此上式兩端對積分，可得Chapman-Komolgorov方程(2)更新過程，由得到：獲取時刻的測量后，利用貝葉斯公式對先驗概率密度進(jìn)行更新，得到后驗概率假設(shè)只由決定，即因此其中，為歸一化常數(shù)貝葉斯濾波以遞推的形式給出后驗(或濾波)概率密度函數(shù)的最優(yōu)解。目標(biāo)狀態(tài)的最優(yōu)估計值可由后驗(或濾波)概率密度函數(shù)進(jìn)行計算。通常根據(jù)極大后驗(MAP)準(zhǔn)則或最小均方誤差(MMSE)準(zhǔn)則，將具有極大后驗概率密度的狀態(tài)或條件均值作為系統(tǒng)狀態(tài)的估計值，即貝葉斯濾波需要進(jìn)行積分運(yùn)算，除了一些特殊的系統(tǒng)模型（如線性高斯系統(tǒng)，有限狀態(tài)的離散系統(tǒng)）之外，對于一般的非線性、非高斯系統(tǒng)，貝葉斯濾波很難得到后驗概率的封閉解析式。因此，現(xiàn)有的非線性濾波器多采用近似的計算方法解決積分問題，以此來獲取估計的次優(yōu)解。在系統(tǒng)的非線性模型可由在當(dāng)前狀態(tài)展開的線性模型有限近似的前提下，基于一試驗的關(guān)鍵步驟。(3)建立各種估計量的估計。一般說來，構(gòu)造出概率模型并能從中抽樣后，便可進(jìn)行現(xiàn)模擬試驗。隨后，就要確定一個隨機(jī)變量，將其作為待求解問題的解進(jìn)行估計。在實際計算中，通常無法直接從后驗概率分布中采樣，如何得到服從后驗概率分布的隨機(jī)樣本是蒙特卡洛方法中基本的問題之一。重要性采樣法引入一個已知的、容易采樣的重要性概率密度函數(shù)，從中生成采樣粒子，利用這些隨機(jī)樣本的加權(quán)和來逼近后驗濾波概率密度，如圖2.3所示。令表示一支撐點(diǎn)集，其中為是時刻第個粒子的狀態(tài)，其相應(yīng)的權(quán)值為，則后驗濾波概率密度可以表示為其中，圖2.SEQ圖2.\*ARABIC2重要性采樣Fig.2.SEQFig._2.\*ARABIC2Importancesampling當(dāng)采樣粒子的數(shù)目很大時，式(2.14)便可近似逼近真實的后驗概率密度函數(shù)。任意函數(shù)的期望估計為序貫重要性采樣算法在基于重要性采樣的蒙特卡洛模擬方法中，估計后驗濾波概率需要利用所有的觀測數(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)下各次觀測獨(dú)立，則有后驗概率密度函數(shù)的遞歸形式可以表示為粒子權(quán)值的遞歸形式可以表示為通常，需要對粒子權(quán)值進(jìn)行歸一化處理，即序貫重要性采樣算法從重要性概率密度函數(shù)中生成采樣粒子，并隨著測量值的依次到來遞推求得相應(yīng)的權(quán)值，最終以粒子加權(quán)和的形式來描述后驗濾波概率密度，進(jìn)而得到狀態(tài)估計。序貫重要性采樣算法的流程可以用如下偽代碼描述：Fori=1:N(1)時間更新，根據(jù)重要性參考函數(shù)生成采樣粒子；(2)量測更新，根據(jù)最新觀測值計算粒子權(quán)值；EndFor粒子權(quán)值歸一化，并計算目標(biāo)狀態(tài)。為了得到正確的狀態(tài)估計，通常希望粒子權(quán)值的方差盡可能趨近于零。然而，序貫蒙特卡洛模擬方法一般都存在權(quán)值退化問題。在實際計算中，經(jīng)過數(shù)次迭代，只有少數(shù)粒子的權(quán)值較大，其余粒子的權(quán)值可忽略不計。粒子權(quán)值的方差隨著時間增大，狀態(tài)空間中的有效粒子數(shù)較少。隨著無效采樣粒子數(shù)目的增加，使得大量的計算浪費(fèi)在對估計后驗濾波概率分布幾乎不起作用的粒子更新上，使得估計性能下降。通常采用有效粒子數(shù)來衡量粒子權(quán)值的退化程度，即有效粒子數(shù)越小，表明權(quán)值退化越嚴(yán)重。在實際計算中，有效粒子數(shù)可以近似為在進(jìn)行序貫重要性采樣時，若小于事先設(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)，在觀測精度不高的場合，將其作為重要性概率密度函數(shù)可以取得較好的濾波效果。然而，采用轉(zhuǎn)移概率密度函數(shù)作為重要性概率密度函數(shù)沒有考慮最新觀測數(shù)據(jù)所提供的信息，從中抽取的樣本與真實后驗分布產(chǎn)生的樣本存在一定的偏差，特別是當(dāng)觀測模型具有較高的精度或預(yù)測先驗與似然函數(shù)之間重疊部分較少時，這種偏差尤為明顯。選擇重要性概率密度函數(shù)的一個標(biāo)準(zhǔn)是使得粒子權(quán)值的方差最小。Doucet等給出的最優(yōu)重要性概率密度函數(shù)為此時，粒子的權(quán)值為以作為重要性概率密度函數(shù)需要對其直接采樣。此外，只有在為有限離散狀態(tài)或為高斯函數(shù)時，才存在解析解。在實際情況中，構(gòu)造最優(yōu)重要性概率密度函數(shù)的困難程度與直接從后驗概率分布中抽取樣本的困難程度等同。從最優(yōu)重要性概率密度函數(shù)的表達(dá)形式來看，產(chǎn)生下一個預(yù)測粒子依賴于已有的粒子和最新的觀測數(shù)據(jù)，這對于設(shè)計重要性概率密度函數(shù)具有重要的指導(dǎo)作用，即應(yīng)該有效利用最新的觀測信息，在易于采樣實現(xiàn)的基礎(chǔ)上，將更多的粒子移動到似然函數(shù)值較高的區(qū)域，如圖2.4所示。圖2.SEQ圖2.\*ARABIC3移動粒子至高似然區(qū)域Fig.2.SEQFig._2.\*ARABIC3Movethesamplesinthepriortoregionsofhighlikelihood輔助粒子濾波算法利用時刻的信息，將時刻最有前途(預(yù)測似然度大)的粒子擴(kuò)展到時刻ADDINEN.CITE<EndNote><Cite><RecNum>142</RecNum><record><rec-number>142</rec-number><ref-typename="JournalArticle">17</ref-type><contributors></contributors><titles><title>PittM,ShephardN.Filteringviasimulation:auxillaryparticlefilter.JournalofAmericanStatisticalAssociation,1999,94:590-599</title></titles><dates></dates><urls></urls></record></Cite></EndNote>[137]，從而生成采樣粒子。與SIR濾波器相比，當(dāng)粒子的似然函數(shù)位于先驗分布的尾部或似然函數(shù)形狀比較狹窄時，輔助粒子濾波能夠得到更精確的估計結(jié)果。輔助粒子濾波引入輔助變量來表示時刻的粒子列表，應(yīng)用貝葉斯定理，聯(lián)合概率密度函數(shù)可以描述為生成的重要性概率密度函數(shù)為其中為由預(yù)測出的與相關(guān)的特征，可以是采樣值或預(yù)測均值。定義，由于則有此時，粒子權(quán)值為采用局部線性化的方法來逼近是另一種提高粒子采樣效率的有效方法。擴(kuò)展Kalman粒子濾波與Uncented粒子濾波算法在濾波的每一步迭代過程中，首先利用最新觀測值，采用UKF或者EKF對各個粒子進(jìn)行更新，得到隨機(jī)變量經(jīng)非線性變換后的均值和方差，并將它作為重要性概率密度函數(shù)ADDINEN.CITE<EndNote><Cite><RecNum>144</RecNum><record><rec-number>144</rec-number><ref-typename="JournalArticle">17</ref-type><contributors></contributors><titles><title>vanderMerweR,A.DoucetA,deFreitasJFG,WanE.TheUnscentedParticleFilter.In:LeenTK,DietterichTG,TrespV,eds.AdvancesinNeuralInformationProcessingSystems.MA,USA:MITPress,2000</title></titles><dates></dates><urls></urls></record></Cite></EndNote>[138]。另外，利用似然函數(shù)的梯度信息，采用牛頓迭代ADDINEN.CITE<EndNote><Cite><RecNum>146</RecNum><record><rec-number>146</rec-number><ref-typename="JournalArticle">17</ref-type><contributors></contributors><titles><title>ChenZ,HuberK.Robustparticlefilterswithapplicationsintrackingandcommunications.TechnologyReport,AdaptiveSystemsLab,McMasterUniversity,2003</title></titles><dates></dates><urls></urls></record></Cite></EndNote>[139]或均值漂移ADDINEN.CITE<EndNote><Cite><RecNum>147</RecNum><record><rec-number>147</rec-number><ref-typename="JournalArticle">17</ref-type><contributors></contributors><titles><title>ShanC,WeiY,TanT.Realtimehandtrackingbycombiningparticlefilteringandmeanshift.IEEEInternationalConferenceonAutomaticFaceandGestureRecognition,Seoul,Korea,2004:669-674</title></titles><dates></dates><urls></urls></record></Cite></EndNote>[140]等方法移動粒子至高似然區(qū)域，也是一種可行的方案，如圖2.5所示。以上這些方法的共同特點(diǎn)是將最新的觀測數(shù)據(jù)融入到系統(tǒng)狀態(tài)的轉(zhuǎn)移過程中，引導(dǎo)粒子到高似然區(qū)域，由此產(chǎn)生的預(yù)測粒子可較好地服從狀態(tài)的后驗概率分布，從而有效地減少描述后驗概率密度函數(shù)所需的粒子數(shù)。圖2.SEQ圖2.\*ARABIC4結(jié)合均值漂移的粒子濾波算法Fig.2.SEQFig._2.\*ARABIC4Particlefiltercombinedwithmeanshift重采樣方法針對序貫重要性采樣算法存在的權(quán)值退化現(xiàn)象，Gordon等提出了一種名為Bootstrap的粒子濾波算法。該算法在每步迭代過程中，根據(jù)粒子權(quán)值對離散粒子進(jìn)行重采樣，在一定程度上克服了這個問題。重采樣方法舍棄權(quán)值較小的粒子，代之以權(quán)值較大的粒子。重采樣過程在滿足條件下，將粒子集合更新為。重采樣策略包括固定時間間隔重采樣與根據(jù)粒子權(quán)值進(jìn)行的動態(tài)重采樣。動態(tài)重采樣通常根據(jù)當(dāng)前的有效粒子數(shù)或最大與最小權(quán)值比來判斷是否需要進(jìn)行重采樣。常用的重采樣方法包括多項式(Multinomialresampling)重采樣、殘差重采樣(Residualresampling)、分層重采樣(Stratifiedresampling)與系統(tǒng)重采樣(Systematicresampling)等。殘余重采樣法具有效率高、實現(xiàn)方便的特點(diǎn)。設(shè)，其中為取整操作。殘余重采樣采用新的權(quán)值選擇余下的個粒子，如圖2.6所示。殘余重采樣的主要過程為(1)計算剩余粒子的權(quán)值累計量。(2)生成在個[0，1]區(qū)間均勻分布的隨機(jī)數(shù)；(3)對于每個，尋找歸一化權(quán)值累計量大于或等于的最小標(biāo)號，即。當(dāng)落在區(qū)間時，被復(fù)制一次。這樣，每個粒子經(jīng)重采樣后的個數(shù)為步驟(3)中被選擇的若干粒子數(shù)目與之和。圖2.SEQ圖2.\*ARABIC5殘差重采樣Fig.2.SEQFig._2.\*ARABIC5ResidualResampling重采樣并沒有從根本上解決權(quán)值退化問題。重采樣后的粒子之間不再是統(tǒng)計獨(dú)立關(guān)系，給估計結(jié)果帶來額外的方差。重采樣破壞了序貫重要性采樣算法的并行性，不利于VLSI硬件實現(xiàn)。另外，頻繁的重采樣會降低對測量數(shù)據(jù)中野值的魯棒性。由于重采樣后的粒子集中包含了多個重復(fù)的粒子，重采樣過程可能導(dǎo)致粒子多樣性的喪失，此類問題在噪聲較小的環(huán)境下更加嚴(yán)重。因此，一個好的重采樣算法應(yīng)該在增加粒子多樣性和減少權(quán)值較小的粒子數(shù)目之間進(jìn)行有效折衷。圖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.SEQ圖2.\*ARABIC6SIR算法示意圖Fig.2.SEQFig._2.\*ARABIC6SIRalgorithm標(biāo)準(zhǔn)的粒子濾波算法流程為：(1)粒子集初始化，：對于，由先驗生成采樣粒子(2)對于，循環(huán)執(zhí)行以下步驟：①重要性采樣：對于，從重要性概率密度中生成采樣粒子，計算粒子權(quán)值，并進(jìn)行歸一化；②重采樣：對粒子集進(jìn)行重采樣，重采樣后的粒子集為；③輸出：計算時刻的狀態(tài)估計值：。粒子濾波中的權(quán)值退化問題是不可避免的。雖然重采樣方法可以在一定程度上緩解權(quán)值退化現(xiàn)象，但重采樣方法也會帶來一些其它的問題。重采樣需要綜合所有的粒子才能實現(xiàn)，限制了粒子濾波的并行計算。另外，根據(jù)重采樣的原則，粒子權(quán)值較大的粒子必然會更多的被選中復(fù)制，經(jīng)過若干步迭代后，必然導(dǎo)致相同的粒子越來越多，粒子將缺乏多樣性，可能出現(xiàn)粒子退化現(xiàn)象，從而使?fàn)顟B(tài)估計產(chǎn)生較大偏差。針對粒子退化問題，一個有效的解決方法是增加馬爾可夫蒙特卡洛(Markovchainmontecarlo,MCMC)移動步驟ADDINEN.CITE<EndNote><Cite><RecNum>148</RecNum><record><rec-number>148</rec-number><ref-typename="JournalArticle">17</ref-type><contributors></contributors><titles><title>GilksW,BerzuiniC.Followingamovingtarget-BayesianinferencefordynamicBayesianmodels.JournaloftheRoyalStatisticalSociety,SeriesB,2001,63(1):127-146</title></titles><dates></dates><urls></urls></record></Cite></EndNote>[141]。馬爾可夫鏈蒙特卡洛方法(如Gibbs采樣、Metropolis-Hastings采樣等)利用不可約馬爾可夫過程可逆平穩(wěn)分布的性質(zhì)，將馬爾可夫過程的平穩(wěn)分布視為目標(biāo)分布，通過構(gòu)造馬爾可夫鏈產(chǎn)生來自目標(biāo)分布的粒子。粒子退化問題是由于重采樣使得粒子過分集中在某些狀態(tài)上而導(dǎo)致的，對重采樣后的粒子進(jìn)行馬爾可夫跳轉(zhuǎn)可以提高粒子群的多樣性，同時保證跳轉(zhuǎn)后的粒子同樣能夠準(zhǔn)確的描述既定的后驗分布。設(shè)粒子分布服從后驗概率，實施核為的馬爾科夫鏈變換之后，若滿足便可以得到一組滿足既定后驗概率分布的粒子集合，且這組新的粒子可能移動到狀態(tài)空間中更為有利的位置。采用Metropolis-Hastings算法，從概率密度中生成粒子的具體步驟為：(1)從[0,1]之間的均勻分布中生成隨機(jī)數(shù)；(2)從重要性概率密度函數(shù)中生成采樣粒子；(3)如果，接受，否則，拒絕另一種解決粒子退化問題的方法是正則化粒子濾波ADDINEN.CITE<EndNote><Cite><RecNum>149</RecNum><record><rec-number>149</rec-number><ref-typename="JournalArticle">17</ref-type><contributors></contributors><titles><title>MussoC,OudjaneN,LeGlandF.Improvingregularisedparticlefilters.In:DoucetA,deFreitasJFG,GordonNJ,Eds.SequentialMonteCarloMethodsinPractice.NewYork:Springer-Verlag,2001</title></titles><dates></dates><urls></urls></record></Cite></EndNote>[142]。傳統(tǒng)的重采樣方法是在離散分布中采樣實現(xiàn)，即在過程噪聲比較小的情況下，傳統(tǒng)的粒子濾波方法(如SIR方法)的粒子退化現(xiàn)象比較嚴(yán)重；正則化粒子濾波首先采用密度估計理論計算后驗密度的連續(xù)分布，然后從連續(xù)分布中采樣來生成采樣粒子，以提高粒子集的多樣性，即其中，為帶寬，為核函數(shù)。在實際計算中，為了減少計算量，通常采用高斯核函數(shù)。粒子濾波程序clear;clc;%functionParticleEx1%Particlefilterexample,adaptedfromGordon,Salmond,andSmithpaper.x=0.1;%初始狀態(tài)Q=1;%過程噪聲協(xié)方差processnoisecovarianceR=1;%觀測噪聲協(xié)方差measurementnoisecovariancetf=50;%模擬長度simulationlengthN=100;%粒子數(shù)目numberofparticlesintheparticlefilterxhat=x;P=2;xhatPart=x;%粒子濾波初始化Initializetheparticlefilter.fori=1:Nxpart(i)=x+sqrt(P)*randn;endxArr=[x];yArr=[x^2/20+sqrt(R)*randn];xhatArr=[x];PArr=[P];xhatPartArr=[xhatPart];closeall;fork=1:tf%模擬系統(tǒng)Systemsimulationx=0.5*x+25*x/(1+x^2)+8*cos(1.2*(k-1))+sqrt(Q)*randn;%狀態(tài)方程y=x^2/20+sqrt(R)*randn;%觀測方程%EKF濾波器ExtendedKalmanfilterF=0.5+25*(1-xhat^2)/(1+xhat^2)^2;P=F*P*F'+Q;H=xhat/10;K=P*H'*(H*P*H'+R)^(-1);xhat=0.5*xhat+25*xhat/(1+xhat^2)+8*cos(1.2*(k-1));%預(yù)測xhat=xhat+K*(y-xhat^2/20);%更新P=(1-K*H)*P;%粒子濾波Particlefilterfori=1:Nxpartminus(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(-vhat^2/2/R);end%歸一化先驗概率Normalizethelikelihoodofeachaprioriestimate.qsum=sum(q);fori=1:Nq(i)=q(i)/qsum;%歸一化權(quán)重end%重采樣Resample.fori=1:Nu=rand;%uniformrandomnumberbetween0and1qtempsum=0;forj=1:Nqtempsum=qtempsum+q(j);ifqtempsum>=uxpart(i)=xpartminus(j);break;endendend%粒子濾波的估計是粒子的平均Theparticlefilterestimateisthemeanoftheparticles.xhatPart=mean(xpart);%畫出特定點(diǎn)的pdfPlottheestimatedpdf'sataspecifictime.ifk==20%Particlefilterpdfpdf=zeros(81,1);form=-40:40fori=1:Nif(m<=xpart(i))&&(xpart(i)<m+1)pdf(m+41)=pdf(m+41)+1;endendendfigure;m=-40:40;plot(m,pdf/N,'r');hold;title('Estimatedpdfatk=20');disp(['min,maxxpart(i)atk=20:',num2str(min(xpart)),',',num2str(max(xpart))]);%Kalmanfilterpdfpdf=(1/sqrt(P)/sqrt(2*pi)).*exp(-(m-xhat).^2/2/P);plot(m,pdf,'b');legend('Particlefilter','Kalmanfilter');end%保存數(shù)據(jù)SavedatainarraysforlaterplottingxArr=[xArrx];yArr=[yArry];xhatArr=[xhatArrxhat];PArr=[PArrP];xhatPartArr=[xhatPartArrxhatPart];endt=0:tf;%figure;%plot(t,xArr);%ylabel('truestate');figure;plot(t,xArr,'b.',t,xhatArr,'k-',t,xhatArr-2*sqrt(PArr),'r:',t,xhatArr+2*sqrt(PArr),'r:');axis([0tf-4040]);set(gca,'FontSize',12);set(gcf,'Color','White');xlabel('timestep');ylabel('state');legend('Truestate','EKFestimate','95%confidenceregion');figure;plot(t,xArr,'b.',t,xhatPartArr,'k-');set(gca,'FontSize',12);set(gcf,'Color','White');xlabel('timestep');ylabel('state');legend('Truestate','Particlefilterestimate');xhatRMS=sqrt((norm(xArr-xhatArr))^2/tf);xhatPartRMS=sqrt((norm(xArr-xhatPartArr))^2/tf);disp(['KalmanfilterRMSerror=',num2str(xhatRMS)]);disp(['ParticlefilterRMSerror=',num2str(xhatPartRMS)]);粒子濾波的matlab實現(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)。粒子濾波\o"算法與數(shù)據(jù)結(jié)構(gòu)知識庫"算法源于Montecarlo思想，即以某事件出現(xiàn)的頻率來指代該事件的概率。因此在濾波過程中，需要用到概率如P(x)的地方，一概對變量x采樣，以大量采樣及其相應(yīng)的權(quán)值來近似表示P(x)。因此，采用此思想，在濾波過程中粒子濾波可以處理任意形式的概率，而不像Kalman濾波只能處理線性高斯分布的概率問題。粒子濾波的一大優(yōu)勢也在于此。下來看看對任意如下的狀態(tài)方程：

x(t)=f(x(t-1),u(t),w(t))

y(t)=h(x(t),e(t))其中的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到達(dá)后，利用觀測方程即條件概率P(y|xi

),對所有的粒子進(jìn)行評價，直白的說，這個條件概率代表了假設(shè)真實狀態(tài)x(t)取第i個粒子xi時獲得觀測y的概率。令這個條件概率為第i個粒子的權(quán)重。如此這般下來，對所有粒子都進(jì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)一無所知，所有我們可以認(rèn)為x(0)在全狀態(tài)空間內(nèi)平均分布。于是初始的采樣就平均分布在整個狀態(tài)空間中。然后將所有采樣輸入狀態(tài)轉(zhuǎn)移方程，得到預(yù)測粒子。然后再評價下所有預(yù)測粒子的權(quán)重，當(dāng)然我們在整個狀態(tài)空間中只有部分粒子能夠獲的高權(quán)值。最后進(jìn)行重采樣，去除低權(quán)值的，將下一輪濾波的考慮重點(diǎn)縮小到了高權(quán)值粒子附近。具體過程：1）初始化階段-提取跟蹤目標(biāo)特征該階段要人工指定跟蹤目標(biāo)，程序計算跟蹤目標(biāo)的特征，比如可以采用目標(biāo)的顏色特征。具體到RobHess的代碼，開始時需要人工用鼠標(biāo)拖動出一個跟蹤區(qū)域，然后程序自動計算該區(qū)域色調(diào)(Hue)空間的直方圖，即為目標(biāo)的特征。直方圖可以用一個向量來表示，所以目標(biāo)特征就是一個N*1的向量V。2）搜索階段-放狗好，我們已經(jīng)掌握了目標(biāo)的特征，下面放出很多條狗，去搜索目標(biāo)對象，這里的狗就是粒子particle。狗有很多種放法。比如，a)均勻的放：即在整個圖像平面均勻的撒粒子(uniformdistribution)；b)在上一幀得到的目標(biāo)附近按照高斯分布來放，可以理解成，靠近目標(biāo)的地方多放，遠(yuǎn)離目標(biāo)的地方少放。RobHess的代碼用的是后一種方法。狗放出去后，每條狗怎么搜索目標(biāo)呢？就是按照初始化階段得到的目標(biāo)特征(色調(diào)直方圖，向量V)。每條狗計算它所處的位置處圖像的顏色特征，得到一個色調(diào)直方圖，向量Vi，計算該直方圖與目標(biāo)直方圖的相似性。相似性有多種度量，最簡單的一種是計算sum(abs(Vi-V)).每條狗算出相似度后再做一次歸一化，使得所有的狗得到的相似度加起來等于1.3）決策階段我們放出去的一條條聰明的狗向我們發(fā)回報告，“一號狗處圖像與目標(biāo)的相似度是0.3”,“二號狗處圖像與目標(biāo)的相似度是0.02”,“三號狗處圖像與目標(biāo)的相似度是0.0003”,“N號狗處圖像與目標(biāo)的相似度是0.013”...那么目標(biāo)究竟最可能在哪里呢？我們做次加權(quán)平均吧。設(shè)N號狗的圖像像素坐標(biāo)是(Xn,Yn),它報告的相似度是Wn,于是目標(biāo)最可能的像素坐標(biāo)X=sum(Xn*Wn),Y=sum(Yn*Wn).4）重采樣階段Resampling既然我們是在做目標(biāo)跟蹤，一般說來，目標(biāo)是跑來跑去亂動的。在新的一幀圖像里，目標(biāo)可能在哪里呢？還是讓我們放狗搜索吧。但現(xiàn)在應(yīng)該怎樣放狗呢？讓我們重溫下狗狗們的報告吧。“一號狗處圖像與目標(biāo)的相似度是0.3”,“二號狗處圖像與目標(biāo)的相似度是0.02”,“三號狗處圖像與目標(biāo)的相似度是0.0003”,“N號狗處圖像與目標(biāo)的相似度是0.013”...綜合所有狗的報告，一號狗處的相似度最高，三號狗處的相似度最低，于是我們要重新分布警力，正所謂好鋼用在刀刃上，我們在相似度最高的狗那里放更多條狗，在相似度最低的狗那里少放狗，甚至把原來那條狗也撤回來。這就是SamplingImportanceResampling，根據(jù)重要性重采樣(更具重要性重新放狗)。(2)->(3)->(4)->(2)如是反復(fù)循環(huán)，即完成了目標(biāo)的動態(tài)跟蹤。程序：clc;cl