“0公式系列”之粒子濾波演算法----逃離迷宮
放在開頭，“0公式系列”：無公式，無理論，單靠動畫，或圖片，加上個人理解，來解釋演算法中的思想。
你醒來在一個迷宮內，不知道自己在這個大迷宮的哪一個位置，你要出去，但是你也不能亂走吧，正當你迷茫的時候，你發現自己手裡有一張紙質的迷宮地圖，該地圖不僅包含了迷宮的形狀，還包括了迷宮的尺寸。於是，希望來了，你現在要做的就是用這個地圖，應用神奇的“粒子濾波”演算法（particle filtering，以下簡稱pf），完成自身的定位，然後走出去（走出去這個環節與pf無關）。
圖1.手中的迷宮地圖
pf第一步：假設你在迷宮的任何一個位置（這可能聽的有點扯），周圍的牆體高大，你爬不上去，如圖2。你假設的你所處的位置就叫粒子（紅點），現在你前方不遠處有一堵牆（等下我再解釋看不到牆的情況），這個所謂的“牆”，它可能是個轉角處，也可能是個死衚衕。你能用你的眼睛，估計你面前這堵牆到你的距離，假設是是20m左右吧，注意這個地方，你眼睛看到的叫觀測值，觀測值是有偏差的。好了，我們有了第一個觀測值，重複一下，我們到我們面前這堵牆的距離有大概20m遠，你不用管你面朝東南西北，你也不用管你的後面有沒有牆。總之啊，你距離一堵眼前的牆有大概20m遠，那麼就是說，你可以把距離轉角處或者死衚衕不在20m範圍內的粒子（假設的位置點）去掉
，這樣的話，你可以排除一些最開始假設的位置點。如圖3。
圖2.紅色的點是你最開始假設的位置，我畫的一點也不密集，實際上假設的位置是鋪滿整個迷宮的
圖3.雖然去除了一些點，但是還是有不少假設的位置
pf第二步：現在你要往前走20m，就走到你面前的牆的那個位置。分三種情況：
（1）是個拐角處，那還好，拐過去就行了。
（2）是個“T”型岔口也沒關係，規定遇到岔口向左轉吧。
（3）是個死衚衕就悲劇了，那就回到出發點向後轉吧，看看你在出發點的時候後面有沒有牆。
pf第三步：假設你的運氣不錯，不是個死衚衕，那麼你到了轉角處後，轉向，然後停！你再看看你的面前沒有新的牆。這分兩種情況：
（1）有牆，這跟你的第一步一樣，估計你目前的位置到這個新牆的距離。不管它是個轉角處還是個死衚衕都不重要。假設你距離新牆的位置是10m。現在重點來了啊！你捋捋思路，你看到前面有堵牆，起身，往前走了大概20m，轉彎後前面又有一堵牆，距離大概10m。也就是說，你轉彎後的視野內，必須有一堵牆在你面前10m左右遠（定義為條件1）。不滿足這個條件的之前的粒子（假設的位置）都要去掉。好了現在我們有了兩幅圖，一幅是所有粒子往前走20m到了牆跟頭，見圖4。你得到了很多藍點，根據條件1，除去不符合條件藍色點以及之前所有假設的紅點。見圖5。
（2）沒有牆，是個無盡的大道。那就有點悲劇了，只能往前走了。直到看到牆位置，而且你還得記得你走了大概多少米，專業點這叫里程計讀到的資料。假設你走了大概100m，終於看到了一個新的牆在你前面50m處，那麼這條道就有100+50m左右的距離。接下來跟上面（1）的過程一樣，看到牆就好了。
圖4.藍色是紅點（也就是你醒來的位置）向前移動大概20m的後的位置
圖5.現在我只保留了符合條件1的藍點，紅色箭頭表示你最開始前進的方向，黃色箭頭表示你到了牆角轉頭後的方向
現在很舒服，你已經定位到地圖3個可能的位置了，比之前全地圖都是假設位置的情況好多了。但是你有沒有發現，你在轉角處看到新牆的位置，是大概10m，有可能是6m，也可能是14m，所以說，這個3個假設位置也是有不同的可信度。根據圖5。例如，對於最上面這個假設位置，看到新牆的距離就比10m小一些，右下角這個位置，看到新牆的距離就很接近10m，左下角的這個位置，看到新牆的距離就比10m大一些。但他們三個都是大概10m， 為了區分，我們就給右下角的可信度大一些，其他兩個位置（最上面和左下角）的可信度就小一些。
話說我定義的可信度有什麼用呢？
pf第四步：我們基於上面三個點重新假設位置，注意，根據可信度的大小，假設位置的點的數量不同，見圖6，就是說我們更相信我們在右下角的位置，但是我們不排除在其他那兩個可信度不高的位置的情況。
然後，重複第二步到第四步，重複幾次，你就可以定位到你的位置了，很神奇吧。這個重複的圖我就不畫了，大家自己想想吧。重點就是，目前的位置跟上一時刻的位置是相關的。
，
整個粒子濾波演算法的流程我就說完了。這裡沒有公式（貝葉斯公式巴拉巴拉），專業術語（重取樣巴拉巴拉），網上有很多原理的介紹，感興趣的，可以一邊看正規的原理和公式，一邊思考我這個迷宮定位的方法，找一找他們之間的聯絡。
另外推薦一本書《粒子濾波演算法及其應用》（朱志宇），上面還有粒子濾波的衍生演算法，以及在目標檢測，地圖定位，FPGA，DSP的應用。