基于后驗噪聲的貝葉斯魯棒卡爾曼濾波器*

張楚元，曹 林，趙宗民，王東峰

(1.北京信息科技大學(xué) a.光電測試技術(shù)及儀器教育部重點實驗室；b.通信工程系，北京100101；2.北京川速微波科技有限公司，北京 100101)

0 引 言

最優(yōu)濾波是信號處理的核心問題，想要設(shè)計出最優(yōu)濾波器，就必須對統(tǒng)計模型有全面的了解[1]。然而，實際工程中不可能獲得模型的全部信息。例如，很難得到某一場景下毫米波雷達(dá)觀測噪聲的準(zhǔn)確信息，但可以設(shè)計一個魯棒濾波器來處理這種不確定性。

卡爾曼濾波[2]是一種通過線性系統(tǒng)狀態(tài)方程以及觀測，對系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計的算法。但它對噪聲高度敏感[3]，精確的噪聲知識是影響估計精度的關(guān)鍵。最近，Dehghannasiri等人[4]利用貝葉斯創(chuàng)新過程和貝葉斯正交性原理，提出了一種本質(zhì)上的貝葉斯魯棒卡爾曼濾波器(Intrinsically Bayesian Robust Kalman Filter,IBRKF)，它相對于成本函數(shù)和不確定噪聲的先驗分布是最優(yōu)的，且該濾波器的遞歸方程與經(jīng)典卡爾曼濾波器的遞歸方程相似。IBRKF定義了有效噪聲統(tǒng)計量和有效卡爾曼增益的概念，并用它們來代替經(jīng)典卡爾曼濾波器中對應(yīng)的變量，以此改善性能。IBRKF是經(jīng)典卡爾曼濾波器相對于不確定噪聲的擴(kuò)展版本。

盡管IBRKF的仿真實驗證明了其性能的優(yōu)越性，但其優(yōu)越的性能僅建立在對先驗知識的了解上，忽略了噪聲信息可依托系統(tǒng)而傳遞這一關(guān)鍵因素。因此，如何有效地從觀測中提取噪聲信息是進(jìn)一步提升估計精度的基石。

本文針對不確定觀測噪聲的問題，將IBRKF框架擴(kuò)展到結(jié)合觀測序列的后驗版本，且所提算法的最終公式與經(jīng)典卡爾曼濾波算法相似。該算法首先尋找與先驗分布最為相似的提議分布，再為獲得后驗噪聲分布的期望而設(shè)計了一種方法，借此使濾波算法不停地修正噪聲參數(shù)，實現(xiàn)更精確的狀態(tài)估計。通過對毫米波雷達(dá)實測數(shù)據(jù)的處理表明，該算法在應(yīng)對不確定觀測噪聲時具有優(yōu)異的性能。

1 貝葉斯濾波器

貝葉斯準(zhǔn)則的含義是找到平均代價最小的濾波器。通常，在狀態(tài)估計中，均方誤差被用來評價濾波器性能的好壞，而優(yōu)化濾波器的性能即最小化代價函數(shù)

C(xk,φ(yk))=E[(xk-φ(yk))T(xk-φ(yk))]。

(1)

因此，可將因果關(guān)系轉(zhuǎn)化為根據(jù)最小化的代價函數(shù)找到一個最優(yōu)的濾波器，即

(2)

式中：Ψ是所有可能的濾波器的類型。滿足上式的濾波器即為最小均方差濾波器。

盡管IBRKF達(dá)到了先驗意義上的最優(yōu)，但先驗噪聲期望與實際噪聲仍存在一定的誤差，無法實現(xiàn)真正意義上的最優(yōu)濾波。故本文重點考察后驗噪聲分布對成本函數(shù)的影響，通過提取觀測中蘊含的有效噪聲信息，進(jìn)一步提升貝葉斯魯棒濾波器的性能。

假設(shè)模型由未知參數(shù)ω=[ω1,ω2,…,ωl]控制，ω∈Ω，Ω為所有可能的參數(shù)的集合。此平均代價最小的貝葉斯魯棒濾波器可表示為

(3)

2 貝葉斯魯棒卡爾曼濾波器

假設(shè)觀測噪聲的協(xié)方差矩陣是未知的，由未知參數(shù)ω決定，即

(4)

并令π(ω)作為控制ω的先驗分布，則狀態(tài)空間模型可被參數(shù)化為

xk+1=Φkxk+Γkuk，

(5)

(6)

以式(3)的角度分析，滿足此貝葉斯魯棒濾波器定義的線性濾波函數(shù)為

(7)

式中：

(8)

卡爾曼遞歸方程的推導(dǎo)基于以下的定理、定義、命題和引理：

定理1[4]：(貝葉斯正交原則)如果一個線性濾波器的權(quán)重函數(shù)滿足式(8)，則式(7)中得到的估計量被稱為最優(yōu)貝葉斯最小二乘估計，當(dāng)且僅當(dāng)

(9)

(10)

命題1[4]：根據(jù)貝葉斯創(chuàng)新過程，可得如下的方程：

(11)

(12)

