基于“新息誤差”的粒子流濾波算法

周德運(yùn)，劉 斌，2*，蘇 茜

（1.冶金自動化與測量技術(shù)教育部工程研究中心（武漢科技大學(xué)），武漢 430081；2.冶金工業(yè)過程系統(tǒng)科學(xué)湖北省重點(diǎn)實(shí)驗(yàn)室（武漢科技大學(xué)），武漢 430081）

（?通信作者電子郵箱liubin@wust.edu.cn）

0 引言

粒子濾波（Particle Filter，PF）是基于遞推貝葉斯后驗(yàn)概率理論的一種序貫蒙特卡羅算法［1］，是一種基于蒙特卡羅模擬方法來實(shí)現(xiàn)貝葉斯濾波的算法。與卡爾曼濾波（Kalman Filter，KF）相比，粒子濾波不受系統(tǒng)模型與噪聲的限制（不需要系統(tǒng)模型為線性，也不需要系統(tǒng)噪聲為高斯分布），因此被廣泛應(yīng)用于視覺跟蹤、目標(biāo)定位導(dǎo)航、通信與信號處理等眾多領(lǐng)域。

粒子濾波作為一種重要的非線性遞歸貝葉斯濾波方法，具有良好的算法可擴(kuò)展性和普適性，然而粒子濾波仍然存在一些理論、方法上的缺陷——權(quán)值退化［2］與粒子多樣性喪失問題。權(quán)值退化是序貫重要性采樣（Sequential Importance Sampling，SIS）難以避免的問題，文獻(xiàn)［3］中指出這是由于貝葉斯公式的函數(shù)相乘形式導(dǎo)致的。1993 年，Gordon 等引入重采樣方法，基于同分布原則［4］，對權(quán)值更新后的粒子集合重新采樣，獲得一個粒子權(quán)值相等的新的粒子集，有效緩解了權(quán)值退化問題［5］；然而，對嚴(yán)重退化的粒子集進(jìn)行無偏重采樣所得到的粒子可能幾乎全部源自極少數(shù)粒子的自我復(fù)制，造成粒子多樣性喪失的問題。針對上述問題，國內(nèi)外學(xué)者做了大量的研究，主要集中在提議分布（重要性采樣函數(shù)）的選取與重采樣的優(yōu)化中：文獻(xiàn)［6］通過參考最新觀測信息提出輔助粒子濾波器；文獻(xiàn)［7］通過非線性高斯濾波器來獲得近似后驗(yàn)，并以此作為提議分布，提出無跡粒子濾波器（Unscented Particle Filter，UPF）；文獻(xiàn)［8］提出混合多核偏最小二乘粒子濾波方法；文獻(xiàn)［9］引入智能算法，將蝙蝠算法與粒子濾波相結(jié)合；文獻(xiàn)［10］提出了基于似然分布調(diào)整的粒子群優(yōu)化粒子濾波方法；文獻(xiàn)［11］采用粒子濾波應(yīng)用于多擴(kuò)展目標(biāo)跟蹤，并提出順序采樣方法來解決維度災(zāi)難的問題；文獻(xiàn)［12-13］提出粒子流濾波（Particle Flow Filter，PFF），以粒子流動的方式來計算貝葉斯公式。文獻(xiàn)［13］中指出粒子流濾波器比經(jīng)典粒子濾波器快了好幾個數(shù)量級，而且對于復(fù)雜非線性問題比擴(kuò)展卡爾曼濾波精確幾個數(shù)量級。

1 貝葉斯估計與粒子濾波

1.1 葉斯濾波

對于狀態(tài)估計問題（定位、跟蹤、動態(tài)參數(shù)）的狀態(tài)空間模型可以描述為：

其中：t 表示連續(xù)時間，k 表示離散時間；狀態(tài)xt、xk∈Rd為連續(xù)、離散時間狀態(tài)；wt、wk-1為過程噪聲；ft、fk為連續(xù)、離散狀態(tài)轉(zhuǎn)移方程；zk∈Rm為k時刻的觀測值，h為觀測方程，vk為觀測噪聲。

貝葉斯濾波為非線性系統(tǒng)的狀態(tài)估計問題提供了一種基于概率分布形式的解決方案。貝葉斯濾波包括預(yù)測和更新兩個過程，假設(shè)k -1時刻概率密度已知為p(xk-1|z1：k-1)，預(yù)測過程即獲取先驗(yàn)概率密度，由Chapman-Kolmogorow方程有：

