怎樣從實際場景上理解粒子濾波（Particle Filter）？

12-28

最近在做移動機器人定位（Mobile Robot Localisation），粒子濾波是其中重要的概念和方法，之前看了好多文章，一直沒理解，有沒有大神解答一下。。。。

幾年前我也很困惑，後來看到了這篇通俗的介紹（轉載自基於粒子濾波的物體跟蹤），希望對你有幫助：

一直都覺得粒子濾波是個挺牛的東西，每次試圖看文獻都被複雜的數學符號搞得看不下去。一個偶然的機會發現了Rob Hess(http://web.engr.oregonstate.edu/~hess/)實現的這個粒子濾波。從代碼入手，一下子就明白了粒子濾波的原理。根據維基百科上對粒子濾波的介紹(Particle filter)，粒子濾波其實有很多變種，Rob Hess實現的這種應該是最基本的一種，Sampling Importance Resampling (SIR)，根據重要性重採樣。下面是我對粒子濾波實現物體跟蹤的演算法原理的粗淺理解：

1）初始化階段-提取跟蹤目標特徵
該階段要人工指定跟蹤目標，程序計算跟蹤目標的特徵，比如可以採用目標的顏色特徵。具體到Rob Hess的代碼，開始時需要人工用滑鼠拖動出一個跟蹤區域，然後程序自動計算該區域色調(Hue)空間的直方圖，即為目標的特徵。直方圖可以用一個向量來表示，所以目標特徵就是一個N*1的向量V。

2）搜索階段-放狗
好，我們已經掌握了目標的特徵，下面放出很多條狗，去搜索目標對象，這裡的狗就是粒子particle。狗有很多种放法。比如，a)均勻的放：即在整個圖像平面均勻的撒粒子(uniform distribution)；b)在上一幀得到的目標附近按照高斯分布來放，可以理解成，靠近目標的地方多放，遠離目標的地方少放。Rob Hess的代碼用的是後一種方法。狗放出去後，每條狗怎麼搜索目標呢？就是按照初始化階段得到的目標特徵(色調直方圖，向量V)。每條狗計算它所處的位置處圖像的顏色特徵，得到一個色調直方圖，向量Vi，計算該直方圖與目標直方圖的相似性。相似性有多種度量，最簡單的一種是計算sum(abs(Vi-V)).每條狗算出相似度後再做一次歸一化，使得所有的狗得到的相似度加起來等於1.

3）決策階段
我們放出去的一條條聰明的狗向我們發回報告，「一號狗處圖像與目標的相似度是0.3」,「二號狗處圖像與目標的相似度是0.02」,「三號狗處圖像與目標的相似度是0.0003」,「N號狗處圖像與目標的相似度是0.013」...那麼目標究竟最可能在哪裡呢？我們做次加權平均吧。設N號狗的圖像像素坐標是(Xn,Yn),它報告的相似度是Wn,於是目標最可能的像素坐標X = sum(Xn*Wn),Y = sum(Yn*Wn).

4）重採樣階段Resampling
既然我們是在做目標跟蹤，一般說來，目標是跑來跑去亂動的。在新的一幀圖像里，目標可能在哪裡呢？還是讓我們放狗搜索吧。但現在應該怎樣放狗呢？讓我們重溫下狗狗們的報告吧。「一號狗處圖像與目標的相似度是0.3」,「二號狗處圖像與目標的相似度是0.02」,「三號狗處圖像與目標的相似度是0.0003」,「N號狗處圖像與目標的相似度是0.013」...綜合所有狗的報告，一號狗處的相似度最高，三號狗處的相似度最低，於是我們要重新分布警力，正所謂好鋼用在刀刃上，我們在相似度最高的狗那裡放更多條狗，在相似度最低的狗那裡少放狗，甚至把原來那條狗也撤回來。這就是Sampling Importance Resampling，根據重要性重採樣(更具重要性重新放狗)。
(2)-&>(3)-&>(4)-&>(2)如是反覆循環，即完成了目標的動態跟蹤。
根據我的粗淺理解，粒子濾波的核心思想是隨機採樣+重要性重採樣。既然我不知道目標在哪裡，那我就隨機的撒粒子吧。撒完粒子後，根據特徵相似度計算每個粒子的重要性，然後在重要的地方多撒粒子，不重要的地方少撒粒子。所以說粒子濾波較之蒙特卡洛濾波，計算量較小。這個思想和RANSAC演算法真是不謀而合。RANSAC的思想也是(比如用在最簡單的直線擬合上)，既然我不知道直線方程是什麼，那我就隨機的取兩個點先算個直線出來，然後再看有多少點符合我的這條直線。哪條直線能獲得最多的點的支持，哪條直線就是目標直線。想法非常簡單，但效果很好。

