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

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

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

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

0 引言

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

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

1 貝葉斯估計(jì)與粒子濾波

1.1 葉斯濾波

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

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

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

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

其中：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)來(lái)近似狀態(tài)后驗(yàn)分布，從而將積分問(wèn)題轉(zhuǎn)換為有限樣本的求和問(wèn)題。然而在實(shí)際計(jì)算中通常無(wú)法直接從后驗(yàn)分布中進(jìn)行采樣，因此需要引入一個(gè)易于采樣的重要性函數(shù)生成采樣樣本，從而利用一組加權(quán)的樣本xk=來(lái)近似后驗(yàn)概率密度：

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

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

1.3 粒子流濾波器

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

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

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

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

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

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

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

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

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

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

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

2.2 求解映射P*與P

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

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

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

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

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

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

引入新息誤差，令：

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

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

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

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

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

2.3 求解BVP方程

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

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

故式（28）可以表示為：

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

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

由式（30）可知：

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

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

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

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

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

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

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

1）如果λ ≤1；

3）計(jì)算新息誤差

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

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

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

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

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

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

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

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

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

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

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

以單站單目標(biāo)純方位角度跟蹤系統(tǒng)模型為例，本文目標(biāo)的機(jī)動(dòng)模型為恒速度模型（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 個(gè)分量分別代表x方向坐標(biāo)、x方向速度、y方向坐標(biāo)、y方向速度。目標(biāo)的狀態(tài)方程如下：

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

在CV 模型下?tīng)顟B(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次蒙特卡洛仿真。仿真時(shí)長(zhǎng)為60 s，采樣周期為1 s，系統(tǒng)距離的噪聲方差為r1=0.05，速度方差為r2=0.1。目標(biāo)開(kāi)始按照CV 模型運(yùn)動(dòng)，40 s后按照CT 模型以角速度ω=5°/s 做勻轉(zhuǎn)彎運(yùn)動(dòng)。選取性能指標(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)的粒子流濾波具有更高的跟蹤精度與計(jì)算效率。

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

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

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

4 結(jié)語(yǔ)

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