蔡宗平， 于澤祥， 楊 劍

(火箭軍工程大學(xué)，西安 710025)

0 引言

在許多實(shí)際問(wèn)題中，目標(biāo)跟蹤是一種非線性[1]濾波問(wèn)題，由于非線性系統(tǒng)通常無(wú)法得到解析遞推表達(dá)式，所以處理該類問(wèn)題必須采用近似方法。

1971年，文獻(xiàn)[2]通過(guò)泰勒展開(kāi)的方式將非線性系統(tǒng)近似為線性系統(tǒng)，提出擴(kuò)展卡爾曼濾波(EKF)算法，在工程上容易實(shí)現(xiàn)。但針對(duì)強(qiáng)非線性系統(tǒng)，該算法的一階線性化近似精度較低[3]。文獻(xiàn)[4]通過(guò)無(wú)跡變換的采樣點(diǎn)集來(lái)逼近非線性函數(shù)概率分布，提出無(wú)跡卡爾曼濾波(UKF)算法，較EKF具有更好的非線性估計(jì)性能，但其性能易受參數(shù)取值的影響，且在狀態(tài)維數(shù)較高時(shí)，會(huì)出現(xiàn)濾波精度下降甚至發(fā)散的現(xiàn)象[5-6]。為此，文獻(xiàn)[7]提出容積卡爾曼濾波(Cubature Kalman Filter,CKF)算法，利用三階球面-徑向容積準(zhǔn)則計(jì)算非線性近似中的概率積分問(wèn)題。相比于UKF算法，CKF算法在高維狀態(tài)下?lián)碛懈叩臄?shù)值穩(wěn)定性和濾波精度[8-9]，然而球面容積公式的固有缺陷[10]限制了其精度和高階擴(kuò)展性。2013年，張?chǎng)未旱葘W(xué)者利用嵌入式容積準(zhǔn)則計(jì)算非線性近似中的概率積分問(wèn)題，提出嵌入式容積卡爾曼濾波(ECKF)[11]算法，實(shí)現(xiàn)了更高精度的濾波估計(jì)，并且有較好的高階擴(kuò)展性[12]。

事實(shí)上，上述算法在解決非線性濾波問(wèn)題時(shí)，需要已知系統(tǒng)的噪聲統(tǒng)計(jì)特性[13]，然而在實(shí)際情況中，系統(tǒng)噪聲的統(tǒng)計(jì)特性往往是時(shí)變未知的，這是因?yàn)橄到y(tǒng)噪聲易受外界干擾、濾波對(duì)象的物理特性等因素的影響。

本文以傳統(tǒng)的Sage-Husa算法[14]為基礎(chǔ)，結(jié)合嵌入式容積規(guī)則推導(dǎo)出適用于非線性條件下的噪聲估計(jì)器，提出自適應(yīng)嵌入式容積卡爾曼濾波(AECKF)算法，以實(shí)現(xiàn)實(shí)時(shí)估計(jì)和修正噪聲統(tǒng)計(jì)特性的目的，并且采用一種基于協(xié)方差匹配判據(jù)[15]的方法對(duì)濾波發(fā)散的情況進(jìn)行判斷，從而抑制濾波中的發(fā)散現(xiàn)象，增強(qiáng)濾波器的魯棒性。理論分析和實(shí)驗(yàn)結(jié)果都表明，本文提出的自適應(yīng)ECKF濾波算法的估計(jì)性能和穩(wěn)定性要明顯優(yōu)于標(biāo)準(zhǔn)ECKF濾波算法。

1 系統(tǒng)描述

考慮一般非線性系統(tǒng)

(1)

式中：xk∈Rn為系統(tǒng)的狀態(tài)向量；zk∈Rp為系統(tǒng)的觀測(cè)向量；fk-1(·)為k-1時(shí)刻到k時(shí)刻的狀態(tài)轉(zhuǎn)移函數(shù)；hk(·)為量測(cè)函數(shù)；wk-1為統(tǒng)計(jì)特性未知的獨(dú)立高斯系統(tǒng)噪聲序列；vk為統(tǒng)計(jì)特性已知的獨(dú)立高斯觀測(cè)噪聲序列。wk-1，vk具有的統(tǒng)計(jì)特性為