Probabilistic Robotics有沒有譯本？

================

本來計劃國慶結束就好好的寫一個答案。拖了一個多月，抱歉。不是很喜歡粒子濾波這個詞，所以以下都稱Particle filter。

首先要說的就是Particle Filter是一個系統化成熟的演算法。這種領域應該看教科書而不是文章。文章應該是在對一個領域已經比較了解之後，希望了解更細化的分支或者跟隨一下大牛的腳步。計算機這個大領域中，十年前的文章，除了經典，基本沒有看的必要。所以，仔細去讀Thrun的Probabilistic Robotics，這是我的經驗之談。如果有人要翻譯這本書，我也是願意貢獻一些力量的。

理論的部分自己去看吧。我這個沒學過概率的人都可以靠做localization混個碩士學位，相信你們都是沒有問題的。需要了解什麼是Monte Carlo，其實就是概率學的一種數值演算法。說爛點就是小學學的枚舉法。然後貝葉斯公式要能看懂，雖然我也就只能看懂最簡單的貝葉斯。。。變個形我就不認識了。基於Particle filter的定位就是靠的蒙特卡洛和貝葉斯，用足夠多的Particle來形成貝葉斯裡面的pdf。

================

寒冷的冬日，我來到了一個陌生的城市。漫無目的的走著，街上看不到什麼人。旁邊的一位老大媽在兜售五塊錢一張的地圖。生活艱辛啊！我慈善式的買了一張地圖，繼續漫無目的的走。天空開始飄雪，光線也慢慢的暗了下來，不遠處的樓房都模糊了，偌大的世界彷彿只有我一個人在這裡！我在那兒？（只有差不多這個時候，我的處境才差不多和一個要開始做Global localization的機器人類似）

我現在能做的，就是打開這張寶貴的地圖（這就是定位時要用的Global map）。現在我完全不知道在哪兒，地圖上任何的點都有可能。不，我在馬路上，應該是任何馬路上的點都有可能（這就是根據條件來做初始化，初始化得當可以縮小很多的範圍。這一點即便是有些很有經驗的機器人研究人員也會忘記）。在我的意念里，我變成了美猴王，揪下一撮猴毛，讓這些猴兵猴將猴子猴孫們均勻的分布在這地圖上的每個街道。現在我要先轉一圈看看四周的情況（這是我的第一次的動作，其實這時候用Motion model來更新一下所有位置意義不大，但是為了演算法每個周期內的一致，我們也都更新一遍）。我發現我能看到一座百米開外的城門。這座城市有十八個城門，那我現在就非常有可能站在其中一個城門的附近。可是我現在在城裡還是城外呢？總之，令人欣喜的是，我可以把我遍布全圖的猴子們，號召到這18個城門附近，因為不知道里里外外，所以大概分成了三十六個陣營（這是Particle filter最美的部分，resampling。其實像是開民主大會，每個人各抒己見，最後大家再投票表決。凡是講的有道理的，都會有人擁護）。隨著我在實際中向城門靠近，它們也在地圖中向城門靠近（雖然內外陣營的移動方向是相反的，但是這些都是嚴格按照我的相對運動來更新的）。離城門不遠的時候，我發現了一條護城河！由於這個驚人的發現，城外的猴子們說服了城內的猴子，大家都紛紛加入了城外的陣營！現在還有18個陣營的猴子！

我決定從城門走進去。這是一個比較大的城門，走過城門，發現已經有很多不堅定的猴子加入了東南西北四個正門的陣營中。我也暗自高興，我已經知道我在城的裡面了。沒走幾步，這座城市的中心建築，東南西北大街交匯處的鐘樓，就依稀能看見輪廓了。看見了鐘樓，原來在其他陣營的猴子都來到了東南西北大街。而這個城市的南大街最短，猴子們也在南大街聚集的最多（主要是因為基於Motion的信任比較高，沒走幾步說明很短，所以比較短的南大街可能性高）繼續向前，我走到了鐘樓下，這時鐘樓的東南西北圍滿了面朝鐘樓的猴子們。向左看，鐘樓飯店，向右看，開元商城。登時所有的猴子都齊刷刷的聚到了南大街，坐南朝北。

=================

