在論文中看到粒子濾波的知識點，在網上找到的幾篇講的很易的文章：

http://blog.csdn.net/heyijia0327/article/details/40899819

http://blog.csdn.net/heyijia0327/article/details/40929097

http://blog.csdn.net/heyijia0327/article/details/41122125

http://blog.csdn.net/heyijia0327/article/details/41142679

前言：

      博主在自主學習粒子濾波的過程中，看了很多文獻或部落格，不知道是看文獻時粗心大意還是悟性太低，看著那麼多公式，總是無法把握住粒子濾波的思路，也無法將理論和實踐對應起來。比如：理論推導過程中那麼多概率公式，概率怎麼和系統的狀態變數對應上了？狀態粒子

是怎麼一步步取樣出來的，為什麼程式裡面都是直接用狀態方程來計算？粒子的權重是怎麼來的？經過一段時間的理解，總算理清了它的脈絡。同時也覺得，只有對理論的推導心中有數了，才能知道什麼樣的地方可以用這個演算法，以及這個演算法有什麼不足。因此，本文將結合實際程式給出粒子濾波的詳細推導，在推導過程中加入博主自己的理解，如有不妥，請指出，謝謝。

     文章架構：

     由最基礎的貝葉斯估計開始介紹，再引出蒙特卡羅取樣，重要性取樣，SIS粒子濾波，重取樣，基本粒子濾波Generic Particle Filter，SIR粒子濾波，這些概念的引進，都是為了解決上一個概念中出現的問題而環環相扣的。最後給出幾個在matlab和python中的應用，例程包括影象跟蹤，濾波，機器人定位。

      再往下看之前，也可以看看《卡爾曼濾波:從推導到應用》，好對這種通過狀態方程來濾波的思路有所瞭解。

一、貝葉斯濾波

      假設有一個系統，我們知道它的狀態方程，和測量方程如下：

        如：       (1)

               如：                                                                  (2)

其中x為系統狀態，y為測量到的資料，f,h是狀態轉移函式和測量函式，v,n為過程噪聲和測量噪聲，噪聲都是獨立同分布的。上面對應的那個例子將會出現在程式中。

      從貝葉斯理論的觀點來看，狀態估計問題（目標跟蹤、訊號濾波）就是根據之前一系列的已有資料（後驗知識）遞推的計算出當前狀態的可信度。這個可信度就是概率公式，它需要通過預測和更新兩個步奏來遞推的計算。

      預測過程是利用系統模型(狀態方程1)預測狀態的先驗概率密度，也就是通過已有的先驗知識對未來的狀態進行猜測，即p( x(k)|x(k-1) )。更新過程則利用最新的測量值對先驗概率密度進行修正，得到後驗概率密度，也就是對之前的猜測進行修正。

     在處理這些問題時，一般都先假設系統的狀態轉移服從一階馬爾科夫模型，即當前時刻的狀態x(k)只與上一個時刻的狀態x(k-1)有關。這是很自然的一種假設，就像小時候玩飛行棋，下一時刻的飛機跳到的位置只由當前時刻的位置和骰子決定。同時，假設k時刻測量到的資料y(k)只與當前的狀態x(k)有關，如上面的狀態方程2。

     為了進行遞推，不妨假設已知k-1時刻的概率密度函式

     預測：由上一時刻的概率密度得到，這個公式的含義是既然有了前面1：k-1時刻的測量資料，那就可以預測一下狀態x(k)出現的概率。

     計算推導如下：

     

                         

                         

等式的第一行到第二行純粹是貝葉斯公式的應用。第二行得到第三行是由於一階馬爾科夫過程的假設，狀態x(k)只由x(k-1)決定。