據(jù)此，可將式(7)改寫為

(13)

然后，將式(13)代入式(12)可得

(14)

則狀態(tài)更新方程可表示如下：

(15)

式中：

(16)

(17)

式中：Q=E[uk(uk)T]為過程噪聲協(xié)方差矩陣。

3 有效噪聲的提取

定理2：①如果x服從高斯分布，對x進(jìn)行線性變換后，新的變量仍服從高斯分布。②如果高斯分布x、v統(tǒng)計獨立，則它們的和仍服從高斯分布。

(18)

在獲得不確定參數(shù)的似然函數(shù)后，利用Metropolis Hastings算法采樣得到后驗噪聲的樣本集，并將樣本均值當(dāng)作k時刻后驗觀測噪聲的期望。其中，樣本選擇采用的是接受-拒絕準(zhǔn)則。假設(shè)ω(j)為第j次迭代結(jié)束時所生成樣本序列的最后一個樣本，在j+1次迭代時，將從提議分布q(ωcand,ω(j))中抽取一個候選值ωcand，這個候選值通過計算接受率而被接受或者拒絕。接受率的表達(dá)式如下：

(19)

Fréchet距離的物理意義是空間中兩條連續(xù)曲線變化趨勢相似性的度量，而離散Fréchet距離則是將空間中的曲線看作由多個有序點構(gòu)成的多邊形，再通過匹配兩條曲線的各個端點，尋找最長鏈接的最小值。

令空間中兩條被多邊形化的曲線為F:{c1,c2,…,cp}和G:{h1,h2,…,hq}，L為兩曲線逐點匹配所構(gòu)成的鏈接序列，表示為

(cf1,hg1),(cf2,hg2),…(cfm,hgm)。

(20)

式中：f1=1,g1=1,fm=p,gm=q，且對于i=1,2,…,q，必須有fi+1=fi或fi+1=fi+1；gi同理，以此保證曲線中各端點的順序關(guān)系。定義兩曲線各匹配點的距離的最大值為

(21)

式中：d(·)表示兩點間的歐氏距離。則F、G間的離散Fréchet距離可表示為[5]

ddF(F,G)=min{‖L‖|L為F、G間鏈接序列}。

(22)

由于文獻(xiàn)[6]指出，當(dāng)以離散Fréchet距離衡量兩曲線的相似性且取到峰值點時，則其中一條曲線的某個峰值點只可能與另一條曲線對應(yīng)的峰值點或其周圍的幾個峰值點相關(guān)，否則兩曲線不可能相似。又因為概率密度函數(shù)體現(xiàn)了隨機(jī)變量在某個確定點附近出現(xiàn)的概率，所以在尋找合適的提議分布時，可只取提議分布和先驗分布波峰部分的點，使得鏈接序列至少含有一個峰值點，以此種方式壓縮搜索空間，提升所尋提議分布的準(zhǔn)確性。

基于提議分布與先驗分布的離散Fréchet距離，可采用改進(jìn)的隨機(jī)游走算法尋找距離的最小值，并將距離最小時的提議分布作為最優(yōu)解輸出。采用改進(jìn)后的算法是由于基礎(chǔ)的隨機(jī)游走算法依賴初始迭代點的設(shè)置，當(dāng)經(jīng)驗不夠可靠時容易使算法陷入局部最優(yōu)。雖然增大迭代次數(shù)和初始步長可在一定程度上對這種問題進(jìn)行改善，但這將造成時間花費的提升。因此，本文將離散Fréchet距離考慮為目標(biāo)函數(shù)，尋找最優(yōu)提議分布的過程如下：

Step1 按經(jīng)驗設(shè)置初始提議分布N(μ,θ(0))，定義初始步長ζ和控制精度ε，計算初始提議分布與先驗分布的離散Fréchet距離D。

Step2 給定迭代次數(shù)N，并令當(dāng)前迭代次數(shù)n=1。

Step3 若n

θ(n)=θ(n-1)+ζ·min{θj},1≤j≤m。

(23)

Step4 計算新提議分布N(μ,θ(n))與先驗分布的離散Fréchet距離D′，若D′D，則拒絕接受新的提議分布為最優(yōu)解，并令n=n+1，返回Step 3。

Step5 若連續(xù)N次迭代都找不到更優(yōu)的離散Fréchet距離，則認(rèn)為最優(yōu)解在以當(dāng)前最優(yōu)解為圓心、半徑為步長ζ的圓內(nèi)。此時，若ζ<ε，則終止算法；否則，令ζ=ζ/2，返回Step 2。

4 實驗與性能分析

4.1 實驗場景