其實人的定位和整個Particle filter localization還是差很多的。人主要是Semantic Map和place recognition。所以開始限定了一下環境。。。大概看看吧，也寫了個把小時啊！

看過很多教程和視頻，講得最好的莫過於Probablistic Robotics的作者Shrun在Udacity 公開課平台中的Programming A Robot Car(Artificial Intelligence for Robotics)的Particle Filter這節課(當然你需要把這節課之前的幾節也看完才能深入理解)，它的好處在於不僅用簡單的語言講解了基本原理，更給出了實現演算法的代碼教程，我覺得看一大坨公式能增加的理解程度都不如看一小段代碼，甚至自己動手編上一段，這樣就能完全理解了。當然公開課上講得還是比較淺的，要想更全面的了解的話還是推薦仔細研讀Probablistic Robotics。
不過這個公開課已經足夠你自己動手實現一個pf出來，你可以用它做一些基本的應用了，我上次課程設計就是通過這個再加上我實驗室採集的機器人laser的數據做了一個簡單的pf定位，還是相當有意思的。圖中顯示的是一條軌跡，那些散點是在不同時間段猜測的機器人的位置，顏色越暖的點權值越大，綠點則是對應時間點的ground truth，所以可以看出這些猜測都挺準的～

填坑。

粒子濾波的本質是monte-carlo模擬，即它是通過對目標區域中物體可能出現的位置概率的一種估測。

我們來考慮機器人中的SLAM（Simultaneous Location And Mapping），這個比較直觀。

1. 假設，我們現在有一輛小車，小車前後左右裝了4個距離探測器。我們把這個小車放到一件房間里，這個房間擺放了很多障礙物，有桌子，有椅子。我們假設這個房間的擺放已經被某些方法獲知了，這樣我們可以拿到一張這個房間的完整地圖。

2. 當小車剛剛被扔到這個房間，最初它是處於一種懵逼的狀態，除了房間本身的擺設，尺寸的信息，自己究竟處於什麼位置，它是完全不知道的。

3. 最開始，我們會假設這個小車初始的位置可以出現在這個房間的任何一個角落

4. 小車開始用距離探測器探測當前它前後左右的障礙物離它有多遠，假設這個距離向量是Ds（前後左右一共4個距離）。

5. 在這個房間里，任何一個位置周邊障礙物與這個位置的距離都是可以計算出來的。我們首先把這個房間均勻地分成M個方塊，對每個方塊中心位置計算出一個距離向量Dx，我們把這些Dx放在一個集合裡面Dset。

6. 很自然地，我們可以比較Ds和Dset裡面所有的Dx，如果Ds和Dx比較接近，那麼我們可以認為小車處於x這個位置的可能性會高一點。

7. 現在，小車在房間的不同位置出現的可能性就不是相同的了，自然有地方會高一些（Ds，Dx差異小一些）有一些地方會低一些（Ds，Dx差異大一些）。我們用Px表示小車在每個位置出現的概率。

8. 接下來，小車朝著某一個方向移動了一些距離Ms。

9. 在移動前，位置x上小車出現的概率是Px，那麼這個概率會因為移動而轉移到位置x+Ms上，這個過程我們叫做狀態轉移。

10. 所有的Px都會因為小車的移動從最初的位置移動Ms落到新的位置上，這樣，小車在每個位置出現的概率Px就徹底經過了一次更新。

11. 接下來，我們對Px更高的位置以及這個位置的周邊劃分出更多，更小的方塊出來，而對Px低的區域劃分面積大一些的方塊，如果Px太小了，那麼這個位置直接就被我們排除掉，這個過程我們叫做重採樣。之所以劃分尺寸不一樣的方塊，是為了確保在我們假設小車均勻出現在各個位置的概率時，會因為格子的密度不同而在不同區域產生不同的概率分布。

12. 然後我們重複6-11步

13. 在小車不停的移動中，我們反覆計算Px和重新劃分位置方塊，所起到的作用就是持續地逼近小車在這個房間所處位置的概率分布，一直到它最終落到一個唯一的區域裡面。

14. 這個計算過程中，有兩個問題需要小心處理：第一，如果小車對周邊的距離計算有誤差，在第11步中我們可能會過早地排除掉一些不應該排除的位置，這是粒子濾波中的die-out問題，直觀上就是粒子濾波跑著跑著就不見了，這時候只能重置濾波器或者將濾波器的狀態回滾；第二，如果小車的移動距離沒有超出一個方塊，那麼它的狀態有可能沒法實現轉移，如果幾次累計下來，可能造成濾波器沒有跟上狀態的轉移，直觀上就是濾波的效果總是慢幾拍。