樓主看到這裡的時候想到兩個問題：

      第一：既然 ，x(k)都只由x(k-1)決定了，即，在這裡弄一個是什麼意思？

      這兩個概率公式含義是不一樣的，前一個是純粹根據模型進行預測，x(k)實實在在的由x(k-1)決定，後一個是既然測到的資料和狀態是有關係的，現在已經有了很多測量資料 y 了，那麼我可以根據已有的經驗對你進行預測，只是猜測x(k)，而不能決定x(k)。

     第二：上面公式的最後一行是假設已知的，但是怎麼得到呢？

     其實它由系統狀態方程決定，它的概率分佈形狀和系統的過程噪聲形狀一模一樣。如何理解呢?觀察狀態方程(1)式我們知道，x(k) = Constant( x(k-1) ) + v(k-1)  也就是x(k)由一個通過x(k-1)計算出的常數疊加一個噪聲得到。看下面的圖：

如果沒有噪聲，x(k)完全由x(k-1)計算得到，也就沒由概率分佈這個概念了，由於出現了噪聲，所以x(k)不好確定，他的分佈就如同圖中的陰影部分，實際上形狀和噪聲是一樣的，只是進行了一些平移。理解了這一點，對粒子濾波程式中，狀態x(k)的取樣的計算很有幫助，要取樣x(k)，直接取樣一個過程噪聲，再疊加上 f(x(k-1)) 這個常數就行了。

     更新：由得到後驗概率。這個後驗概率才是真正有用的東西，上一步還只是預測，這裡又多了k時刻的測量，對上面的預測再進行修正，就是濾波了。這裡的後驗概率也將是代入到下次的預測，形成遞推。

     推導：

     

                              

其中歸一化常數：

      

等式第一行到第二行是因為測量方程知道, y(k)只與x(k)有關，也稱之為似然函式，由量測方程決定。也和上面的推理一樣，, x(k)部分是常數，也是隻和量測噪聲n(k)的概率分佈有關，注意這個也將為SIR粒子濾波里權重的取樣提供程式設計依據。

 

       貝葉斯濾波到這裡就告一段落了。但是，請注意上面的推導過程中需要用到積分，這對於一般的非線性，非高斯系統，很難得到後驗概率的解析解。為了解決這個問題，就得引進蒙特卡洛取樣。

reference：

1.M. Sanjeev Arulampalam 《A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking》

2.ZHE CHEN 《Bayesian Filtering: From Kalman Filters to Particle Filters, and Beyond》

3.Sebastian THRUN 《Probabilistic Robotics》

3.百度文庫 《粒子濾波理論》

 

二、蒙特卡洛取樣

假設我們能從一個目標概率分佈p(x)中取樣到一系列的樣本（粒子），（至於怎麼生成服從p(x)分佈的樣本，這個問題先放一放），那麼就能利用這些樣本去估計這個分佈的某些函式的期望值。譬如：

         

           

上面的式子其實都是計算期望的問題，只是被積分的函式不同。

蒙特卡洛取樣的思想就是用平均值來代替積分，求期望：

        

這可以從大數定理的角度去理解它。我們用這種思想去指定不同的f(x)以便達到估計不同東西的目的。比如：要估計一批同齡人的體重，不分男女，在大樣本中男的有100個，女的有20個，為了少做事，我們按比例抽取10個男的，2個女的，測算這12個人的體重求平均就完事了。注意這裡的按比例抽取，就可以看成從概率分佈p(x)中進行抽樣。

          下面再看一個稍微學術一點的例子：

          假設有一粒質地均勻的骰子。規定在一次遊戲中，連續四次拋擲骰子，至少出現一次6個點朝上就算贏。現在來估計贏的概率。我們用來表示在第n次遊戲中，第k次投擲的結果，k=1...4。對於分佈均勻的骰子，每次投擲服從均勻分佈,即：

               

這裡的區間是取整數，1,2,3,4,5,6，代表6個面。由於每次投擲都是獨立同分布的，所以這裡的目標分佈p(x)也是一個均勻分佈。一次遊戲就是空間中的一個隨機點。

       為了估計取勝的概率，在第n次遊戲中定義一個指示函式:

                