(2)

式中，δkj為克羅內(nèi)克函數(shù)。

2 自適應(yīng)嵌入式容積卡爾曼濾波(AECKF)算法

2.1 嵌入式容積卡爾曼濾波(ECKF)算法

傳統(tǒng)的CKF算法采用三階球面-徑向容積準(zhǔn)則，當(dāng)狀態(tài)空間的維數(shù)較大時(shí)，會(huì)出現(xiàn)容積點(diǎn)超出定義區(qū)間，或容積點(diǎn)為復(fù)數(shù)等情況，導(dǎo)致濾波發(fā)散。同時(shí)，若需構(gòu)造高階CKF濾波器，則需同時(shí)提高球面容積準(zhǔn)則和高斯-拉蓋爾積分公式的階數(shù)，計(jì)算過(guò)程十分復(fù)雜。針對(duì)傳統(tǒng)三階CKF算法的不足，張?chǎng)未旱葘W(xué)者利用嵌入式容積準(zhǔn)則計(jì)算非線性近似中的概率積分問(wèn)題，有效地解決了傳統(tǒng)CKF算法誤差隨狀態(tài)空間維數(shù)呈線性增長(zhǎng)的缺點(diǎn)。

對(duì)于n維狀態(tài)，采用N=2n+1個(gè)容積點(diǎn)實(shí)現(xiàn)積分的數(shù)值逼近，即

IN(f)

(3)

式中，相應(yīng)的容積點(diǎn)和權(quán)值為

(4)

(5)

2.2 非線性噪聲統(tǒng)計(jì)估計(jì)器

文獻(xiàn)[14]基于卡爾曼濾波框架推導(dǎo)出了常規(guī)Sage-Husa噪聲估計(jì)算法，并用來(lái)解決線性條件下噪聲統(tǒng)計(jì)特性未知的濾波問(wèn)題，當(dāng)系統(tǒng)為非線性時(shí)該算法不再適用。本文在此基礎(chǔ)上對(duì)傳統(tǒng)的Sage-Husa算法進(jìn)行改進(jìn)，采用嵌入式容積準(zhǔn)則得到可以應(yīng)用于ECKF的噪聲統(tǒng)計(jì)估計(jì)器。

在常值噪聲條件下，對(duì)于式(1)描述的非線性系統(tǒng)，基于Sage-Husa算法的系統(tǒng)噪聲均值估計(jì)算式為

(6)

結(jié)合標(biāo)準(zhǔn)ECKF濾波算法得到嵌入式容積規(guī)則下系統(tǒng)噪聲均值的估計(jì)算式為

(7)

而在常規(guī)Sage-Husa算法中系統(tǒng)噪聲協(xié)方差的遞推公式為

(8)

可得嵌入式容積規(guī)則下系統(tǒng)噪聲協(xié)方差矩陣的遞推公式為

(9)

當(dāng)系統(tǒng)噪聲的統(tǒng)計(jì)特性為時(shí)變未知時(shí)，利用漸消記憶指數(shù)加權(quán)法得到時(shí)變?cè)肼暯y(tǒng)計(jì)估計(jì)器為

(10)

(11)

式中,dk=(1-b)/(1-bk)，b為遺忘因子，通常取值范圍為0.95

2.3 防止算法發(fā)散的改進(jìn)

解決目標(biāo)跟蹤問(wèn)題時(shí)常常會(huì)因?yàn)闉V波環(huán)境發(fā)生變化導(dǎo)致濾波發(fā)散，同時(shí)，雷達(dá)環(huán)境下的目標(biāo)檢測(cè)通常會(huì)產(chǎn)生一定比例的野值，在濾波時(shí)如果不進(jìn)行甄別并剔除，也會(huì)造成濾波結(jié)果不穩(wěn)定甚至發(fā)散等問(wèn)題。

