汪家寶,陳樹新,吳昊,2,何仁珂,徐涵

(1.空軍工程大學(xué)信息與導(dǎo)航學(xué)院,710077,西安;2.地理信息工程國家重點實驗室,710054,西安;3.93184部隊,100076,北京;4.95655部隊,611530,成都)

非線性濾波和估計問題存在于信號處理、目標跟蹤(例如水下跟蹤、飛行器監(jiān)視等)、組合導(dǎo)航等諸多領(lǐng)域[1-4]。目標跟蹤是指利用相應(yīng)傳感器獲得測量信號以完成對目標狀態(tài)的連續(xù)迭代估計的過程,其中純方位無源跟蹤[5-6]能夠被動地利用一系列角度信息完成目標的跟蹤。由于其高非線性以及弱可觀測性,對采用的非線性濾波估計算法要求較高。

目前,為了實現(xiàn)對非線性系統(tǒng)的狀態(tài)估計,基于貝葉斯框架和高斯密度假設(shè)的高斯近似濾波得到了深入的研究,其核心是計算形如“非線性函數(shù)×高斯概率密度函數(shù)”的多維積分[7]。其中,擴展卡爾曼濾波[8]利用泰勒級數(shù)展開獲得系統(tǒng)方程的近似表達,但在系統(tǒng)非線性程度較高時會引來較大的截斷誤差?；凇皩Ω怕史植歼M行近似要比對非線性函數(shù)近似容易”的認識,無跡卡爾曼濾波(UKF)[9-10]、高斯-厄米特積分濾波(GHQF)[11-12]、容積卡爾曼濾波(CKF)[13-14]等算法相繼被提出,它們通過確定性采樣來近似系統(tǒng)狀態(tài)的后驗概率密度,區(qū)別在于數(shù)值積分規(guī)則有所不同,分別采用無跡變換、高斯-厄米特積分規(guī)則以及容積準則。

考慮從數(shù)值逼近的角度提高估計精度,文獻[15]推導(dǎo)了5階球面-相徑容積準則,建立了高階CKF算法(HCKF),其比3階CKF擁有更高的估計精度。Wang等采用5階球面單純形準則計算球面積分,提出了5階球面單純形-相徑容積卡爾曼濾波[16],相比于HCKF可進一步提高估計精度。Bhaumik等將容積準則與高斯-拉蓋爾積分規(guī)則結(jié)合,提出容積積分卡爾曼濾波(CQKF)[17]。CQKF是CKF的廣義形式,當采用的切比雪夫-拉蓋爾多項式階數(shù)d≥2時,其精度高于CKF。GHQF采用高斯-厄米特積分(GHQ)規(guī)則進行數(shù)值逼近,可獲得更高的估計精度,但GHQF運用張量積規(guī)則將單變量高斯積分擴展到多維積分,其計算復(fù)雜度隨系統(tǒng)維數(shù)指數(shù)增長。事實上,隨著各個領(lǐng)域?qū)V波精度的要求越來越高,如何進一步提高數(shù)值積分精度需要深入研究。

文獻[18]推導(dǎo)了一種精確且數(shù)值穩(wěn)定的高斯核積分權(quán)重近似值,該近似是建立在縮放的高斯-厄米特積分節(jié)點的基礎(chǔ)上,但該數(shù)值積分方法并未系統(tǒng)地運用于高斯濾波過程。

因此,本文利用高斯核積分規(guī)則結(jié)合張量積方法推導(dǎo)了高斯核積分濾波算法(GKQF),該算法通過選定與高斯-厄米特積分節(jié)點成比例的積分點,采用高斯核積分規(guī)則計算出相應(yīng)權(quán)重近似值,形成基于比例高斯-厄米積分點的高斯核積分規(guī)則,并與GHQF算法同樣利用張量積方法實現(xiàn)多維積分,在運算量接近的情況下其能夠獲得比GHQF更好的濾波效果,并且可結(jié)合實際情況靈活調(diào)整參數(shù)。