其中，指示函式I{x }是指，若x的條件滿足，則結果為1，不滿足結果為0。回到這個問題，這裡函式 f()的意義就是單次遊戲中，若四次投擲中只要有一個6朝上，f()的結果就會是1。由此，就可以估計在這樣的遊戲中取勝的期望，也就是取勝的概率：

              

當抽樣次數N足夠大的時候，上式就逼近真實取勝概率了，看上面這種估計概率的方法，是通過蒙特卡洛方法的角度去求期望達到估計概率的目的。是不是就跟我們拋硬幣的例子一樣，拋的次數足夠多就可以用來估計正面朝上或反面朝上的概率了。

       當然可能有人會問，這樣估計的誤差有多大，對於這個問題，有興趣的請去檢視我最下面列出的參考文獻2。（囉嗦一句：管的太多太寬，很容易讓我們忽略主要問題。博主就是在看文獻過程中，這個是啥那個是啥，都去查資料，到頭來粒子濾波是幹嘛完全不知道了，又重新看資料。個人感覺有問題還是先放一放，主要思路理順了再關注細節。）

        接下來，回到我們的主線上，在濾波中蒙特卡洛又是怎麼用的呢？

        由上面我們知道，它可以用來估計概率，而在上一節中，貝葉斯後驗概率的計算裡要用到積分，為了解決這個積分難的問題，可以用蒙特卡洛取樣來代替計算後驗概率。

        假設可以從後驗概率中取樣到N個樣本，那麼後驗概率的計算可表示為：

        

其中，在這個蒙特卡洛方法中，我們定義,是狄拉克函式(dirac delta function)，跟上面的指示函式意思差不多。

       看到這裡，既然用蒙特卡洛方法能夠用來直接估計後驗概率，現在估計出了後驗概率，那到底怎麼用來做影象跟蹤或者濾波呢？要做影象跟蹤或者濾波，其實就是想知道當前狀態的期望值：

       

                          

                                        (1)

也就是用這些取樣的粒子的狀態值直接平均就得到了期望值，也就是濾波後的值，這裡的 f(x) 就是每個粒子的狀態函式。這就是粒子濾波了，只要從後驗概率中取樣很多粒子，用它們的狀態求平均就得到了濾波結果。

      思路看似簡單，但是要命的是，後驗概率不知道啊，怎麼從後驗概率分佈中取樣！所以這樣直接去應用是行不通的，這時候得引入重要性取樣這個方法來解決這個問題。

 

三、重要性取樣

        無法從目標分佈中取樣，就從一個已知的可以取樣的分佈裡去取樣如 q(x|y)，這樣上面的求期望問題就變成了：      

 

        

                             

                                                    (2)式

其中

                          

由於：

        

所以(2)式可以進一步寫成：

       

                           

                          

                                         (3)式

上面的期望計算都可以通過蒙特卡洛方法來解決它，也就是說，通過取樣N個樣本,用樣本的平均來求它們的期望，所以上面的（3）式可以近似為：

        

                                                  (4)式

其中：

          

這就是歸一化以後的權重，而之前在(2)式中的那個權重是沒有歸一化的。

         注意上面的(4)式，它不再是（1）式中所有的粒子狀態直接相加求平均了，而是一種加權和的形式。不同的粒子都有它們相應的權重，如果粒子權重大，說明信任該粒子比較多。

       到這裡已經解決了不能從後驗概率直接取樣的問題，但是上面這種每個粒子的權重都直接計算的方法，效率低，因為每增加一個取樣，p( x(k) |y(1:k))都得重新計算，並且還不好計算這個式子。所以求權重時能否避開計算p(x(k)|y(1:k))？而最佳的形式是能夠以遞推的方式去計算權重，這就是所謂的序貫重要性取樣（SIS），粒子濾波的原型。

      下面開始權重w遞推形式的推導：

      假設重要性概率密度函式，這裡x的下標是0:k，也就是說粒子濾波是估計過去所有時刻的狀態的後驗。假設它可以分解為：

                    

 

     後驗概率密度函式的遞迴形式可以表示為：

                

                             

