基于修正的自適應(yīng)平方根容積卡爾曼濾波算法

李春輝, 馬 健, 楊永建,2,*, 肖冰松, 鄧有為, 盛 濤

(1. 空軍工程大學(xué)航空工程學(xué)院, 陜西 西安 710038; 2. 西北工業(yè)大學(xué)電子信息學(xué)院, 陜西 西安 710072)

0 引 言

近年來,非線性系統(tǒng)確定采樣型濾波算法受到國(guó)內(nèi)外學(xué)者廣泛關(guān)注并在目標(biāo)跟蹤領(lǐng)域有著重要的應(yīng)用[1]。典型的包括無跡卡爾曼濾波(unscented Kalman filter,UKF)、中心差分卡爾曼濾波(central difference Kalman filter,CDKF)、容積卡爾曼濾波(cubature Kalman filter,CKF)等[2-5]。其中,Arasaratnam和Haykin等人提出的CKF算法利用球面徑向容積準(zhǔn)則逼近最優(yōu)估計(jì)的狀態(tài)后驗(yàn)分布,CKF不僅能克服UKF在高維和強(qiáng)非線性狀態(tài)估計(jì)中的缺點(diǎn)同時(shí)具有更高的濾波精度。為了進(jìn)一步提高CKF算法的穩(wěn)定性和濾波精度,學(xué)者將正交三角分解引入CKF,通過直接計(jì)算協(xié)方差矩陣的平方根,提出了平方根CKF(square-root CKF,SRCKF)。

盡管SRCKF算法性能優(yōu)良,但是卻無法克服系統(tǒng)模型的不確定性,當(dāng)目標(biāo)狀態(tài)突變或者出現(xiàn)不良量測(cè)時(shí),SRCKF算法跟蹤性能就會(huì)下降。主要可以從兩方面解決這一問題,一是對(duì)目標(biāo)機(jī)動(dòng)模型進(jìn)行再建模,提高模型的自適應(yīng)性,如文獻(xiàn)[6-7]中分別將自適應(yīng)當(dāng)前統(tǒng)計(jì)(current statistical, CS)模型以及改進(jìn)的交互式多模型(interacting multiple model, IMM)與SRCKF結(jié)合,有效提高了對(duì)機(jī)動(dòng)目標(biāo)的跟蹤精度。另外一種方法是對(duì)濾波算法進(jìn)行改進(jìn)以提高濾波器性能,目前改進(jìn)SRCKF算法主要選擇的是構(gòu)造強(qiáng)跟蹤濾波器(strong tracking filter, STF),通過引入漸消因子實(shí)時(shí)調(diào)整增益矩陣以彌補(bǔ)較大的預(yù)測(cè)誤差[8-14]。但這種方法不僅理論推導(dǎo)復(fù)雜,求解計(jì)算量較大[15-16],而且也存在漸消因子引入位置隨意[17-19]、收斂精度不高等問題。

為了避免上述構(gòu)造STF時(shí)的局限和不足,本文在SRCKF算法的基礎(chǔ)上,通過平衡先驗(yàn)的預(yù)測(cè)值和后驗(yàn)反饋的量測(cè)值在濾波中所占的比重,提出基于修正的自適應(yīng)SRCKF算法。該算法通過設(shè)置判定準(zhǔn)則和修正準(zhǔn)則直接修正狀態(tài)預(yù)測(cè)值或?yàn)V波增益,能夠有效減小狀態(tài)估計(jì)誤差。仿真結(jié)果表明,該算法具有在目標(biāo)狀態(tài)突變和量測(cè)非線性時(shí)的良好濾波性能和數(shù)值穩(wěn)定性,同時(shí)相比STF在計(jì)算量和收斂速度上具有優(yōu)勢(shì)。

1 目標(biāo)建模與SRCKF

1.1 目標(biāo)建模

非線性加性噪聲離散系統(tǒng)可以通過下式表示:

(1)