針對(duì)這些問(wèn)題，在算法中引入判斷機(jī)制，通過(guò)實(shí)際余項(xiàng)與理論余項(xiàng)的相互關(guān)系來(lái)判斷濾波狀態(tài)是否發(fā)散,即

(12)

式中：ε(k)為殘差序列；γ為事先設(shè)定的儲(chǔ)備系數(shù)，γ≥1；tr為矩陣的跡。

由濾波原理可知

(13)

蔡宗平等： 基于自適應(yīng)嵌入式CKF的目標(biāo)跟蹤算法把式(13)代入式(12)，發(fā)散判據(jù)變?yōu)?/p>

(14)

若式(14)成立，說(shuō)明濾波器工作正常；若該式不成立，則說(shuō)明濾波器可能發(fā)散，此時(shí)直接跳過(guò)k時(shí)刻的狀態(tài)，依據(jù)k-1時(shí)刻的狀態(tài)對(duì)k+1時(shí)刻進(jìn)行估計(jì)，有

(15)

此時(shí)，理論上有

(16)

跳過(guò)k時(shí)刻狀態(tài)后，濾波發(fā)散判據(jù)變?yōu)?/p>

(17)

若式(17)成立，則說(shuō)明跳過(guò)k時(shí)刻的狀態(tài)后，濾波器由發(fā)散狀態(tài)變?yōu)榉€(wěn)定狀態(tài)，此時(shí)可以將k時(shí)刻的量測(cè)值判定為野值并進(jìn)行剔除，從而抑制濾波發(fā)散的問(wèn)題；若該式不成立，則說(shuō)明觀測(cè)環(huán)境已發(fā)生變化，需要在k時(shí)刻對(duì)Qk重新進(jìn)行計(jì)算。

2.4 AECKF算法步驟

將2.2節(jié)得到的時(shí)變?cè)肼暯y(tǒng)計(jì)估計(jì)器應(yīng)用于標(biāo)準(zhǔn)ECKF濾波算法的框架下，得到AECKF算法如下。

1) 系統(tǒng)初始化。

(18)

2) 時(shí)間更新。

Sk-1|k-1=chol(Pk-1|k-1)

(19)

(20)

(21)

(22)

(23)

Sk|k-1=chol(Pk|k-1)

(24)

(25)

Zi,k|k-1=h(Xi,k|k-1,uk)+r

(26)

(27)

(28)

3) 發(fā)散判斷。

4) 量測(cè)更新。

(29)

(30)

(31)

(32)

(33)

5) 噪聲估計(jì)。

根據(jù)式(10)、式(11)遞推估計(jì)系統(tǒng)噪聲的統(tǒng)計(jì)特性。

3 仿真對(duì)比與性能評(píng)估

3.1 實(shí)驗(yàn)場(chǎng)景

本文選用二維空間，目標(biāo)在0～40 s內(nèi)做勻速運(yùn)動(dòng)；在41～70 s內(nèi)做角速度ω=1.8 rad/s的勻速轉(zhuǎn)彎運(yùn)動(dòng)；在71～100 s內(nèi)做ω=-1.8 rad/s的勻速轉(zhuǎn)彎運(yùn)動(dòng)。仿真時(shí)間為100 s。

(34)

仿真場(chǎng)景1為a=1。

仿真場(chǎng)景2為

(35)

3.2 仿真結(jié)果

分別在兩個(gè)仿真場(chǎng)景下做100次蒙特卡羅仿真，將AECKF與CKF，ECKF算法進(jìn)行仿真實(shí)驗(yàn)對(duì)比。仿真過(guò)程中，Sage-Husa估計(jì)器的參數(shù)b=0.97，均方根誤差(RMSE)定義為

(36)