其中，為了表示方便，將 y(1:k) 用 Y(k) 來表示，注意 Y 與 y 的區別。同時，上面這個式子和上一節貝葉斯濾波中後驗概率的推導是一樣的，只是之前的x(k)變成了這裡的x(0:k)，就是這個不同，導致貝葉斯估計裡需要積分，而這裡後驗概率的分解形式卻不用積分。

 

       粒子權值的遞迴形式可以表示為:

                   ( 5)式

注意，這種權重遞推形式的推導是在前面（2）式的形式下進行推導的，也就是沒有歸一化。而在進行狀態估計的公式為這個公式中的的權重是歸一化以後的，所以在實際應用中，遞推計算出w(k)後,要進行歸一化，才能夠代入(4)式中去計算期望。同時，上面(5)式中的分子是不是很熟悉，在上一節貝葉斯濾波中我們都已經做了介紹，p( y|x ),p( x(k)|x(k-1) )的形狀實際上和狀態方程中噪聲的概率分佈形狀是一樣的，只是均值不同了。因此這個遞推的(5)式和前面的非遞推形式相比，公式裡的概率都是已知的，權重的計算可以說沒有程式設計方面的難度了。權重也有了以後，只要進行稍微的總結就可以得到SIS Filter。

 

四、Sequential Importance Sampling (SIS) Filter 

      在實際應用中我們可以假設重要性分佈q()滿足：

           

這個假設說明重要性分佈只和前一時刻的狀態x(k-1)以及測量y(k)有關了，那麼(5)式就可以轉化為：

          

在做了這麼多假設和為了解決一個個問題以後，終於有了一個像樣的粒子濾波演算法了，他就是序貫重要性取樣濾波。

下面用虛擬碼的形式給出這個演算法：

----------------------pseudo code-----------------------------------

 

      

       For i=1:N

             (1)取樣：；

             (2)根據遞推計算各個粒子的權重；

       End For

      粒子權值歸一化。粒子有了，粒子的權重有了，就可以由(4)式,對每個粒子的狀態進行加權去估計目標的狀態了。

-----------------------end -----------------------------------------------

 

      這個演算法就是粒子濾波的前身了。只是在實際應用中，又發現了很多問題，如粒子權重退化的問題，因此就有了重取樣( resample )，就有了基本的粒子濾波演算法。還有就是重要性概率密度q()的選擇問題，等等。都留到下一章 去解決。

     在這一章中，我們是用的重要性取樣這種方法去解決的後驗概率無法取樣的問題。實際上，關於如何從後驗概率取樣，也就是如何生成特定概率密度的樣本，有很多經典的方法（如拒絕取樣，Markov Chain Monte Carlo，Metropolis-Hastings演算法，Gibbs取樣），這裡面可以單獨作為一個課題去學習了，有興趣的可以去看看《統計之都 的一篇博文》，強烈推薦，參考文獻裡的前幾個也都不錯。    

reference：

1. Gabriel A. Terejanu  《Tutorial on Monte Carlo Techniques》

2. Taylan Cemgil 《A Tutorial Introduction to Monte Carlo methods, Markov Chain Monte Carlo and Particle Filtering》

3. M. Sanjeev Arulampalam 《A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking》

4. ZHE CHEN 《Bayesian Filtering: From Kalman Filters to Particle Filters, and Beyond》

5.百度文庫《粒子濾波理論》

6. Haykin 《Neural Networks and learning Machines 》Chapter 14 

7.  統計之都  <LDA-math-MCMC 和 Gibbs Sampling>