同時,為了增強GKQF算法的數(shù)值穩(wěn)定性,如同UKF、GHQF以及CKF的平方根版本[19-21]采用QR分解代替Cholesky分解,確保了協(xié)方差矩陣的半正定性從而改進數(shù)值的穩(wěn)定性,本文推導(dǎo)了平方根高斯核積分濾波算法(SGKQF)。通過對典型二維非線性濾波系統(tǒng)與純方位目標跟蹤實例的仿真實驗,驗證了GKQF以及SGKQF算法相較于UKF、CKF、GHQF等傳統(tǒng)算法具有更高的估計性能。

1 問題描述和高斯-厄米特積分規(guī)則

1.1 問題描述

考慮非線性離散時間狀態(tài)空間模型

xk=f(xk-1)+wk-1

(1)

zk=h(xk)+rk

(2)

式中:xk∈nx和zk∈nz分別表示k時刻的狀態(tài)向量和量測向量;f(·)和h(·)分別表示非線性系統(tǒng)的狀態(tài)函數(shù)和測量函數(shù);過程噪聲wk和量測噪聲rk是互不相關(guān)的均值為0的高斯白噪聲,方差分別為Qk-1和Rk。

高斯濾波的前提是假設(shè)濾波分布近似服從高斯分布,狀態(tài)xk的后驗概率密度滿足

(3)

(4)

式中:Px=SST;Np為積分點數(shù);ξl和ωl分別為隨機變量y滿足概率密度p(y)=N(y;0,Inx)時的積分點和相應(yīng)權(quán)值,可根據(jù)GHQ規(guī)則、無跡變換或者容積準則來進行選取。

1.2 高斯-厄米特積分規(guī)則

對于具有高斯密度N(x;0,1)的標量x,采用數(shù)值近似獲得非線性函數(shù)g(x)的期望,可通過高斯-厄米特積分規(guī)則計算ξl和ωl。傳統(tǒng)的方法采用矩匹配法確定積分點和權(quán)重,但其計算較為復(fù)雜,一種簡便方法是利用正交多項式和三對角矩陣之間的關(guān)系[11],假定J是一個具有0對角元素的對稱三對角矩陣,其他元素計算式為

(5)

對于x是多維隨機向量的情況,可通過張量積規(guī)則將單維獲得的Np個積分點及相應(yīng)權(quán)重擴展到多維積分[22]。

2 平方根高斯核積分濾波器

2.1 基于比例高斯-厄米特節(jié)點的高斯核積分規(guī)則

本文選取帶比例因子的高斯-厄米特積分點,通過高斯核構(gòu)造的線性方程組計算相應(yīng)的核積分權(quán)重近似值,兩者構(gòu)成單變量高斯核積分規(guī)則的積分點與相應(yīng)權(quán)重,再將獲得的高斯核積分規(guī)則利用張量積方法從單變量擴展到多變量形式,使之適應(yīng)多維積分的數(shù)值近似。

首先介紹運用高斯核技巧的高斯核積分規(guī)則。已知x,y∈,高斯核κ(x,y)定義為[23]

(6)

式中σ為高斯核帶寬。

對于給出的互異積分點ξ=[ξ1,…,ξi,…,ξN]T,考慮函數(shù)f的數(shù)值積分形式[18]

(7)

如果積分權(quán)重ω=[ω1,…,ωi,…,ωN]T是通過線性關(guān)系計算出,則該數(shù)值積分規(guī)則稱為高斯核積分規(guī)則。具體地,線性關(guān)系式為

κω=κI

(8)

單變量高斯核積分點ξ的一個特殊選擇是單變量高斯-厄米特積分點的比例形式,比例因子的選擇以及積分權(quán)重ω的計算此處不進行詳細推導(dǎo),直接給出引理。

(9)

(10)

(11)

式(11)中的相關(guān)參數(shù)可定義為

(12)

(13)

引理1可用于單變量數(shù)值積分近似,通過該方法獲得的高斯核積分點以及相匹配的積分權(quán)重構(gòu)成了本文所提算法的基礎(chǔ),可稱為基于比例高斯-厄米特節(jié)點的高斯核積分規(guī)則,簡稱為高斯核積分規(guī)則。