然后，利用貝葉斯公式求解后驗(yàn)概率密度p(xk|z1：k)實(shí)現(xiàn)更新過程：

其中：p(xk|z1：k)為后驗(yàn)概率密度、p(xk|z1：k-1)為先驗(yàn)概率密度、p(zk|xk)為似然，分母p(zk|z1：k-1)為歸一化常數(shù)：

1.2 基本粒子濾波器

粒子濾波基于蒙特卡羅模擬方法，利用所求狀態(tài)空間中大量的樣本點(diǎn)來近似狀態(tài)后驗(yàn)分布，從而將積分問題轉(zhuǎn)換為有限樣本的求和問題。然而在實(shí)際計算中通常無法直接從后驗(yàn)分布中進(jìn)行采樣，因此需要引入一個易于采樣的重要性函數(shù)生成采樣樣本，從而利用一組加權(quán)的樣本xk=來近似后驗(yàn)概率密度：

式（6）中，權(quán)值更新公式為：

對粒子權(quán)重進(jìn)行歸一化處理后即可用樣本均值代替復(fù)雜的積分運(yùn)算。式（3）～（7）即SIS，文獻(xiàn)［2］指出SIS 存在嚴(yán)重的粒子權(quán)值退化現(xiàn)象。權(quán)值退化指濾波器經(jīng)過多次迭代后，很多粒子的權(quán)重都變得很?。ㄚ吔?），只有少數(shù)粒子（甚至一個）的權(quán)重比較大，并且粒子權(quán)值的方差隨著時間增大，狀態(tài)空間中的有效粒子數(shù)較少，大幅浪費(fèi)計算資源。因此在SIS之后再進(jìn)行重采樣在一定程度上解決了這個問題，形成了常見的SIR（Sampling Importance Resampling）粒子濾波器。

1.3 粒子流濾波器

粒子流濾波是將狀態(tài)空間中服從先驗(yàn)分布的粒子移動到其對應(yīng)的后驗(yàn)分布上，以粒子流動的方式代替貝葉斯公式實(shí)現(xiàn)貝葉斯濾波。粒子流濾波需要建立一個微分方程來平滑移動粒子，得到后驗(yàn)隨時間變化的形式。

文獻(xiàn)［12-13］中，忽略式（4）的分母獲得未歸一化的貝葉斯公式。為簡化表達(dá)，此處以p(x) 表示后驗(yàn)概率密度p(xk|z1：k)，g(x)表示先驗(yàn)概率密度p(xk|z1：k-1)，l(x)表示似然p(zk|xk)，則未歸一化貝葉斯公式可以簡化為：

對式（8）兩端同時取對數(shù)并定義如下同倫函數(shù)：

式中：f(x，λ)為滿足Fokker-Planck方程的函數(shù)，w為過程噪聲，可以認(rèn)為f(x，λ)為粒子從先驗(yàn)分布移動到后驗(yàn)分布的“速度場”。

不考慮噪聲w，p(x，λ)滿足零擴(kuò)散項的Fokker-Planck 方程［15］，有式（11）成立：

其中??(?)表示(?)的散度。對式（9）求λ 偏導(dǎo)，并結(jié)合式（11），可得：

由式（12）求出f(x，λ)后對式（10）進(jìn)行0到1積分，將先驗(yàn)粒子平滑移動到后驗(yàn)分布上。其中，f(x，λ)的求解是粒子流濾波中一個難點(diǎn)，文獻(xiàn)［16］給出了f(x，λ)的17種求解方法。

2 改進(jìn)粒子流濾波器

2.1 粒子流模型改進(jìn)優(yōu)化

與粒子流濾波不同的是，本文考慮歸一化形式下的貝葉斯公式。在粒子流動過程中引入了“新息誤差”結(jié)構(gòu)，使用Galerkin 有限元法求得f(x，λ)的弱形式解（數(shù)值解）。將式（1）和（2）的濾波問題，重寫為下列隨機(jī)微分方程（Stochastic Differential Equation，SDE）形式：

其中：Xt∈Rd為t時刻的狀態(tài)；Ztn為t=tn時刻的觀測值；a(?)、h(?)為狀態(tài)轉(zhuǎn)移函數(shù)與測量函數(shù)；{Bt}為獨(dú)立于X 的d 維標(biāo)準(zhǔn)維納過程；Wtn是一個獨(dú)立于X 的m 維變量，每個分量都服從高斯分布，協(xié)方差為R。通常將函數(shù)h(?)表示為列向量h=(h1(x)，h2(x)，…，hm(x))，第j個元素表示為hj，假設(shè)h 對x可微。