15. 大部分的粒子濾波器往往都假設一些很理想的狀態轉移與重採樣的情況。而實際上，由於計算能力和數據採集能力的限制，大部分的粒子濾波往往都是建立在一些稀疏的Dset上面，這樣有可能會造成一些隱藏的狀態被轉移到觀察點上。我的理解是，基於稀疏Dset的粒子濾波必須要確保每次的狀態轉移的偏置量足夠大。放狗的理論在視頻跟蹤里是行得通的，因為它要處理的是一幅完整的圖像。

啊,剛好這會正在翻 probabilistic robotic, 順便簡要答一下這個問題好了

首先粒子濾波其實是這麼多種濾波中最最好理解的那一種(計算物理課上學的),本質上來說,粒子濾波是一種蒙特卡洛方法,我們無妨在物理學角度來看這個東西,
這個問題要統計的看,粒子濾波其實就是,一百萬個機器人放到這個環境里,會發生什麼,產生這一百萬個機器人代表的粒子和環境作用的概率模型可以認為是一種熱力學的規律,而我們最後要做的是統計發生的結果,來尋找到新的規律,規律可以是物理學公式(比如計算物理學),也可能是機器人位置的假設,(比如 slam).

這裡我們有兩種規律,一個是機器人大尺度的分布,這是宏觀的,根據此採樣出粒子,而其餘的互相作用也好,生成概率也好,重採樣也好,其實都只是一個微觀的物理學規律而已,產生的,就是最後的概率分布,這又是一個宏觀規律.

所以說粒子濾波是一個宏觀規律-&>微觀互相作用模擬-&>再次歸納出宏觀規律的過程

推薦一個以前看過的小視頻，很直觀地介紹了粒子濾波的主要思想，貼在這裡，希望有用。

Particle Filter Explained without Equations，youtube

雖然我的回答可能有點不切題，但我覺得很多人對粒子濾波的不理解並不是無法聯繫實際，而且即使給出了實際例子也是一知半解。所以我還是從理論框架上來解釋，但不會寫公式。一句話總結：狹義上的粒子濾波是在鏈狀因子圖（factor graph）上的BP型粒子式序貫貝葉斯推斷。

一般來說，一個貝葉斯推斷問題經常能表示為因子圖的形式，而因子圖可以具有很一般的形式：鏈狀（如單用戶導航）、樹狀（如快速傅立葉變換）、網狀（如網路定位與導航）。

在因子圖上有很多分散式貝葉斯推斷演算法，這些演算法經常被形象地稱為「消息傳遞」。一般而言貝葉斯推斷（無論是MAP還是MMSE類型的）是NP-hard問題，因此一般的因子圖（帶環的）上的消息傳遞演算法都是包含近似的。

一種很強大的近似框架稱為「變分推斷」（variational inference），基本上就是先定義一個分布間的差異（如相對熵，以及其他的散度），然後約束近似分布所屬的集合，通過最小化上述差異函數把真實分布投影到這個集合中。在變分推斷框架下的消息傳遞演算法包括「置信傳播」（Belief Propagtion, BP）型和「變分消息傳遞」（Variational Message Passing）型等等。狹義的粒子濾波一般都是BP型的，但VMP型的其實也存在，如果讀者做過多目標追蹤的話可以考慮一下這句話：PHD濾波器實際上是對點過程的一種VMP型近似。

消息傳遞演算法的另外一個關鍵問題是所謂「消息表示」問題，就是用什麼形式來表示傳遞的消息（基本上可以看作是pdf）。通常這些pdf都很複雜不具有閉式形式（Kalman是極特殊的線性高斯形式，於是有閉式），難以用參數化形式表示。解決方法有二：近似為參數化形式（如擴展Kalman），或者用半參數化和非參數化方法（如CKF、UKF）。粒子表示法屬於一種非參數化方法。

消息表示問題的關鍵在於，如何使用所選的表示方法來實現消息傳遞過程中的運算。BP型演算法中包括三種運算：1. 同變數的消息相乘；2. 變數的Belief和因子相乘；3. 積分（邊緣化）。用粒子來實現的話，3最簡單：把粒子中對應的維度刪去即可。2可採用ancestral sampling，即先在變數的Belief里採樣，然後固定這個樣本在因子中採樣。1比較複雜，方法也很多，其中最常見的就是採樣-重要性-重採樣。