采用平方根高斯核積分濾波的目標(biāo)跟蹤算法

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

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

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

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

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

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

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

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

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

1.1 問(wèn)題描述

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

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

(1)

zk=h(xk)+rk

(2)

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

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

(3)

(4)

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

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

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

(5)

對(duì)于x是多維隨機(jī)向量的情況,可通過(guò)張量積規(guī)則將單維獲得的Np個(gè)積分點(diǎn)及相應(yīng)權(quán)重?cái)U(kuò)展到多維積分[22]。

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

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

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

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

(6)

式中σ為高斯核帶寬。

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

(7)

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

κω=κI

(8)

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

(9)

(10)

(11)

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

(12)

(13)

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

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

(14)

(15)

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

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

2.2 高斯核積分濾波算法

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

(1)濾波初始化。

(16)

(17)

(18)

傳播采樣點(diǎn)

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

(19)

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

(20)

Pk|k-1=

(21)

(22)

(23)

進(jìn)行采樣點(diǎn)的傳播

(24)

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

(25)

Pzz,k|k-1=

(26)

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

Pxz,k|k-1=

(27)

(28)

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

(29)

(30)

2.3 平方根GKQF

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

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

(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)

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

(37)

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

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

(38)

式中

(39)

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

3 仿真實(shí)驗(yàn)與分析

3.1 一類非線性濾波問(wèn)題

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

(40)

(41)

(42)

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

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

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

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

3.2 純方位目標(biāo)跟蹤問(wèn)題

本文主要考慮目標(biāo)做勻速運(yùn)動(dòng)時(shí)的跟蹤問(wèn)題,所構(gòu)建的目標(biāo)相對(duì)運(yùn)動(dòng)系統(tǒng)方程可表示成

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

(43)

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

(44)

(45)

(46)

式中:Δt為采樣間隔;q為過(guò)程噪聲強(qiáng)度。

觀測(cè)站純方位跟蹤的量測(cè)方程為

(47)

目標(biāo)及觀測(cè)站運(yùn)動(dòng)軌跡如圖3所示,觀測(cè)站的初始位置在(0,0),其在0～11 min和17～30 min作勻速運(yùn)動(dòng),在12～16 min作機(jī)動(dòng)運(yùn)動(dòng)。采樣間隔Δt=1 min,仿真時(shí)長(zhǎng)為30 min。過(guò)程噪聲強(qiáng)度q=10-11km2/s3。

圖3 目標(biāo)及觀測(cè)站運(yùn)動(dòng)軌跡Fig.3 The movement trajectory of target and observing station

(48)

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

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

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

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

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

4 結(jié) 論

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

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

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