2.2 求解映射P*與P

映射P*與P可以分為兩部分：第一部分為由n -1時刻的后驗(yàn)通過狀態(tài)方程得到n時刻的先驗(yàn)，即由由兩者是相同的；第二部分即由n 時刻的先驗(yàn)得到后驗(yàn)，對于P*第二部分由貝葉斯公式即式（4）得到，構(gòu)造同倫函數(shù)：

其中：l(x)為似然，λ ∈[0，1]。由式（16）可知同倫函數(shù)定義了粒子從先驗(yàn)分布（λ=0）到后驗(yàn)分布（λ=1）的變化過程中的概率分布，l(x)表達(dá)式為：

對式（16）兩端同時取對數(shù)：

對式（18）兩端對λ求偏導(dǎo)有：

式（19）即映射P*的第二部分，接下來求映射P 的第二部分，由式（11）有：

注意到式（19）的表達(dá)形式，將f(x，λ)分為包含最新觀測Ztn與不包含最新觀測兩部分。

引入新息誤差，令：

其中K(x，λ)、v(x，λ)滿足連續(xù)二次可微，且滿足邊界條件：

式中：K(x，λ)為d × m 矩陣，可以表示為K=[?φ1，?φ2，…，?φm]，?表示微分算子，?φj表示φj的梯度；v(x，λ)為d × 1 矩陣。為簡化表達(dá)，用pn、K、v 表示pn(x，λ)、v(x，λ)、K(x，λ)，故式（20）可以寫為：

式中，K(x，λ)為式（22）、（24）BVP 方程的解。同理，v(x，λ)滿足下列BVP方程：

式（26）與式（19）具有相同的表達(dá)形式，由此可知在先驗(yàn)相等的情況下映射P、P*是相同的，即后驗(yàn)結(jié)合式（21）與式（13）有：

由式（27）可知，本文提出的改進(jìn)粒子流濾波引入了“新息誤差”結(jié)構(gòu)Ztn-h(xi)，粒子流在求解f(x，λ)時得到的弱形式解與觀測方程有關(guān)涉及(Ztn-h(xi))2項。而且，本文提出的改進(jìn)算法在求解BVP 方程時未引入最新的觀測，與粒子流濾波相比，一定程度上削弱了對觀測的依賴性。

2.3 求解BVP方程

對于固定時刻t有K=[?φ1，?φ2，…，?φm]，求解BVP方程（24），其弱形式解可定義如下，對任意一階可微函數(shù)（測試函數(shù)）滿足式（28）：

基于Galerkin 有限元法可以用有限維空間中一組基函數(shù)的線性組合來近似φj(x，λ)：

故式（28）可以表示為：

將式（31）中的ψ(x)由ψl(x)替換，令L=d，則式（31）可以表示為由d 個方程組成的線性矩陣方程Amj=b，Asl表示A 中第s行l(wèi)列元素，b中第s個元素表示為bs，故A、b可以表示為：

結(jié)合式（32）與（34）可知，A=I 為一個單位陣，即可以得到mj=b。

由式（30）可知：

由式（35）可以得到K(x，λ)為：

同理可得BVP方程式（25）的解：

2.4 改進(jìn)粒子流濾波算法步驟

綜上所述，改進(jìn)的粒子流濾波算法步驟如下：

步驟1 初始化，從初始分布p*(0)中采樣N個樣本；

步驟2 從t=1到t=T，執(zhí)行步驟3～步驟5；

步驟4 λ=0，執(zhí)行映射P；

1）如果λ ≤1；

3）計算新息誤差

5）令λ=λ+Δλ，返回1）；

步驟6 t=t+1，返回步驟3。

3 實(shí)驗(yàn)仿真結(jié)果與分析

3.1 一維非線性濾波模型

選擇非線性一維系統(tǒng)的狀態(tài)方程為：

其中：w(k)、v(k)符合高斯分布，均值為0、方差為Q、R。

用粒子濾波、粒子流與本文提出的改進(jìn)的粒子流濾波算法對該非線性系統(tǒng)進(jìn)行狀態(tài)估計和跟蹤。由于粒子濾波結(jié)果存在隨機(jī)性，進(jìn)行30 次蒙特卡洛仿真。選擇性能指標(biāo)為均方根誤差（Root Mean Square Error，RMSE）：