將單變量高斯核積分擴展為多變量積分的方法可利用張量積方法。張量積方法變換后的形式與多維高斯-厄米特積分相似,僅采用的積分點和權(quán)重不同,有

(14)

(15)

式中L=(Np)nx為多變量高斯核積分點數(shù)。

將張量積方法擴維后的高斯核積分點及相應(yīng)權(quán)值置于高斯濾波框架之下獲得的非線性濾波方法稱為高斯核積分濾波GKQF算法。

2.2 高斯核積分濾波算法

與一般的高斯近似濾波相似,GKQF算法主要通過時間更新和量測更新兩個步驟來實現(xiàn)。濾波過程如下。

(1)濾波初始化。

(16)

(17)

(18)

傳播采樣點

Xl,k|k-1=f(xl,k-1|k-1)

(19)

計算預(yù)測狀態(tài)和預(yù)測協(xié)方差矩陣

(20)

Pk|k-1=

(21)

(22)

(23)

進行采樣點的傳播

(24)

計算預(yù)測量測及新息協(xié)方差矩陣

(25)

Pzz,k|k-1=

(26)

估計互協(xié)方差矩陣并計算濾波增益

Pxz,k|k-1=

(27)

(28)

k時刻的狀態(tài)估計與估計協(xié)方差

(29)

(30)

2.3 平方根GKQF

GKQF算法存在協(xié)方差矩陣的平方根分解,為了提高濾波的數(shù)值穩(wěn)定性,可采用QR分解來代替?zhèn)鹘y(tǒng)的Cholesky分解,這樣便形成了平方根高斯核積分濾波算法。具體的實現(xiàn)方法如下。

(1)預(yù)測協(xié)方差平方根Sk|k-1被直接計算以進行積分點的傳播,避免每一步運行時對Pk|k-1計算并進行因式分解。具體地,Sk|k-1計算式為

(31)

式中qr(·)表示QR分解,且有

(32)

(33)

利用式(31)替換式(21)(22)。

(2)新息協(xié)方差矩陣Pzz,k|k-1采用平方根形式

Szz,k|k-1=qr([Zk|k-1,SRk])

(34)

式中

(35)

(36)

進而,濾波增益調(diào)整為

(37)

(3)誤差協(xié)方差平方根Sk-1|k-1被直接計算以避免對Pk-1|k-1計算并進行因式分解,即

Sk|k=qr([χk|k-1-WkZk|k-1,WkSRk])

(38)

式中

(39)

利用式(38)替換式(17)(30)。

3 仿真實驗與分析

3.1 一類非線性濾波問題

為驗證本文所提算法的有效性,考慮典型二維非線性系統(tǒng)

(40)

(41)

(42)

圖1 不同σ下GKQF的平均均方根誤差Fig.1 The eARMSE of GKQF with different σ

其次,將本文所提的GKQF、SGKQF算法與CKF、GHQF算法進行對比說明。各算法濾波精度仿真結(jié)果如圖2所示。可以看出:當σ=0.4時,GKQF和SGKQF算法相較于UKF、CKF、GHQF具有更高的估計精度;當σ=10時,GKQF與GHQF均方根誤差相接近,這與σ→∞時,GKQF等價于GHQF這一特性具有一致性?？梢愿爬?當σ∈[0.4,10]時,GKQF可取得較GHQF更好的估計性能。

圖2 各算法在狀態(tài)1時的均方根誤差比較Fig.2 The eRMSE comparison of different algorithms under state 1

綜合可知,GKQF算法的精度隨著高斯核帶寬σ(從正無窮起始,當σ→∞,GKQF等價于GHQF)的減小而增加,但隨著σ減小至一定數(shù)值后,誤差急劇上升。因此,在仿真實驗過程中可以先選取較大的高斯核帶寬,通過向下逼近以獲得更高的估計精度。

3.2 純方位目標跟蹤問題

