1. 粒子濾波理論
粒子濾波通過非參數化的蒙特卡洛(Monte Carlo)模擬方法來實現遞推貝葉斯濾波，適用於任何能用狀態空間模型描述的非線性系統，精度可以逼近最優估計。
1.1. 貝葉斯濾波
動態系統的目標跟蹤問題可以通過下圖的狀態空間模型來描述。
在目標跟蹤問題中，動態系統的狀態空間模型可描述為
其中f()和h()分別為狀態轉移方程與觀測方程，xk為系統狀態，yk為觀測值，uk為過程噪聲，vk為觀測噪聲。為便於描述,
分別表示0到k時刻所有的狀態與觀測值。
在處理目標跟蹤問題時，通常假設目標的狀態轉移過程服從一階馬爾可夫模型，即當前時刻的狀態只與上一時刻的狀態有關，另外一個假設為觀測值相互獨立，即觀測值yk只與k時刻的狀態xk有關。
貝葉斯濾波為非線性系統的狀態估計問題提供了一種基於概率分布形式的解決方案。貝葉斯濾波將狀態估計視為一個概率推理過程，即將目標狀態的估計問題轉換為利用貝葉斯公式求解后驗概率密度或濾波概率密度，進而獲得目標狀態的最優估計。貝葉斯濾波包含預測和更新兩個階段，預測過程利用系統模型預測狀態的先驗概率密度，更新過程則利用最新的測量值對先驗概率密度進行修正，得到后驗概率密度。
貝葉斯濾波以遞推的形式給出后驗(或濾波)概率密度函數的最優解。目標狀態的最優估計值可由后驗(或濾波)概率密度函數進行計算。通常根據極大后驗(MAP)准則或最小均方誤差(MMSE)准則，將具有極大后驗概率密度的狀態或條件均值作為系統狀態的估計值。
貝葉斯濾波需要進行積分運算，除了一些特殊的系統模型（如線性高斯系統，有限狀態的離散系統）之外，對於一般的非線性、非高斯系統，貝葉斯濾波很難得到后驗概率的封閉解析式。因此，現有的非線性濾波器多采用近似的計算方法解決積分問題，以此來獲取估計的次優解。在系統的非線性模型可由在當前狀態展開的線性模型有限近似的前提下，基於一階或二階Taylor級數展開的擴展Kalman濾波得到廣泛應用。在一般情況下，逼近概率密度函數比逼近非線性函數容易實現。據此，Julier與Uhlmann提出一種Unscented Kalman濾波器，通過選定的sigma點來精確估計隨機變量經非線性變換后的均值和方差，從而更好的近似狀態的概率密度函數，其理論估計精度優於擴展Kalman濾波。獲取次優解的另外一中方案便是基於蒙特卡洛模擬的粒子濾波器。
1.2. 粒子濾波
1.2.1. 貝葉斯重要性采樣
蒙特卡洛模擬是一種利用隨機數求解物理和數學問題的計算方法，又稱為計算機隨機模擬方法。該方法源於第一次世界大戰期間美國研制原子彈的曼哈頓計划，著名數學家馮諾伊曼作為該計划的主持人之一，用馳名世界的賭城，摩納哥的蒙特卡洛來命名這種方法。蒙特卡洛模擬方法利用所求狀態空間中大量的樣本點來近似逼近待估計變量的后驗概率分布，從而將積分問題轉換為有限樣本點的求和問題。粒子濾波算法的核心思想便是利用一系列隨機樣本的加權和表示后驗概率密度，通過求和來近似積分操作。
蒙特卡洛方法一般可以歸納為以下三個步驟：
(1)構造概率模型。對於本身具有隨機性質的問題，主要工作是正確地描述和模擬這個概率過程。對於確定性問題，比如計算定積分、求解線性方程組、偏微分方程等問題，采用蒙特卡洛方法求解需要事先構造一個人為的概率過程，將它的某些參量視為問題的解。
(2)從指定概率分布中采樣。產生服從己知概率分布的隨機變量是實現蒙特卡洛方法模擬試驗的關鍵步驟。
(3)建立各種估計量的估計。一般說來，構造出概率模型並能從中抽樣后，便可進行現模擬試驗。隨后，就要確定一個隨機變量，將其作為待求解問題的解進行估計。
在實際計算中，通常無法直接從后驗概率分布中采樣，如何得到服從后驗概率分布的隨機樣本是蒙特卡洛方法中基本的問題之一。重要性采樣法引入一個已知的、容易采樣的重要性概率密度函數，從中生成采樣粒子，利用這些隨機樣本的加權和來逼近后驗濾波概率密度。
1.2.2序貫重要性采樣算法
在基於重要性采樣的蒙特卡洛模擬方法中，估計后驗濾波概率需要利用所有的觀測數據，每次新的觀測數據來到都需要重新計算整個狀態序列的重要性權值。序貫重要性采樣作為粒子濾波的基礎，它將統計學中的序貫分析方法應用到的蒙特卡洛方法中，從而實現后驗濾波概率密度的遞推估計。
（推導過程）
序貫重要性采樣算法從重要性概率密度函數中生成采樣粒子，並隨着測量值的依次到來遞推求得相應的權值，最終以粒子加權和的形式來描述后驗濾波概率密度，進而得到狀態估計。序貫重要性采樣算法的流程可以用如下偽代碼描述：
為了得到正確的狀態估計，通常希望粒子權值的方差盡可能趨近於零。然而，序貫蒙特卡洛模擬方法一般都存在權值退化問題。在實際計算中，經過數次迭代，只有少數粒子的權值較大，其余粒子的權值可忽略不計。粒子權值的方差隨着時間增大，狀態空間中的有效粒子數較少。隨着無效采樣粒子數目的增加，使得大量的計算浪費在對估計后驗濾波概率分布幾乎不起作用的粒子更新上，使得估計性能下降。
克服序貫重要性采樣算法權值退化現象最直接的方法是增加粒子數，而這會造成計算量的相應增加，影響計算的實時性。因此，一般采用以下兩種途徑：(1)選擇合適的重要性概率密度函數；(2)在序貫重要性采樣之后，采用重采樣方法。
1.2.3. 重要密度函數的選擇
重要性概率密度函數的選擇對粒子濾波的性能有很大影響，在設計與實現粒子濾波器的過程中十分重要。在工程應用中，通常選取狀態變量的轉移概率密度函數作為重要性概率密度函數。轉移概率的形式簡單且易於實現，在觀測精度不高的場合，將其作為重要性概率密度函數可以取得較好的濾波效果。然而，采用轉移概率密度函數作為重要性概率密度函數沒有考慮最新觀測數據所提供的信息，從中抽取的樣本與真實后驗分布產生的樣本存在一定的偏差，特別是當觀測模型具有較高的精度或預測先驗與似然函數之間重疊部分較少時，這種偏差尤為明顯。
在實際情況中，構造最優重要性概率密度函數的困難程度與直接從后驗概率分布中抽取樣本的困難程度等同。從最優重要性概率密度函數的表達形式來看，產生下一個預測粒子依賴於已有的粒子和最新的觀測數據，這對於設計重要性概率密度函數具有重要的指導作用，即應該有效利用最新的觀測信息，在易於采樣實現的基礎上，將更多的粒子移動到似然函數值較高的區域。
1.2.4. 重采樣方法
針對序貫重要性采樣算法存在的權值退化現象，Gordon等提出了一種名為Bootstrap的粒子濾波算法。該算法在每步迭代過程中，根據粒子權值對離散粒子進行重采樣，在一定程度上克服了這個問題。重采樣方法舍棄權值較小的粒子，代之以權值較大的粒子。重采樣策略包括固定時間間隔重采樣與根據粒子權值進行的動態重采樣。動態重采樣通常根據當前的有效粒子數或最大與最小權值比來判斷是否需要進行重采樣。常用的重采樣方法包括多項式(Multinomial resampling)重采樣、殘差重采樣(Residual resampling)、分層重采樣(Stratified resampling)與系統重采樣(Systematic resampling)等。
殘余重采樣采用新的權值選擇余下的粒子，殘余重采樣的主要過程為
重采樣並沒有從根本上解決權值退化問題。重采樣后的粒子之間不再是統計獨立關系，給估計結果帶來額外的方差。重采樣破壞了序貫重要性采樣算法的並行性，不利於VLSI硬件實現。另外，頻繁的重采樣會降低對測量數據中野值的魯棒性。由於重采樣后的粒子集中包含了多個重復的粒子，重采樣過程可能導致粒子多樣性的喪失，此類問題在噪聲較小的環境下更加嚴重。因此，一個好的重采樣算法應該在增加粒子多樣性和減少權值較小的粒子數目之間進行有效折衷。
標准化粒子濾波算法流程：