取粒子數(shù)N=50，仿真時長T=50，粒子流與改進(jìn)粒子流取相同時間步長Δλ。表1 為在R、Q、Δλ 在不同設(shè)定下，狀態(tài)估計偏差和運(yùn)行時間的對比。當(dāng)Q=0.1、R=0.1 時，仿真結(jié)果如圖1（a）所示；當(dāng)Q=1、R=0.1 時，仿真結(jié)果如圖1（b）所示。

圖1 Q為0.1或1時的狀態(tài)估計均方根誤差Fig.1 RMSE of state estimation with Q of 0.1 or 1

由圖1 可知，當(dāng)過程噪聲較小時，三種算法均可取得較好的結(jié)果；在過程噪聲較大時，粒子流濾波效果較差。這是由于噪聲較大時求得的f(x，λ)誤差較大，粒子濾波與改進(jìn)粒子流濾波結(jié)果相對較好，其中改進(jìn)的粒子流濾波具有更高的精度。從表1 可以看出，當(dāng)過程噪聲與量測噪聲都較大時，改進(jìn)粒子流濾波仍可取得較好的結(jié)果。

此外由表1可以看出，PFF與改進(jìn)的PFF算法運(yùn)行時間高于粒子濾波。PFF 與改進(jìn)的PFF 算法運(yùn)行時間取決于時間步長Δλ，當(dāng)Δλ 相同時，本文算法與粒子流算法運(yùn)行時間相近。這是因?yàn)楸疚母倪M(jìn)了“速度場”f(x，λ)的結(jié)構(gòu)，提高了濾波精度的同時并沒有增加計算量。

表1 三種算法仿真結(jié)果對比Tab.1 Comparison of simulation results of three algorithms

當(dāng)N=50，過程噪聲為伽馬噪聲w～Γ(3，2)時，對比PF、UPF、PFF、改進(jìn)PFF算法，仿真結(jié)果如圖2所示。

由圖2 可知，在過程噪聲為伽馬噪聲情況下，PFF 算法無法進(jìn)行較好的估計；在測量噪聲較大情況下，改進(jìn)PFF算法有相對較好的效果；在測量噪聲較小時，UPF 與改進(jìn)的PFF 算法有較好的效果。文獻(xiàn)［17］證明當(dāng)觀測噪聲較為顯著時，基于最新觀測優(yōu)化的提議分布并不能帶來更高的濾波精度。

由上述仿真實(shí)驗(yàn)可以看出，粒子濾波、粒子流濾波、UPF在過程噪聲較小、觀測信息比較精確時才更有效。原因是PF利用最新的觀測信息計算權(quán)重，PFF、UPF 參考最新的觀測信息來優(yōu)化提議分布。另外利用智能算法、啟發(fā)式算法與粒子濾波結(jié)合也是基于最新的觀測，然而多數(shù)方案缺乏堅實(shí)的理論證明，且以增加計算復(fù)雜度為代價。其中與觀測相關(guān)的項設(shè)為b=(Zk-h(xk))，粒子濾波在計算權(quán)重時是根據(jù)觀測方程來計算的涉及項；粒子流在求解f(x，λ)時，得到的弱形式解涉及b2項，且弱形式解為數(shù)值解，當(dāng)噪聲變大時得到的解的誤差較大；改進(jìn)的粒子流濾波雖然也利用了最新的觀測新息，但是作為新息誤差項而引入只涉及b，從而削弱了觀測的影響。而且，BVP方程求解也未引入最新觀測，一定程度上抑制了系統(tǒng)對觀測的依賴性。

圖2 不同噪聲R下的狀態(tài)估計均方根誤差Fig.2 RMSE of state estimation under different noise R

3.2 機(jī)動目標(biāo)跟蹤模型

以單站單目標(biāo)純方位角度跟蹤系統(tǒng)模型為例，本文目標(biāo)的機(jī)動模型為恒速度模型（Constant Velocity，CV）與恒轉(zhuǎn)向模型 （Constant Turn，CT），目標(biāo)的狀態(tài)為X(k)=[xp(k)，xv(k)，yp(k)，yv(k)]T，狀態(tài)的4 個分量分別代表x方向坐標(biāo)、x方向速度、y方向坐標(biāo)、y方向速度。目標(biāo)的狀態(tài)方程如下：

其中：w(k)、v(k)為過程噪聲與測量噪聲，且滿足均值為零，協(xié)方差為Q與R的高斯分布，(x0，y0)為基站坐標(biāo)。