本文主要考慮目標做勻速運動時的跟蹤問題,所構(gòu)建的目標相對運動系統(tǒng)方程可表示成

Xk=f(Xk-1,uk-1)+wk-1=FXk-1-uk-1+wk-1

(43)

式中:Xk∈nx;nx為狀態(tài)向量維數(shù);過程噪聲wk-1是滿足均值為0協(xié)方差為Qk-1的高斯白噪聲;轉(zhuǎn)移矩陣F、確定性輸入uk-1以及Qk-1的表達式分別為

(44)

(45)

(46)

式中:Δt為采樣間隔;q為過程噪聲強度。

觀測站純方位跟蹤的量測方程為

(47)

目標及觀測站運動軌跡如圖3所示,觀測站的初始位置在(0,0),其在0～11 min和17～30 min作勻速運動,在12～16 min作機動運動。采樣間隔Δt=1 min,仿真時長為30 min。過程噪聲強度q=10-11km2/s3。

圖3 目標及觀測站運動軌跡Fig.3 The movement trajectory of target and observing station

(48)

圖4和圖5分別記錄了各算法位置和速度均方根誤差。其中,SGKQF算法分別選取高斯核帶寬σ=2.5,3,3.5?？梢钥闯?所提算法在σ=3附近可使純方位目標跟蹤的位置和速度誤差達到最小,其濾波效果優(yōu)于UKF、CKF、HCKF以及GHQF。僅考慮跟蹤過程后20 min內(nèi)的平均均方根誤差,當高斯核帶寬取為3時,相比于GHQF,SGKQF算法的位置和速度估計精度分別提高了8.7%和11.8%。從這兩個例子可以發(fā)現(xiàn),基于高斯核積分規(guī)則形成的GKQF和SGKQF算法具有更強的估計性能。

圖4 各算法的位置均方根誤差比較Fig.4 The eRMSEpos comparison of algorithms

圖5 各算法的速度均方根誤差比較Fig.5 The eRMSEvel comparison of algorithms

表1給出了各算法的積分點數(shù)與相對運算時間,相對運算時間以CKF為基準,系統(tǒng)狀態(tài)維數(shù)n=4。在運算復(fù)雜度方面,與CKF、UKF以及HCKF算法相比較,GHQF和SGKQF需要更多計算時間。SGKQF和GHQF的采樣點數(shù)均為3n,其計算代價相近。SGKQF相較于GHQF在沒有增加較大運算復(fù)雜度的前提下提高了目標跟蹤精度,且能夠通過調(diào)整高斯核帶寬來適應(yīng)不同的應(yīng)用需求,靈活性更高。

表1 各算法的積分點數(shù)與相對運算時間Table 1 Quadrature points number and relative computation time of algorithms

4 結(jié) 論

本文以提高非線性系統(tǒng)濾波估計精度為目的,首先構(gòu)造與高斯-厄米特積分節(jié)點成比例的積分點,采用高斯核構(gòu)造的線性方程組計算出相應(yīng)近似權(quán)重,建立了單變量高斯核積分規(guī)則。再利用張量積方法將其擴展為多維數(shù)值積分,并推導(dǎo)了高斯核積分非線性濾波的平方根形式,即SGKQF算法。通過仿真實驗可得到以下結(jié)論。

(1)與GHQF算法相比,SGKQF算法能夠在同等計算復(fù)雜度下獲得更高的目標跟蹤精度,且靈活性更強,體現(xiàn)了高斯核積分規(guī)則的優(yōu)越性。同時,所提算法有望提高目標跟蹤、信息融合等應(yīng)用領(lǐng)域所涉及的非線性濾波的精度,能夠獲得更為精確的狀態(tài)估計。

(2)在實際應(yīng)用中,可以考慮采用高斯核帶寬的經(jīng)驗取值,也可以提前進行實驗測試獲得更為準確的高斯核帶寬以進一步提高濾波精度。此外,如何采用自適應(yīng)方法來實時更新高斯核帶寬,以及如何在高維非線性系統(tǒng)中降低算法計算復(fù)雜度也是未來的探索方向。