五、重取樣

       在應用SIS 濾波的過程中，存在一個退化的問題。就是經過幾次迭代以後，很多粒子的權重都變得很小，可以忽略了，只有少數粒子的權重比較大。並且粒子權值的方差隨著時間增大，狀態空間中的有效粒子數較少。隨著無效取樣粒子數目的增加，使得大量的計算浪費在對估計後驗濾波概率分佈幾乎不起作用的粒子上，使得估計效能下降，如圖所示。

 

       通常採用有效粒子數來衡量粒子權值的退化程度，即

              

 

這個式子的含義是，有效粒子數越小，即權重的方差越大，也就是說權重大的和權重小的之間差距大，表明權值退化越嚴重。在實際計算中，有效粒子數可以近似為：

                

         在進行序貫重要性取樣時，若上式小於事先設定的某一閾值，則應當採取一些措施加以控制。克服序貫重要性取樣演算法權值退化現象最直接的方法是增加粒子數，而這會造成計算量的相應增加，影響計算的實時性。因此，一般採用以下兩種途徑：(1)選擇合適的重要性概率密度函式；(2)在序貫重要性取樣之後，採用重取樣方法。

      對於第一種方法：選取重要性概率密度函式的一個標準就是使得粒子權值的方差最小。關於這部分內容，還是推薦百度文庫的那篇文章《粒子濾波理論》，他在這裡也引申出來了幾種不同的粒子濾波方法。

      這裡著重講第二種方法，重取樣。

      重取樣的思路是：既然那些權重小的不起作用了，那就不要了。要保持粒子數目不變，得用一些新的粒子來取代它們。找新粒子最簡單的方法就是將權重大的粒子多複製幾個出來，至於複製幾個？那就在權重大的粒子裡面讓它們根據自己權重所佔的比例去分配，也就是老大分身分得最多，老二分得次多，以此類推。下面以數學的形式來進行說明。

      前面已經說明了求某種期望問題變成了這種加權和的形式：

                   (1)

通過重取樣以後，希望表示成：

               (2)

，注意對比(1)和(2)。是第k時刻的粒子。是k時刻重取樣以後的粒子。其中n(i)是指粒子在產生新的粒子集時被複制的次數。（2）式中第一個等號說明重取樣以後，所有的粒子權重一樣，都是1/N，只是有的粒子多出現了n(i)次。

       思路有了，就看具體的操作方法了。在《On resampling algorithms for particle fi lters》 這篇paper裡講了四種重取樣的方法。這四種方法大同小異。如果你接觸過遺傳演算法的話，理解起來就很容易，就是遺傳演算法中那種輪盤賭的思想。

      在這裡用個簡單的例子來說明：

      假設有3個粒子，在第k時刻的時候，他們的權重分別是0.1, 0.1 ,0.8, 然後計算他們的概率累計和(matlab 中為cumsum() )得到： [0.1, 0.2, 1]。接著，我們用服從[0,1]之間的均勻分佈隨機取樣3個值，假設為0.15 , 0.38 和 0.54。也就是說，第二個粒子複製一次，第三個粒子複製兩次。