式中:xk+1∈Rn和zk+1∈Rm分別是k+1時(shí)刻的系統(tǒng)狀態(tài)向量和量測(cè)向量;fk(·)和hk(·)分別是非線性狀態(tài)轉(zhuǎn)移函數(shù)和非線性量測(cè)函數(shù);wk是系統(tǒng)過程噪聲,vk是量測(cè)噪聲,兩者相互獨(dú)立且wk～N(0,Qk),vk～N(0,Rk);系統(tǒng)初始狀態(tài)x0～N(0,P0)且與wk和vk互不相關(guān)。

1.2 SRCKF

為了保持CKF遞推過程中誤差協(xié)方差矩陣的正定性和對(duì)稱性,SRCKF在濾波時(shí)引入QR分解,直接計(jì)算協(xié)方差矩陣的平方根并進(jìn)行迭代更新,大大提高了濾波穩(wěn)定性和數(shù)值精度。

通常將QR分解運(yùn)算定義為S=Tria(A)=RT,其中R是矩陣AT通過QR分解得到的上三角矩陣,則此時(shí)S為下三角矩陣。根據(jù)文獻(xiàn)[4],SRCKF算法流程可以歸納為如下過程。

1.2.1 時(shí)間更新

(1) 分解狀態(tài)估計(jì)誤差協(xié)方差矩陣:

Pk|k=Sk|k(Sk|k)T

(2)

(2) 構(gòu)造容積點(diǎn):

(3)

(3) 容積點(diǎn)經(jīng)非線性傳播并計(jì)算狀態(tài)量預(yù)測(cè)值:

(4)

(5)

(6)

(4) 計(jì)算預(yù)測(cè)誤差協(xié)方差矩陣的平方根:

(7)

(8)

1.2.2 量測(cè)更新

(1) 構(gòu)造容積點(diǎn):

(9)

(2) 容積點(diǎn)經(jīng)非線性傳播并計(jì)算量測(cè)預(yù)測(cè)值:

Zi,k+1|k=hk+1(Xi,k+1|k),i=1,2,…,m

(10)

(11)

(3) 計(jì)算新息和新息協(xié)方差矩陣的平方根:

(12)

Szz,k+1|k=Tria([Zk+1|k,SR])

(13)

式中:SR表示Rk+1的平方根系數(shù),可以通過對(duì)Rk+1進(jìn)行Cho-lesky分解獲得,即SR=Chol(Rk+1);Zk+1|k是加權(quán)中心矩陣,表示為

(14)

(4) 計(jì)算互協(xié)方差矩陣:

(15)

加權(quán)中心矩陣Xk+1|k定義為

(16)

(5) 計(jì)算濾波增益:

(17)

(6) 狀態(tài)估計(jì):

(18)

(7) 計(jì)算估計(jì)誤差協(xié)方差矩陣的平方根:

Sk+1|k+1=Tria([Xk+1|k-Kk+1Zk+1|k,Kk+1SR])

(19)

2 基于修正的SRCKF算法

2.1 誤差分析與修正

在進(jìn)行非線性濾波時(shí),目標(biāo)運(yùn)動(dòng)建模的不準(zhǔn)確是導(dǎo)致濾波精度下降、跟蹤效果惡化甚至跟蹤發(fā)散的關(guān)鍵原因。目標(biāo)機(jī)動(dòng)、實(shí)際量測(cè)的非線性等因素都會(huì)使得目標(biāo)模型與實(shí)際不匹配。

主要有兩種方法來克服模型誤差,一是通過改進(jìn)濾波算法以提高濾波器精度;二是提高模型的準(zhǔn)確性和自適應(yīng)性,如單模型(single model,SM)算法中的CS模型、Singer模型[20]等,以及多模型(multiple model,MM)算法中的交互式多模型[21]、可能模型集(likely model set, LMS)[22]等。

