[發明專利]一種二次采樣的粒子濾波算法

有效

• 申請號: 201910315096.3
• 申請日: 2019-04-18
• 公開（公告）號: CN110113030B
• 公開（公告）日: 2023-06-30
• 發明（設計）人: 張小國;高燁;鄭冰清;張開心;王慧青
• 申請（專利權）人: 東南大學
• 主分類號: H03H17/02
• 分類號: H03H17/02
• 代理公司: 南京眾聯專利代理有限公司 32206
• 代理人: 周蔚然

本發明公開了一種二次采樣的粒子濾波算法，包括如下步驟：（1）初始化，按初始分布生成粒子集；（2）用重要性概率密度函數進行重要性采樣，并基于觀測方程對其進行評價；（3）選取評價最優的P個粒子做二次采樣；（4）根據粒子權重退化程度判斷是否對粒子集進行重采樣；（5）輸出狀態估計值。本發明對基于觀測方程評價后的采樣粒子進行高密度二次采樣，能夠有效提高粒子濾波的精度。

技術領域

本發明屬于粒子濾波技術領域，具體涉及一種二次采樣的粒子濾波算法。

背景技術

粒子濾波(Particle?Filter)的思想基于貝葉斯濾波與蒙特卡羅采樣，為了解決序貫重要性采樣(SIS，Sequential?Importance?Sampling)的粒子權重退化的問題，在SIS的基礎上引進了重采樣(Resample)。重采樣主要思想為舍棄權值小的粒子，復制權值較大的粒子。重采樣的方式有很多，常用的重采樣方法包括多項式(Multinomial?resampling)重采樣、分層重采樣(Stratified?resampling)、殘差重采樣(Residual?resampling)與系統重采樣(Systematic?resampling)等。

粒子濾波的核心思想是從后驗概率中生成隨機狀態粒子對重要概率密度進行近似，按照蒙特卡羅思想，即用采樣粒子的平均值來代替積分運算，從而得到狀態估計值。

粒子濾波是在某一時刻用若干個粒子去逼近該時刻目標的實際狀態，所以粒子數目的大小會直接影響到估計狀態的準確性。但是粒子數目的增多意味著運算時間更長，計算復雜度更高，為了符合實際應用，需要在粒子采樣數目與估計狀態精度之間做權衡。

發明內容

為解決上述問題，本發明公開了一種二次采樣的粒子濾波算法，能夠有效提高粒子濾波精度。

為達到上述目的，本發明的技術方案如下：

一種二次采樣的粒子濾波算法，包括以下步驟：

(1)初始化，按初始分布生成粒子集；

(2)重要性概率密度函數進行重要性采樣，并基于觀測方程對其進行評價；

(3)選取評價最優的p個粒子做二次采樣；

(4)根據粒子權重退化程度判斷是否對粒子集進行重采樣；

(5)輸出狀態估計值。

所述步驟(1)具體包括以下步驟：

(11)根據先驗概率密度p(x₀)，采樣M個粒子并初始化每個粒子對應的權重值為優選均勻采樣；以初始點為中心，按照3σ原則，在±3σ范圍內進行均勻采樣。

(12)采樣粒子數M:

σ為p(x₀)的標準差，R為采樣步長，ρ為采樣密度，表示在一個采樣步長內取的采樣點數；N表示維度。

所述步驟(2)具體包括以下步驟：

(21)引入一個與后驗概率密度函數p(x_0：k|y_1：k)分布相近且易于采樣的重要性密度函數，從重要性概率密度中生成M個采樣粒子

(22)基于觀測方程，對M個采樣粒子進行評價，計算k時刻，采樣粒子的權重值

所述步驟(3)包括以下步驟：

(31)取步驟2生成的M個采樣粒子中對應權重值最大的P個(建議取3)，對其進行二次高密度采樣，以粒子為中心點，±L(建議取)為采樣范圍，以R₁為采樣步長(建議取)，ρ₁為采樣密度，得到Q個采樣粒子