圖1給出了場(chǎng)景1下目標(biāo)的真實(shí)運(yùn)動(dòng)軌跡以及分別采用CKF，ECKF與AECKF算法跟蹤得到的目標(biāo)運(yùn)動(dòng)軌跡。圖2為在仿真場(chǎng)景1中，3種算法得到的目標(biāo)位置和速度的均方根誤差曲線比較。由圖2可知，在系統(tǒng)噪聲為未知常值的情況下，AECKF算法能有效地減小ECKF算法跟蹤的位置和速度誤差，提高跟蹤精度。表1中的統(tǒng)計(jì)數(shù)據(jù)同樣驗(yàn)證了以上分析。

圖1 場(chǎng)景1目標(biāo)的真實(shí)運(yùn)動(dòng)軌跡與濾波軌跡Fig.1 True trajectory and filtering trajectory of target in Scene 1

圖2 場(chǎng)景1位置和速度的均方根誤差曲線Fig.2 RMSE of location and speed in Scene 1

表1 場(chǎng)景1性能比較

仿真場(chǎng)景2中目標(biāo)的真實(shí)運(yùn)動(dòng)軌跡以及分別采用CKF，ECKF與AECKF算法跟蹤得到的目標(biāo)運(yùn)動(dòng)軌跡如圖3所示。

圖3 場(chǎng)景2目標(biāo)真實(shí)運(yùn)動(dòng)軌跡與濾波軌跡

圖4為3種算法得到的目標(biāo)位置和速度的均方根誤差曲線比較。表2是場(chǎng)景2中目標(biāo)的位置和速度的均方根誤差均值統(tǒng)計(jì)數(shù)據(jù)。

圖4 場(chǎng)景2位置和速度的均方根誤差曲線Fig.4 RMSE of location and speed in Scene 2

表2 場(chǎng)景2性能比較

由圖4和表2可知，通過(guò)AECKF算法得到的位置、速度誤差均小于采用標(biāo)準(zhǔn)CKF算法和ECKF算法跟蹤目標(biāo)得到的誤差，說(shuō)明在系統(tǒng)噪聲未知時(shí)變且與先驗(yàn)噪聲存在一定差異的情況下，AECKF算法能夠有效地抑制目標(biāo)跟蹤的發(fā)散，從而減小跟蹤誤差，提高了跟蹤精度。

表3列出了2種仿真場(chǎng)景下不同濾波算法的平均運(yùn)行時(shí)間。3種算法都具有較高的運(yùn)算效率，但由于AECKF算法采用自適應(yīng)濾波算法對(duì)時(shí)變?cè)肼曔M(jìn)行估計(jì)，所以該算法的速度較CKF算法與ECKF算法慢一些。但與濾波精度的提高相比，AECKF算法的時(shí)間增加處于可接受的范圍內(nèi)。

表3 3種算法的平均運(yùn)行時(shí)間

4 結(jié)束語(yǔ)

嵌入式容積卡爾曼濾波(ECKF)算法是一種高精度的非線性濾波算法，但在濾波時(shí)，系統(tǒng)噪聲的統(tǒng)計(jì)特性未知，可能會(huì)導(dǎo)致其在處理非線性濾波問(wèn)題時(shí)濾波精度降低，嚴(yán)重時(shí)甚至引起發(fā)散。本文提出了一種AECKF濾波算法，利用嵌入式容積準(zhǔn)則對(duì)傳統(tǒng)的Sage-Husa算法進(jìn)行改進(jìn)，得到適用于嵌入式容積卡爾曼濾波器的非線性噪聲統(tǒng)計(jì)估計(jì)器，并引入判斷機(jī)制來(lái)抑制目標(biāo)跟蹤時(shí)的發(fā)散問(wèn)題。仿真結(jié)果表明，該算法能夠有效克服傳統(tǒng)非線性算法在系統(tǒng)噪聲統(tǒng)計(jì)特性未知和時(shí)變的情況下濾波精度較低以及容易引起發(fā)散的缺點(diǎn)，提高了濾波器的自適應(yīng)性與穩(wěn)定性，狀態(tài)估計(jì)的精度要顯著優(yōu)于傳統(tǒng)的非線性濾波算法。