在MATLAB中一句命令就可以方便的實現這個過程：

       [~, j] = histc(rand(N,1), [0 cumsum(w')]); 關於histc的用法可以點選histc用法。

對於上面的過程，還可以對著下面的圖加深理解：

 

將重取樣的方法放入之前的SIS演算法中，便形成了基本粒子濾波演算法。

      重取樣的思路很簡單，但是當你仔細分析權重的計算公式時：

                  

會有疑問，權重大的就多複製幾次，這一定是正確的嗎？權重大，如果是分子大，即後驗概率大，那說明確實應該在後驗概率大的地方多放幾個粒子。但權重大也有可能是分母小造成的，這時候的分子也可能小，也就是實際的後驗概率也可能小，這時候的大權重可能就沒那麼優秀了。何況，這種簡單的重取樣會使得粒子的多樣性丟失，到最後可能都變成了只剩一種粒子的分身。在遺傳演算法中好歹還引入了變異來解決多樣性的問題。當然，粒子濾波里也有專門的方法：正則粒子濾波，有興趣的可以查閱相關資料。

      至此，整個粒子濾波的流程已經清晰明朗了，在實際應用中還有一些不確定的就是重要性概率密度的選擇。在下一章中，首先引出 SIR 粒子濾波，接著用SIR濾波來進行實踐應用。

 

reference:

1. N. J. Gordon 《Beyond the Kalman Filter:Particle filters for tracking applications》

2. 百度文庫《粒子濾波理論》

3. Jeroen D. Hol《On resampling algorithms for particle fi lters》

4. Particle filters: How to do resampling?       

5. Gabriel A. Terejanu  《Tutorial on Monte Carlo Techniques》

 

六、Sampling Importance Resampling Filter (SIR)

       SIR濾波器很容易由前面的基本粒子濾波推匯出來，只要對粒子的重要性概率密度函式做出特定的選擇即可。在SIR中，選取：

       

p( x(k)|x(k-1) )這是先驗概率，在第一章貝葉斯濾波預測部分已經說過怎麼用狀態方程來得到它。將這個式子代入到第二章SIS推匯出的權重公式中：

       

得到：

                 (1)式

由之前的重取樣我們知道，實際上每次重取樣以後，有。

所以(1)式可以進一步簡化成：

           

這裡又出來一個概率取樣 ，實際怎麼得到這個概率，程式裡面又怎麼去取樣呢？

      先搞清這個概率的含義，它表示在狀態x出現的條件下，測量y出現的概率。在機器人定位裡面就是，在機器人處於位姿x時，此時感測器資料y出現的概率。更簡單的例子是，我要找到一個年齡是14歲的男孩（狀態x），身高為170（測量y）的概率。要知道y出現的概率，需要知道此時y的分佈。這裡以第一篇文章的狀態方程為例，由系統狀態方程可知，測量是在真實值附近添加了一個高斯噪聲。因此，y的分佈就是以真實測量值為均值，以噪聲方差為方差的一個高斯分佈。因此，權重的取樣過程就是：當粒子處於x狀態時，能夠得到該粒子的測量y。要知道這個測量y出現的概率，就只要把它放到以真實值為均值，噪聲方差為方差的高斯分佈裡去計算就行了：

         

       到這裡，就可以看成SIR只和系統狀態方程有關了，不用自己另外去設計概率密度函式，所以在很多程式中都是用的這種方法。

下面以虛擬碼的形式給出SIR濾波器：

----------------------SIR Particle Filter pseudo code-----------------------------------

• FOR i = 1:N

                (1)取樣粒子：

              (2)計算粒子的權重：

• END FOR
• 計算粒子權重和，t=sum(w)
• 對每個粒子，用上面的權重和進行歸一化,w = w/t
• 粒子有了，每個粒子權重有了，進行狀態估計     
• 重取樣

-----------------------end -----------------------------------------------

在上面演算法中，每進行一次，都必須重取樣一次，這是由於權重的計算方式決定的。

      分析上面演算法中的取樣，發現它並沒有加入測量y(k)。只是憑先驗知識p( x(k)|x(k-1) )進行的取樣，而不是用的修正了的後驗概率。所以這種演算法存在效率不高和對奇異點(outliers)敏感的問題。但不管怎樣，SIR確實簡單易用。

七、粒子濾波的應用

      在這裡主要以第一章的狀態方程作為例子進行演示。

         

          

在這個存在過程噪聲和量測噪聲的系統中，估計狀態x(k)。

 

%% SIR粒子濾波的應用，演算法流程參見部落格http://blog.csdn.net/heyijia0327/article/details/40899819    
clear all    
close all    
clc    
%% initialize the variables    
x = 0.1; % initial actual state    
x_N = 1; % 系統過程噪聲的協方差  (由於是一維的，這裡就是方差)    
x_R = 1; % 測量的協方差    
T = 75;  % 共進行75次    
N = 100; % 粒子數，越大效果越好，計算量也越大    
    
%initilize our initial, prior particle distribution as a gaussian around    
%the true initial value    
    
V = 2; %初始分佈的方差    
x_P = []; % 粒子    
% 用一個高斯分佈隨機的產生初始的粒子    
for i = 1:N    
    x_P(i) = x + sqrt(V) * randn;    
end    
    
z_out = [x^2 / 20 + sqrt(x_R) * randn];  %實際測量值    
x_out = [x];  %the actual output vector for measurement values.    
x_est = [x]; % time by time output of the particle filters estimate    
x_est_out = [x_est]; % the vector of particle filter estimates.    
    
for t = 1:T    
    x = 0.5*x + 25*x/(1 + x^2) + 8*cos(1.2*(t-1)) +  sqrt(x_N)*randn;    
    z = x^2/20 + sqrt(x_R)*randn;    
    for i = 1:N    
        %從先驗p(x(k)|x(k-1))中取樣    
        x_P_update(i) = 0.5*x_P(i) + 25*x_P(i)/(1 + x_P(i)^2) + 8*cos(1.2*(t-1)) + sqrt(x_N)*randn;    
        %計算取樣粒子的值，為後面根據似然去計算權重做鋪墊    
        z_update(i) = x_P_update(i)^2/20;    
        %對每個粒子計算其權重，這裡假設量測噪聲是高斯分佈。所以 w = p(y|x)對應下面的計算公式    
        P_w(i) = (1/sqrt(2*pi*x_R)) * exp(-(z - z_update(i))^2/(2*x_R));    
    end    
    % 歸一化.    
    P_w = P_w./sum(P_w);    
      
    %% Resampling這裡沒有用部落格裡之前說的histc函式，不過目的和效果是一樣的    
    for i = 1 : N    
        x_P(i) = x_P_update(find(rand <= cumsum(P_w),1));   % 粒子權重大的將多得到後代    
    end                                                     % find( ,1) 返回第一個 符合前面條件的數的 下標    
        
    %狀態估計，重取樣以後，每個粒子的權重都變成了1/N    
    x_est = mean(x_P);    
        
    % Save data in arrays for later plotting    
    x_out = [x_out x];    
    z_out = [z_out z];    
    x_est_out = [x_est_out x_est];    
        
end    
    
t = 0:T;    
figure(1);    
clf    
plot(t, x_out, '.-b', t, x_est_out, '-.r','linewidth',3);    
set(gca,'FontSize',12); set(gcf,'Color','White');    
xlabel('time step'); ylabel('flight position');    
legend('True flight position', 'Particle filter estimate');    

濾波後的結果如下：

 

    

       這是粒子濾波的一個應用，還有一個目標跟蹤(matlab)，機器人定位(python)的例子，我一併放入壓縮檔案，供大家下載，下載請點選。(下載需要1個積分，下載完評論資源你就可以賺回那1積分，相當於沒損失)。請原諒博主的這一點點自私。兩個程式得效果如下：

      

      

      粒子濾波從推導到應用這個系列到這裡就結束了。結合前面幾章的問題起來看，基本的粒子濾波里可改進的地方很多，正由於此才誕生了很多優化了的演算法，而這篇部落格只理順了基本演算法的思路，希望有幫到大家。

     另外，個人感覺粒子濾波和遺傳演算法真是像極了。同時，如果你覺得這種用很多粒子來計算的方式效率低，在工程應用中不好接受，推薦看看無味卡爾曼濾波（UKF）,他是有選擇的產生粒子，而不是盲目的隨機產生。

 

reference：

1.M. Sanjeev Arulampalam 《A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking》

其他參考：

https://en.wikipedia.org/wiki/Particle_filter

http://blog.csdn.net/jinshengtao/article/details/30970733

http://blog.csdn.net/hujingshuang/article/details/45535423