在CV 模型下狀態(tài)轉(zhuǎn)移矩陣為ΦCV，在CT 模型下為ΦCT，目標(biāo)初始狀態(tài)為 X(0)=[1000，10，1500，15]T，w(k)=[wxp(k)，wxv(k)，wyp(k)，wyv(k)]T，各參數(shù)值如下：

當(dāng)粒子數(shù)都為N=50，目標(biāo)初始位置(x0，y0)=（1 355.33，1 736.23）（隨機(jī)取得）。由于粒子濾波存在隨機(jī)性，在上述條件下進(jìn)行200次蒙特卡洛仿真。仿真時長為60 s，采樣周期為1 s，系統(tǒng)距離的噪聲方差為r1=0.05，速度方差為r2=0.1。目標(biāo)開始按照CV 模型運(yùn)動，40 s后按照CT 模型以角速度ω=5°/s 做勻轉(zhuǎn)彎運(yùn)動。選取性能指標(biāo)為目標(biāo)位置均方根誤差，仿真結(jié)果如表2、圖3 所示，其中N1、N2分別表示PF 與改進(jìn)PFF粒子數(shù)。

表2 兩種算法跟蹤性能比較Tab.2 Tracking performance comparison of two algorithms

由表2、圖3 可知在相同條件下，改進(jìn)的粒子流濾波具有更高的跟蹤精度與計算效率。

從表2可以看出，改進(jìn)的粒子流濾波在粒子數(shù)為50時，跟蹤精度已高于粒子濾波在粒子數(shù)為200 時的精度，并且算法運(yùn)行時間更短。這是因?yàn)樵跐M足一定數(shù)值近似誤差要求下，蒙特卡羅近似所需的樣本數(shù)隨著狀態(tài)維度的增加會急劇增加，俗稱維數(shù)災(zāi)難［18］，而且高維狀態(tài)空間的粒子權(quán)值更容易退化。在實(shí)驗(yàn)3.1 節(jié)中，改進(jìn)粒子流濾波運(yùn)行時間更長是因?yàn)樵谔幚硪痪S情況下粒子濾波運(yùn)算效率高，但在處理多維情況下粒子濾波運(yùn)算時間將會大大增加。綜合考慮濾波精度與運(yùn)行時間，改進(jìn)的粒子流算法具有較高的綜合性能。

另外，從表2 可以看出，在過程噪聲變大的情況下，粒子濾波已經(jīng)不能較好地反映真實(shí)的軌跡，誤差已經(jīng)比較大，而改進(jìn)的粒子流濾波依然能相對較好地反映真實(shí)的軌跡，各狀態(tài)估計的誤差保持在相對較小的范圍內(nèi)，證明本文算法具有較好的魯棒性。另外粒子濾波在粒子數(shù)增大的情況下濾波精度有明顯提高，而改進(jìn)的粒子流濾波在粒子數(shù)增加的情況下濾波精度并沒有提高，這是由于改進(jìn)的算法避免了重采樣對粒子的舍棄，而粒子數(shù)較多時，會導(dǎo)致低似然區(qū)域的粒子增加。這也說明了改進(jìn)的粒子流算法適用于粒子數(shù)較少時的高精度快速預(yù)測，例如激光雷達(dá)、紅外目標(biāo)跟蹤等。

圖3 PF和改進(jìn)PFF的仿真結(jié)果比較Fig.3 Comparison of simulation results of PF and improved PFF

4 結(jié)語

本文在粒子流動過程中引入了新息誤差，構(gòu)造了一種與卡爾曼濾波相似的結(jié)構(gòu)。與基于最新觀測信息優(yōu)化粒子濾波的方法相比在一定程度上削弱了對觀測的依賴性，在觀測誤差較大情況下也有相對較好的結(jié)果。由于算法過程中利用粒子流動形式實(shí)現(xiàn)貝葉斯公式以及未進(jìn)行重采樣，故不存在粒子權(quán)值退化問題與粒子多樣性喪失問題。實(shí)驗(yàn)結(jié)果表明，本文提出的改進(jìn)方法，在提高狀態(tài)估計精度的同時，在處理多維情況下運(yùn)算效率更高，具有較高的綜合性能。本文在求解BVP 方程時并未對其解析解進(jìn)行分析，下一步研究方向是如何求得解析解及其滿足的條件以及如何獲得更優(yōu)的數(shù)值解，提高濾波精度。