針對(duì)系統(tǒng)建模不準(zhǔn)確或者目標(biāo)狀態(tài)突變?cè)斐傻恼`差,改進(jìn)SRCKF算法主要通過構(gòu)造強(qiáng)跟蹤濾波器引入漸消因子,通過實(shí)時(shí)調(diào)整增益矩陣以彌補(bǔ)較大的預(yù)測(cè)誤差。但這種改進(jìn)方法理論推導(dǎo)復(fù)雜,求解計(jì)算量較大,而且也存在漸消因子引入位置隨意和收斂精度不高等問題。

從式(18)可以看出,狀態(tài)估計(jì)值由先驗(yàn)的預(yù)測(cè)值和量測(cè)新息的反饋兩部分組成。

一方面當(dāng)k時(shí)刻目標(biāo)狀態(tài)發(fā)生突變或者目標(biāo)運(yùn)動(dòng)模型與實(shí)際不匹配時(shí),k+1時(shí)刻的狀態(tài)預(yù)測(cè)值和量測(cè)預(yù)測(cè)值會(huì)出現(xiàn)較大偏差,進(jìn)而狀態(tài)估計(jì)的精度也隨之下降,這體現(xiàn)在k+1時(shí)刻較大的新息里。強(qiáng)跟蹤濾波器通過引入漸消因子調(diào)整增益矩陣從而使保持濾波殘差序列正交,以此達(dá)到對(duì)目標(biāo)實(shí)際狀態(tài)的跟蹤,其本質(zhì)上是對(duì)式(18)第二項(xiàng)進(jìn)行修正。實(shí)際上可以對(duì)第一項(xiàng)即狀態(tài)預(yù)測(cè)值直接進(jìn)行修正,這樣即使目標(biāo)狀態(tài)發(fā)生突變,通過對(duì)預(yù)測(cè)值進(jìn)行符合實(shí)際變化的修正補(bǔ)償,濾波器也能夠?qū)崟r(shí)地“追上”目標(biāo)狀態(tài),并最終達(dá)到提高估計(jì)精度的目的。這種修正方法不僅直觀形象而且相比之下計(jì)算量減小很多。

另一方面由于量測(cè)的非線性以及實(shí)際干擾和量測(cè)噪聲的存在,量測(cè)值不準(zhǔn)確的情況也時(shí)有發(fā)生。當(dāng)不良量測(cè)出現(xiàn)時(shí),新息也會(huì)增大,仍然對(duì)預(yù)測(cè)值修正顯然會(huì)導(dǎo)致預(yù)測(cè)誤差進(jìn)一步增大甚至導(dǎo)致濾波發(fā)散。此時(shí)可以通過直接修正增益項(xiàng)來實(shí)時(shí)減小量測(cè)值在濾波中的比重。

通過上面的分析可以得出新息的突變決定了是否修正;同時(shí)應(yīng)當(dāng)設(shè)置門限來判定誤差的主要來源,以此決定修正的方式。

2.2 判定準(zhǔn)則與修正準(zhǔn)則

量測(cè)誤差協(xié)方差Rk反映了量測(cè)值的偏差程度。建模時(shí)噪聲為高斯白噪聲,則若以k+1時(shí)刻的量測(cè)值為圓心,以Rk的均方值為半徑畫一個(gè)圓,當(dāng)沒有出現(xiàn)不良量測(cè)等情況,目標(biāo)k+1時(shí)刻的真實(shí)狀態(tài)值以及量測(cè)預(yù)測(cè)值就在這個(gè)量測(cè)圓里。同理,可以將修正門限等效為修正圓的半徑,當(dāng)目標(biāo)狀態(tài)突變或者出現(xiàn)不良量測(cè)時(shí),新息的絕對(duì)值會(huì)增大即量測(cè)預(yù)測(cè)值超出了修正圓,此時(shí)需要修正。

圖1是修正判定準(zhǔn)則的示意圖,圖中虛線圓是量測(cè)圓,實(shí)線圓是修正圓,只畫出了四分之一。Rk+1(i,i)為Rk+1對(duì)角線上第i個(gè)分量,也是對(duì)應(yīng)維度變量的量測(cè)誤差。

圖1 修正判定準(zhǔn)則Fig.1 Judgement rules of amending

r2是最大的修正圓半徑,是因?yàn)閷?shí)際情況中,一旦出現(xiàn)了不良量測(cè),量測(cè)值與狀態(tài)真實(shí)值以及預(yù)測(cè)值都相差較多,新息就會(huì)劇增;相比之下目標(biāo)運(yùn)動(dòng)狀態(tài)變化或者積累誤差造成的建模不準(zhǔn)確引起的新息變化要小一些,這是因?yàn)榧铀俣鹊淖兓瘋鬟f到實(shí)際可量測(cè)的位置、速度并非瞬時(shí)的。

參數(shù)α是調(diào)節(jié)因子,用于平衡量測(cè)值和預(yù)測(cè)值對(duì)修正區(qū)域的影響。α可以選取在[0.6,0.9]之間,與量測(cè)精度相關(guān)。調(diào)節(jié)因子的設(shè)置一方面可以有效減小濾波過程中誤差積累帶來的狀態(tài)偏差,同時(shí)也可以根據(jù)實(shí)際的系統(tǒng)量測(cè)誤差來調(diào)整修正區(qū)域,避免過度修正。

實(shí)際的量測(cè)系統(tǒng)通常都能對(duì)位置和速度進(jìn)行量測(cè),因此利用新息對(duì)k時(shí)刻位置和速度狀態(tài)預(yù)測(cè)值進(jìn)行直接補(bǔ)償修正,對(duì)加速度預(yù)測(cè)值利用速度修正前后變化量進(jìn)行補(bǔ)償。因此當(dāng)量測(cè)預(yù)測(cè)值在r1和r2之間的區(qū)域時(shí),采用的修正準(zhǔn)則如下:

(20)

為了避免過度修正或者修正補(bǔ)償效果不足,實(shí)際選取k1、k2、k3時(shí),一方面要考慮量測(cè)系統(tǒng)對(duì)應(yīng)變量的量測(cè)精度,另一方面也要根據(jù)新息中所提供的變化信息比重,比如一些量測(cè)系統(tǒng)得到的新息只包含位置和速度的變化,則對(duì)k1和k2的選取可以稍大。

當(dāng)量測(cè)預(yù)測(cè)值在r2之外的區(qū)域時(shí),主要是因?yàn)椴涣剂繙y(cè)的影響,直接對(duì)增益項(xiàng)進(jìn)行修正以減小量測(cè)值在濾波中的影響。修正準(zhǔn)則如下：

(21)

式中:Kk+1(i,i)是增益矩陣對(duì)角線上第i個(gè)分量;βi是增益調(diào)節(jié)因子,可以選取在[0.5,0.9]之間,為了避免出現(xiàn)過度修正,選取時(shí)保持與量測(cè)系統(tǒng)的量測(cè)精度成正相關(guān)。

參數(shù)α和βi的選取范圍是基于實(shí)驗(yàn)得到的,在該范圍內(nèi)算法的濾波性能可以達(dá)到最優(yōu)。盡管是基于實(shí)驗(yàn)的經(jīng)驗(yàn)參數(shù),α和βi的選取也必須把握一個(gè)基本原則,即避免對(duì)濾波過程產(chǎn)生過度修正。這一原則的主要目的是通過選取α和βi來設(shè)置合理的修正范圍,以此平衡先驗(yàn)的預(yù)測(cè)值和后驗(yàn)反饋的量測(cè)值在濾波中所占的比重,進(jìn)而減小狀態(tài)估計(jì)誤差。比如,在仿真中α設(shè)置為小于0.6或者βi設(shè)置為小于0.5時(shí),濾波精度會(huì)明顯降低甚至濾波發(fā)散,這就是對(duì)預(yù)測(cè)值或者量測(cè)值過度修正的結(jié)果。

不論是在哪一個(gè)修正區(qū)域,修正之后都會(huì)對(duì)濾波估計(jì)誤差協(xié)方差造成影響。為了避免狀態(tài)估計(jì)穩(wěn)態(tài)精度受到較大影響,在上述兩種方式的修正之后對(duì)估計(jì)誤差協(xié)方差矩陣平方根進(jìn)行補(bǔ)償,公式如下:

(22)

2.3 算法流程

圖2是算法流程示意圖。具體步驟如下。

圖2 算法流程圖Fig.2 Algorithm flow chart

步驟 1由式(2)～式(12)求出k+1時(shí)刻的新息;

步驟 2由修正判定準(zhǔn)則判定是否進(jìn)行修正以及量測(cè)預(yù)測(cè)值處于哪一修正區(qū)域內(nèi),若不需要修正則按式(13)～式(19)繼續(xù)進(jìn)行濾波;

步驟 3若需要對(duì)預(yù)測(cè)值進(jìn)行修正,則按式(20)進(jìn)行修正后,再進(jìn)行式(6)～式(19)的濾波過程,并用式(22)對(duì)估計(jì)誤差協(xié)方差進(jìn)行補(bǔ)償;

步驟 4若需要對(duì)增益進(jìn)行修正,則進(jìn)行式(13)～式(17)求得增益后,按式(20)進(jìn)行修正,之后完成濾波過程并用式(22)對(duì)估計(jì)誤差協(xié)方差進(jìn)行補(bǔ)償。

可見相比較SRCKF算法,本文通過增加新息比較以及直接修正補(bǔ)償使得算法自適應(yīng)能力得到有效改善,同時(shí)相比較STF計(jì)算量有明顯的減小。而且依據(jù)實(shí)際量測(cè)系統(tǒng)量測(cè)精度和新息情況對(duì)修正參數(shù)進(jìn)行調(diào)節(jié)后,濾波性能可以得到有效保證。

3 仿真驗(yàn)證與分析

仿真采用500次蒙特卡羅實(shí)驗(yàn),并以目標(biāo)位置、速度、加速度均方根誤差(root mean square error, RMSE)為評(píng)價(jià)指標(biāo),定義為

(23)

下面針對(duì)目標(biāo)模型的不同對(duì)本文算法的有效性和優(yōu)劣性進(jìn)行驗(yàn)證分析。

3.1 目標(biāo)狀態(tài)突變

該仿真中目標(biāo)初始狀態(tài)x0=[0,10,0]T,在前20 s加速度為0 m/s2,即目標(biāo)做勻速運(yùn)動(dòng),在21～40 s目標(biāo)做勻加速運(yùn)動(dòng),加速度為30 m/s2,在41～80 s目標(biāo)做勻減速運(yùn)動(dòng),加速度變?yōu)?20 m/s2,在81～100 s又恢復(fù)成加速度為30 m/s2的勻加速運(yùn)動(dòng)。采樣間隔為1 s,持續(xù)時(shí)間為100 s。系統(tǒng)噪聲協(xié)方差Q=diag[1,1,1],量測(cè)數(shù)據(jù)只有位置這一維,量測(cè)值噪聲協(xié)方差R=100 m2。初始協(xié)方差P0=diag[8,3,3],初始狀態(tài)估計(jì)值和目標(biāo)初始狀態(tài)取一致。

文獻(xiàn)[8]在STF框架上采用自適應(yīng)SRCKF構(gòu)建了強(qiáng)跟蹤自適應(yīng)SRCKF,在系統(tǒng)存在模型不確定性時(shí)跟蹤性能較好。表1是兩種算法采樣時(shí)間內(nèi)的RMSE和平均耗時(shí)對(duì)比。圖3～圖5是本文算法和文獻(xiàn)[8]中算法進(jìn)行目標(biāo)跟蹤時(shí)位置、速度、加速度RMSE的對(duì)比。

表1 算法性能對(duì)比

圖3 位置RMSEFig.3 RMSE of position

圖4 速度RMSEFig.4 RMSE of velocity

圖5 加速度RMSEFig.5 RMSE of acceleration

由表1和圖3～圖5可以看出,兩種算法穩(wěn)態(tài)跟蹤精度相差不大,但本文算法平均跟蹤精度要更優(yōu);從位置和速度平均RMSE及圖3、圖4來看,在目標(biāo)狀態(tài)發(fā)生突變時(shí)刻即20 s、40 s、80 s時(shí),本文算法在峰值誤差和收斂速度上要更優(yōu);從加速度平均RMSE及圖5來看,兩種算法區(qū)別不大,但是本文算法在誤差收斂速度上更優(yōu)。從算法平均用時(shí)來看,本文算法避免求解漸消因子而直接進(jìn)行修正,在計(jì)算量和算法耗時(shí)上有較大優(yōu)勢(shì)。之所以加速度修正效果不明顯主要是因?yàn)榱繙y(cè)數(shù)據(jù)僅有位置一維,新息中體現(xiàn)出的速度和加速度變化的信息較少,通過位置的變化傳遞到速度再傳遞到加速度修正效果不好。

3.2 量測(cè)非線性目標(biāo)跟蹤

量測(cè)的非線性以及較大的量測(cè)誤差都很有可能使得不良量測(cè)出現(xiàn),通過檢驗(yàn)算法非線性濾波性能可以分析其數(shù)值穩(wěn)定性和動(dòng)態(tài)目標(biāo)跟蹤能力。

角測(cè)量跟蹤(bearing-only tracking,BOT)模型是一個(gè)二維動(dòng)態(tài)目標(biāo)跟蹤模型,也是檢驗(yàn)非線性濾波算法的一種常用模型[23-24]。

仿真中模型參數(shù)設(shè)置、初始狀態(tài)設(shè)置以及誤差定義同文獻(xiàn)[23]。圖6是目標(biāo)真實(shí)軌跡和兩種算法跟蹤濾波軌跡對(duì)比。圖7～圖8是本文算法和文獻(xiàn)[23]中算法進(jìn)行目標(biāo)跟蹤時(shí)位置、速度均方根誤差的對(duì)比。從圖6可以看出,相比于文獻(xiàn)[23]中的算法,本文算法在穩(wěn)定跟蹤后能更好地貼合實(shí)際軌跡。從圖7和圖8可以看出本文算法在位置、速度跟蹤精度以及濾波數(shù)值穩(wěn)定性上都具有優(yōu)勢(shì),對(duì)于非線性量測(cè)系統(tǒng)有較好的跟蹤性能。

圖6 目標(biāo)濾波軌跡Fig.6 Target filtering trajectory

圖7 位置跟蹤RMSEFig.7 RMSE of position tracking

圖8 速度跟蹤RMSEFig.8 RMSE of velocity tracking

4 結(jié) 論

目標(biāo)機(jī)動(dòng)、不良量測(cè)等因素都會(huì)使得目標(biāo)模型與實(shí)際不匹配,從而導(dǎo)致濾波精度下降,濾波算法失效。本文在SRCKF算法的基礎(chǔ)上首先分析了誤差的主要來源,并提出了修正的思路,本質(zhì)上就是通過直接修正狀態(tài)預(yù)測(cè)值或者濾波增益來平衡先驗(yàn)的預(yù)測(cè)值和后驗(yàn)反饋的量測(cè)值在濾波中所占的比重進(jìn)而減小狀態(tài)估計(jì)誤差。

之后本文設(shè)置了與實(shí)際系統(tǒng)量測(cè)精度密切相關(guān)的修正判定準(zhǔn)則和修正準(zhǔn)則,構(gòu)成了基于修正的自適應(yīng)SRCKF算法。仿真結(jié)果表明,在目標(biāo)狀態(tài)突變時(shí),本文提出的算法相比于強(qiáng)跟蹤SRCKF算法有著更高的濾波精度和誤差收斂速度,而且在計(jì)算量和計(jì)算時(shí)間方面也有明顯優(yōu)勢(shì);同時(shí)量測(cè)非線性目標(biāo)跟蹤的仿真也表明本文算法具有較強(qiáng)的非線性系統(tǒng)濾波數(shù)值穩(wěn)定性。