本文針對真實場景下的測量數(shù)據(jù)進(jìn)行了實驗分析，實驗的對象是人。實驗情況描述如下：在室內(nèi)劃定175 cm×450 cm的區(qū)域，并在實驗區(qū)域的底部放置毫米波雷達(dá)。以雷達(dá)的位置為坐標(biāo)原點(僅考慮二維空間的情況)，利用雷達(dá)在沒有任何目標(biāo)的情況下對實驗區(qū)域中的環(huán)境進(jìn)行采樣，并以此為基準(zhǔn)濾除非目標(biāo)點。數(shù)據(jù)采集時，一個人以近似勻速的運動方式在實驗區(qū)域內(nèi)按預(yù)定的軌跡走動，如圖1所示。毫米波雷達(dá)安裝位置如圖2所示。在獲得測量點后，對目標(biāo)點進(jìn)行聚類預(yù)處理，并以聚類中心為目標(biāo)的觀測位置。預(yù)處理結(jié)束后，利用三種不同的卡爾曼濾波器對這些位置數(shù)據(jù)進(jìn)行濾波處理并分析誤差。

圖2 毫米波雷達(dá)安裝位置

4.2 濾波軌跡

在濾波處理中，使用的濾波器分別為經(jīng)典卡爾曼濾波器、IBRKF和本文所提的算法。根據(jù)所使用的雷達(dá)型號和多次測量的經(jīng)驗可知觀測噪聲的方差服從均勻分布。上述三種卡爾曼濾波器的濾波結(jié)果如圖3所示，其中圖3(a)代表本文所提出的算法，圖3(b)代表IBR方法，圖3(c)代表經(jīng)典卡爾曼濾波算法。從圖3可以明顯看出，本文所提算法的濾波軌跡優(yōu)于其他兩種方法，更接近目標(biāo)的真實軌跡。另外，經(jīng)典卡爾曼濾波算法由于狀態(tài)空間模型的不精確而導(dǎo)致的噪聲積累會對濾波的穩(wěn)定性產(chǎn)生影響，而IBRKF和本文所提算法都利用了有效信息來削弱這種影響，使得它們的魯棒性比經(jīng)典算法更強(qiáng)。

(a)本文所提算法

4.3 位置誤差

以目標(biāo)估計位置與真實位置之間的位置誤差(Position Error,PE)為算法評價標(biāo)準(zhǔn)之一，其計算公式如下：

(24)

式中：xtrue、yture表示目標(biāo)的真實坐標(biāo)，xes、yes表示由濾波器濾波后目標(biāo)的估計坐標(biāo)。從圖4可以看出，盡管所設(shè)計濾波器的PE在某些時刻超過了IBRKF和經(jīng)典卡爾曼濾波器，但從整體上觀察，本文所提算法PE曲線大部分都位于其他兩種濾波器的下方，誤差也相對集中。而通過計算三種濾波器的平均PE(分別為4.614 7 cm、8.147 0 cm和9.892 8 cm)，也可看出本文所提算法整體上的估計位置與真實位置間的誤差更小。

4.4 均方根誤差

分別計算了三種算法的均方根誤差(Root Mean Square Error,RMSE)，對三種算法的性能進(jìn)行直觀地比較。從圖5可以看出，本文所提算法的RMSE明顯低于其他兩種算法。另外，三種算法RMSE的平均值(Mean of RMSE,MMSE)也清晰地顯示出它們?yōu)V波性能的優(yōu)劣。其中，經(jīng)典卡爾曼濾波器的MMSE為11.406 7 cm，IBRKF的MMSE為9.516 1 cm，本文所提算法的MMSE為5.184 8 cm。

圖5 三種算法的RMSE

4.5 時間花費

表1是不同迭代次數(shù)下，尋找最優(yōu)提議分布和整體算法的時間花費情況。其中，ε=0.000 1,算法運行的平臺是Matlab R2018b，處理器為1.40 GHz的Intel(R) Core(TM) i5-8257 CPU和8 GB RAM。根據(jù)實驗數(shù)據(jù)可以看出，隨著迭代次數(shù)增加，尋優(yōu)耗時也將成倍增長，但最優(yōu)解的變化不大。而由于尋優(yōu)過程與濾波是串行的關(guān)系，且尋優(yōu)過程在整體算法中只執(zhí)行一次，因此迭代次數(shù)的增加并不會影響后續(xù)每一時刻的濾波時間。故在某些情況下，可通過控制迭代次數(shù)和適當(dāng)增大控制精度來提升整體算法的實時性。

表1 不同迭代次數(shù)下的尋優(yōu)耗時和算法總耗時

5 結(jié)束語

本文針對毫米波雷達(dá)測量時觀測噪聲不確定的問題，從觀測中提取有效噪聲信息，設(shè)計了一種新型的貝葉斯魯棒卡爾曼濾波器，并提出了一種提議分布的自適應(yīng)選取方法。實驗證明了與經(jīng)典和最新算法相比，所提算法在處理不確定觀測噪聲時性能突出。但目前仍存在一些問題，例如在面對非高斯噪聲時，所提算法的性能會大幅降低，且設(shè)計時忽略了過程噪聲的轉(zhuǎn)移，原因是過程噪聲與狀態(tài)變量不獨立，需要通過大量的數(shù)據(jù)分析來獲得它們的相關(guān)系數(shù)，但此方法無法保證算法的準(zhǔn)確性和實時性。下一步研究將把過程噪聲加入該濾波框架，進(jìn)一步提升所提濾波器的性能，并為計算后驗噪聲期望設(shè)計更有效、更便捷的方法。