1）**階段：粒子濾波首先根據狀態轉移函式**生成大量的取樣，這些取樣就稱之為粒子，利用這些粒子的加權和來逼近後驗概率密度。根據等式2，權值可通過以下公式計算2）校正階段：隨著觀測值的依次到達，為每個粒子計算相應的重要性權值。這個權值代表了**的位姿取第個粒子時獲得觀測的概率。如此這般下來，對所有粒子都進行這樣乙個評價，越有可能獲得觀測的粒子，獲得的權重越高。
4）地圖估計：對於每個取樣的粒子，通過其取樣的軌跡與觀測計算出相應的地圖估計。
sir演算法需要在新的觀測值到達時從頭評估粒子的權重。當軌跡的長度隨著時間的推移而增加時，這個過程的計算複雜度將越來越高。因此doucet等學者通過式(2)限制重要性概率密度函式來獲得遞迴公式去計算重要性權值。
典型的粒子濾波器應用里程計運動模型作為重要性概率密度函式。這種運動模型的計算非常簡單，並且權值只根據觀測模型即可算出。然而，這種模型並不是最理想的。當機械人裝備雷射雷達（如sick，hokuyo等）時，雷射測得的資料比里程計精確的多，因此使用觀測模型作為重要性概率密度函式將要準確的多。圖1展示了觀測模型的分布明顯小於運動模型的分布。由於觀測模型的分布區域很小，樣本處在觀測的分布（圖中的區域）的機率很小，在保證充分覆蓋觀測的分布情況下所需要的粒子數就會變得很多，這將會導致使用運動模型作為重要性概率密度函式類似的問題：需要大量的樣本來充分覆蓋分布的區域。
為了克服這個問題，可以在生成下一次採用時將最近的觀測考慮進去。通過將整合到概率分布中，可以將抽樣集中在觀測似然的有意義的區域。為此doucet等提出了最優重要性概率密度函式，式4為粒子權重方差的最優分布。
所以式(4)在機械人裝備雷射測距儀的時候非常適用。
現在的rbpf演算法過程是這樣的，首先根據運動模型對機械人下一時刻位姿進行**，得到**的狀態值並且對其進行取樣。第二步是通過最優概率密度函式（4）對各個粒子進行權值的計算。之後進行重取樣，根據粒子的權重重新分布粒子，為下次**提供輸入。最後，根據粒子的軌跡計算地圖的後驗概率密度函式。
圖2中展示了在不同場景下的粒子分布情況。（a）為在開放的走廊中，粒子沿著走廊分布。（b）為在死胡同中，粒子分布的不確定性很小，分布的很集中。（c）為根據里程計運動模型**生成的粒子分布，分布的很分散。
因此，該演算法將最近的里程計資訊與觀測資訊同時併入重要性概率密度函式中，使用匹配掃瞄過程來確定觀察似然函式的分布區域，這樣就把取樣的重點集中在可能性更高的區域。當由於觀察不佳或者當前掃瞄與先前計算的地圖重疊區域太小而失敗時，將會用圖2中（c）所示的里程計運動模型作為重要性概率密度函式。
對粒子濾波的效能具有重要影響的另乙個因素是重取樣步驟。在重取樣期間，低權值的粒子通常由高權值的取樣代替。由於用來逼近目標分布使用的粒子數量是有限的，所以重取樣步驟非常重要。重取樣步驟也可能把一些好的粒子濾去，隨著的進行，粒子的數目會逐漸減少，最後導致粒子耗盡使該演算法失效。通常採用有效粒子數來衡量粒子權值的退化程度，即
這裡的為粒子的歸一化權值。
doucet等為了減少進行重取樣步驟的次數，提出了一種理論判定方法來判定是否需要進行重取樣。只有當下降到閾值（，為粒子數）以下時，才進行一次重取樣。由於重取樣只在需要時進行，進行重取樣的次數將大大減少。多次的實驗證明了這種方法大大降低了將好粒子濾去的風險。
圖3的(a)圖是通過配有sick雷射感測器的先鋒2(pioneer 2)機械人在長寬均為28m的室內環境中構建的地圖。圖3的(b)圖顯示了該地圖的放大細節，展示了該演算法建圖的準確性。
圖4顯示了包含幾個巢狀閉環的室內環境。在這種環境下地圖構建非常困難，閉環增加了rao-blackwellized粒子濾波器粒子耗盡的機率。圖4展示了rbpf演算法在使用60個粒子時可以生成一致並且準確的地圖，然而，此時產生的地圖有時會產生雙層牆壁。
本文介紹了一種由doucet等提出的改進rao-blackwellized粒子濾波器構建柵格地圖的演算法。該方法基於最近的感測器資訊，測距和掃瞄匹配過程的觀測似然函式來計算高精度的後驗概率密度函式。改進演算法比原始演算法應用了更少的粒子來構建地圖，並且使用了一種更準確的方式分布取樣粒子。此外，該演算法使用了基於有效樣本量的自適應重取樣策略。該方法減少了粒子濾波器中不必要的重取樣過程的次數，從而大大降低了粒子耗盡的風險。
該演算法通過應用不同機械人裝備雷射測距儀進行了多次實驗。在這些實驗中，該演算法所需的粒子數量通常比原始rbpf演算法所需的粒子數小乙個數量級，證明了其魯棒性、優越性。
粒子濾波(particle filter)matlab實現
particle filter tutorial 粒子濾波：從推導到應用
粒子濾波理論
20170330 濾波 基於粒子濾波的物體跟蹤
一直都覺得粒子濾波是個挺牛的東西，每次試圖看文獻都被複雜的數學符號搞得看不下去。乙個偶然的機會發現了rob hess 實現的這個粒子濾波。從 入手，一下子就明白了粒子濾波的原理。根據維基百科上對粒子濾波的介紹 粒子濾波其實有很多變種，rob hess實現的這種應該是最基本的一種，sampling i...
基於粒子濾波的物體跟蹤
一直都覺得粒子濾波是個挺牛的東西，每次試圖看文獻都被複雜的數學符號搞得看不下去。乙個偶然的機會發現了rob hess 實現的這個粒子濾波。從 入手，一下子就明白了粒子濾波的原理。根據維基百科上對粒子濾波的介紹 粒子濾波其實有很多變種，rob hess實現的這種應該是最基本的一種，sampling i...
基於粒子濾波的物體跟蹤
一直都覺得粒子濾波是個挺牛的東西，每次試圖看文獻都被複雜的數學符號搞得看不下去。乙個偶然的機會發現了rob hess 實現的這個粒子濾波。從 入手，一下子就明白了粒子濾波的原理。根據維基百科上對粒子濾波的介紹 粒子濾波其實有很多變種，rob hess實現的這種應該是最基本的一種，sampling i...