李春輝，馬健，楊永建，甘軼

（空軍工程大學(xué)航空工程學(xué)院，西安 710038）

確定采樣型濾波算法估計(jì)精度高，實(shí)現(xiàn)簡(jiǎn)單，是近年來(lái)非線性系統(tǒng)狀態(tài)估計(jì)領(lǐng)域的一個(gè)研究熱點(diǎn)，并廣泛應(yīng)用于目標(biāo)跟蹤、金融、信號(hào)處理和導(dǎo)航等領(lǐng)域［1］。確定采樣型濾波算法包括無(wú)跡卡爾曼濾波（unscented Kalman filter，UKF）［2］、中心差分卡爾曼濾波（central difference Kalman filter，CDKF）［3］及容積卡爾曼濾波（cubature Kalman filter，CKF）［4-5］，理論上已經(jīng)證明，這些算法能至少以二階泰勒精度逼近各種非線性系統(tǒng)的后驗(yàn)協(xié)方差和均值［6］，因此不僅具有比擴(kuò)展卡爾曼濾波（extended Kalman filter，EKF）更高的濾波精度，還能克服EKF需要計(jì)算雅可比矩陣、強(qiáng)非線性時(shí)較差的數(shù)值穩(wěn)定性等不足。相比于粒子濾波（particle filter，PF），確定采樣型濾波在計(jì)算量上具有很大優(yōu)勢(shì)，而且不會(huì)出現(xiàn) PF粒子退化和貧化問(wèn)題［1］。

Arasaratnam和Haykin［4-5］利用球面徑向容積準(zhǔn)則逼近最優(yōu)估計(jì)的狀態(tài)后驗(yàn)分布而提出的CKF算法，不僅能克服UKF在高維度和強(qiáng)非線性系統(tǒng)中的應(yīng)用局限，同時(shí)具備更好的濾波精度及數(shù)值穩(wěn)定性［7］。

在降低CKF算法復(fù)雜度方面，文獻(xiàn)［20］提出了一種簡(jiǎn)化CKF算法，但是該算法對(duì)克服建模不確定性的提高并不大。文獻(xiàn)［17］將STF與假設(shè)檢驗(yàn)理論結(jié)合以檢測(cè)不確定性，在運(yùn)行時(shí)間上有很大優(yōu)化，但是求解漸消因子的計(jì)算量仍然不可避免。

為了能使CKF算法具有對(duì)目標(biāo)模型不匹配及狀態(tài)突變時(shí)的魯棒性，同時(shí)能夠避免強(qiáng)跟蹤C(jī)KF算法較大的計(jì)算量，提出了一種低復(fù)雜度自適應(yīng)CKF算法。首先，設(shè)立了基于新息的自適應(yīng)修正判決準(zhǔn)則及修正方式，然后，對(duì)系統(tǒng)狀態(tài)預(yù)測(cè)值進(jìn)行基于新息的直接補(bǔ)償修正，使得濾波算法能夠及時(shí)“跟上”目標(biāo)真實(shí)狀態(tài)，進(jìn)而減小濾波誤差。本文算法具有良好的濾波性能，同時(shí)在算法復(fù)雜度和仿真運(yùn)行時(shí)間上有明顯改善。

1 容積卡爾曼濾波

對(duì)目標(biāo)建模時(shí)考慮如下非線性離散加性噪聲系統(tǒng)：

式中：f和h分別為非線性的狀態(tài)轉(zhuǎn)移函數(shù)和量測(cè)函數(shù)；xk+1∈Rnx和yk+1∈Rny分別為系統(tǒng)狀態(tài)向量和量測(cè)向量；wk和vk+1分別為系統(tǒng)過(guò)程噪聲和量測(cè)噪聲，兩者是相互獨(dú)立的零均值白噪聲且對(duì)應(yīng)的協(xié)方差矩陣分別為Qk和Rk。系統(tǒng)初始狀態(tài)為x0～N（0，P0），與wk、vk+1互不相關(guān)，P0為初始協(xié)方差矩陣。

CKF算法采用球面徑向容積規(guī)則來(lái)逼近最優(yōu)框架中的狀態(tài)后驗(yàn)均值和后驗(yàn)協(xié)方差，根據(jù)文獻(xiàn)［5，11］，將CKF算法流程總結(jié)如下。

1.1 時(shí)間更新

1.2 量測(cè)更新

2 自適應(yīng)修正容積卡爾曼濾波

2.1 誤差分析

2.2 基于新息的自適應(yīng)修正判決準(zhǔn)則和修正方式

由2.1節(jié)的分析可知，當(dāng)目標(biāo)模型不匹配或者目標(biāo)狀態(tài)發(fā)生突變時(shí)，新息ek+1能反映系統(tǒng)狀態(tài)預(yù)測(cè)值和量測(cè)預(yù)測(cè)值出現(xiàn)的較大偏差，同時(shí)也能反映出預(yù)測(cè)值偏離實(shí)際狀態(tài)的“趨勢(shì)”和“程度”。因此，基于新息來(lái)判決是否修正及如何修正。

圖1 基于新息的修正判決準(zhǔn)則Fig.1 Amending judgment criteria based on innovation sequence

修正系數(shù)的選取首先要遵循避免過(guò)度修正基本原則，其次主要考慮兩方面：一是修正系數(shù)與系統(tǒng)量測(cè)精度相關(guān)；二是新息ek+1所提供的信息維度，如假設(shè)新息中只含有位置的信息，則在選取時(shí)c1或c3可以選取得稍大些，速度的修正可以通過(guò)位置變化來(lái)傳遞，但是對(duì)應(yīng)的系數(shù)c2或c4選取得稍小些。

2.3 低復(fù)雜度自適應(yīng)CKF算法實(shí)現(xiàn)

根據(jù)建立的判決準(zhǔn)則和修正方式總結(jié)了本文算法的流程，如圖2所示。圖中在判別之后的虛線是低復(fù)雜度自適應(yīng)CKF算法的路徑，實(shí)線是標(biāo)準(zhǔn)CKF算法的路徑，具體步驟總結(jié)如下：

圖2 低復(fù)雜度自適應(yīng)CKF算法流程Fig.2 Flowchart of low-complexity adaptive CKF algorithm

步驟1 通過(guò)式（2）～式（12）完成時(shí)間更新及量測(cè)更新的一部分，并得到新息ek+1。

步驟2 將新息中對(duì)應(yīng)變量的絕對(duì)值與門(mén)限進(jìn)行比較，若滿足修正判決條件，則用式（18）進(jìn)行狀態(tài)預(yù)測(cè)值修正，之后用式（9）～式（17）完成濾波，并用式（19）進(jìn)行預(yù)測(cè)誤差協(xié)方差補(bǔ)償。

步驟3 若不需要進(jìn)行修正，則按式（13）～式（17）完成標(biāo)準(zhǔn)CKF濾波。

步驟4 重復(fù)步驟1～步驟3。

3 算法復(fù)雜度分析

使用浮點(diǎn)操作數(shù)（flops）來(lái)分析算法復(fù)雜度是直觀有效的方法，根據(jù)文獻(xiàn)［21-22］，定義以下4種矩陣運(yùn)算的flops：

1）矩陣加減法。A∈Rn×m，B∈Rn×m，A±B運(yùn)算的復(fù)雜度為nm flops。

2）矩陣乘法。A∈Rn×m，B∈Rm×l，AB運(yùn)算的復(fù)雜度為2nm l-nl flops。

3）矩陣求逆。A∈Rn×n，A-1運(yùn)算的復(fù)雜度為n flops。

4）Cholesky分解。A∈Rn×n，chol（A）運(yùn)算的復(fù)雜度為n3／3 flops。

假設(shè)系統(tǒng)狀態(tài)變量為n維，量測(cè)變量為l維，則利用上述矩陣運(yùn)算復(fù)雜度定義，對(duì)標(biāo)準(zhǔn)CKF算法、強(qiáng)跟蹤C(jī)KF算法［13］及本文提出的低復(fù)雜度自適應(yīng)CKF算法的復(fù)雜度進(jìn)行計(jì)算，結(jié)果如表1所示。

表1 三種算法復(fù)雜度計(jì)算結(jié)果Table 1 Computational results of complexity of three algorithms

CKF算法的復(fù)雜度分析具體過(guò)程可見(jiàn)文獻(xiàn)［21］，參照該過(guò)程得到了其他2種算法的復(fù)雜度計(jì)算結(jié)果。

強(qiáng)跟蹤C(jī)KF算法主要考慮文獻(xiàn)［13］中式（3）、式（6）、式（10）和式（11）求解漸消因子上，4個(gè)公式對(duì)應(yīng)的算法復(fù)雜度依次為2 l、2 l2、2n3+6n2l+2 l2n+l2-3nl和2 l2flops。

低復(fù)雜度自適應(yīng)CKF算法主要考慮式（12）新息的計(jì)算及式（18）和式（19）的計(jì)算，對(duì)應(yīng)步驟的算法復(fù)雜度依次為l、n和n2+l flops。

盡管文獻(xiàn)［13］中的強(qiáng)跟蹤C(jī)KF算法已經(jīng)進(jìn)行了改進(jìn)，求解時(shí)避免了計(jì)算雅可比矩陣，但是相比而言，本文算法在復(fù)雜度上仍有明顯改善。將強(qiáng)跟蹤C(jī)KF算法與低復(fù)雜度自適應(yīng)CKF算法的復(fù)雜度求差可得Δflops＝n（2n2-n-1）+3nl（2n-1）+2 l2n+5 l2，在n和l都大于1時(shí)，這一差值會(huì)隨著n和l的增大而變大，即隨著狀態(tài)向量、量測(cè)向量維度的增加，低復(fù)雜度自適應(yīng)CKF算法的優(yōu)勢(shì)會(huì)更大。為了更直觀地體現(xiàn)這一差異，根據(jù)得到的算法復(fù)雜度公式繪制了3種算法的復(fù)雜度隨狀態(tài)向量、量測(cè)向量維度分別變化時(shí)的曲線，如圖3和圖4所示。

圖3 三種算法的復(fù)雜度比較（l＝8）Fig.3 Comparison of three algorithms complexity（l＝8）

圖4 三種算法的復(fù)雜度比較（n＝8）Fig.4 Comparison of three algorithms complexity（n＝8）

4 仿真與分析

4.1 建模與仿真條件設(shè)置

仿真中，目標(biāo)運(yùn)動(dòng)模型為勻速運(yùn)動(dòng)和勻速轉(zhuǎn)彎運(yùn)動(dòng)的混合機(jī)動(dòng)。系統(tǒng)狀態(tài)向量為X＝［x，˙x，y，˙y］T，其中勻速運(yùn)動(dòng)模型的狀態(tài)轉(zhuǎn)移矩陣為FCV，勻速轉(zhuǎn)彎運(yùn)動(dòng)模型的狀態(tài)轉(zhuǎn)移矩陣為FCT，分別如下：

為了驗(yàn)證低復(fù)雜度自適應(yīng)CKF算法在目標(biāo)建模不匹配時(shí)的濾波性能，在跟蹤時(shí)與CKF算法、強(qiáng)跟蹤C(jī)KF算法進(jìn)行對(duì)比，同時(shí)3種算法在目標(biāo)建模時(shí)都僅使用勻速運(yùn)動(dòng)模型FCV。

在目標(biāo)運(yùn)動(dòng)仿真參數(shù)的設(shè)置上，轉(zhuǎn)彎率ω＝5°／s，采樣時(shí)間T＝1 s，目標(biāo)在0～250 s的時(shí)間內(nèi)保持交替變換運(yùn)動(dòng)狀態(tài)，其中勻速直線運(yùn)動(dòng)時(shí)間段為［0，70］s，（120，145］s，（160，200］s，（225，250］s；勻速轉(zhuǎn)彎運(yùn)動(dòng)時(shí)間段為（70，120］s，（145，160］s，（200，225］s，共3次轉(zhuǎn)彎。

目標(biāo)初始狀態(tài)為x0＝［1 000 m，200 m／s，1 000 m，10 m／s］T，系統(tǒng)初始協(xié)方差P0＝diag（3，1，3，1）；系統(tǒng)過(guò)程噪聲協(xié)方差矩陣Q＝diag（1，1，1，1），量測(cè)噪聲協(xié)方差矩陣R＝diag（100，100）；強(qiáng)跟蹤C(jī)KF算法的遺忘因子取0.95。

仿真結(jié)果以目標(biāo)x軸和y軸位置上的均方根誤差（root mean square error，RMSE）來(lái)評(píng)價(jià)，并進(jìn)行150次蒙特卡羅仿真，均方根誤差的定義為

4.2 仿真結(jié)果分析

圖5為其中一次蒙特卡羅仿真的目標(biāo)真實(shí)運(yùn)動(dòng)軌跡及3種算法的濾波軌跡?？梢钥吹剑蛷?fù)雜度自適應(yīng)CKF算法及強(qiáng)跟蹤C(jī)KF算法均能很好地跟蹤目標(biāo)，但是由于濾波建模時(shí)使用的始終是勻速運(yùn)動(dòng)模型，CKF算法在3次機(jī)動(dòng)轉(zhuǎn)彎時(shí)跟蹤性能就會(huì)下降。

圖6和圖7分別為濾波階段x軸和y軸位置的均方根誤差，左上角為對(duì)10～20 s階段放大顯示?？梢钥闯?，整體上低復(fù)雜度自適應(yīng)CKF算法及強(qiáng)跟蹤C(jī)KF算法能夠在建模不準(zhǔn)確時(shí)始終保持較高的濾波精度，但是CKF算法只有在目標(biāo)勻速運(yùn)動(dòng)階段即建模匹配時(shí)才能發(fā)揮很好的濾波性能，在目標(biāo)轉(zhuǎn)彎階段時(shí)其濾波精度受到很大影響。但是從圖6和圖7中放大顯示部分來(lái)看，CKF算法在目標(biāo)運(yùn)動(dòng)建模準(zhǔn)確時(shí)濾波精度要比另外2種算法要高。

表2為3種算法在濾波時(shí)間段里的平均均方根誤差。結(jié)合其中的數(shù)據(jù)及圖6和圖7可以看出，低復(fù)雜度自適應(yīng)CKF算法能夠保持和強(qiáng)跟蹤C(jī)KF算法基本一致的濾波精度，可以較好地克服由于目標(biāo)建模不準(zhǔn)確或者目標(biāo)狀態(tài)突變帶來(lái)的不利影響。

圖6 x軸位置均方根誤差Fig.6 RMSE of position in x axis

圖7 y軸位置均方根誤差Fig.7 RMSE of position in y axis

表2 不同算法平均均方根誤差對(duì)比Table 2 Comparison of mean RMSE among different algorithms

在硬件配置為Intel i7 4710處理器和4 GB內(nèi)存的筆記本電腦上使用MATLAB 2017a進(jìn)行了試驗(yàn)。3種算法的單次仿真平均運(yùn)行時(shí)間和運(yùn)行時(shí)間增加百分比如表3所示。運(yùn)行時(shí)間增量計(jì)算方式為（Ti-T0）／T0，T0為CKF算法的運(yùn)行時(shí)間，Ti為其他2種算法的運(yùn)行時(shí)間。

由表3可見(jiàn)，CKF算法耗時(shí)最短，低復(fù)雜度自適應(yīng)CKF算法在運(yùn)行時(shí)間上略微增加了1.29%，而強(qiáng)跟蹤C(jī)KF算法由于需要求解漸消因子，耗時(shí)最多，相比較CKF算法增加了29.53%。運(yùn)行時(shí)間對(duì)比數(shù)據(jù)和前文算法復(fù)雜度計(jì)算結(jié)果相吻合。

表3 不同算法運(yùn)行時(shí)間對(duì)比Table 3 Comparison of running time among different algorithms

為了驗(yàn)證低復(fù)雜度自適應(yīng)CKF算法的濾波數(shù)值穩(wěn)定性，繪制3種算法每次蒙特卡羅仿真中x軸和y軸位置的平均均方根誤差，如圖8和圖9所示。可以看出，低復(fù)雜度自適應(yīng)CKF算法及強(qiáng)跟蹤C(jī)KF算法每次仿真中的位置平均均方根誤差都低于CKF算法，這是因?yàn)镃KF算法在每次仿真中目標(biāo)建模不匹配即轉(zhuǎn)彎階段時(shí)的濾波誤差會(huì)劇增。同時(shí)相比于CKF算法，另外2種算法的位置平均均方根誤差沒(méi)有出現(xiàn)某次仿真發(fā)散的情況，而且曲線波動(dòng)幅度更小，數(shù)值穩(wěn)定性更優(yōu)。

圖8 x軸位置平均均方根誤差Fig.8 Mean RMSE of position in x axis

圖9 y軸位置平均均方根誤差Fig.9 Mean RMSE of position in y axis

5 結(jié) 論

1）仿真結(jié)果表明，在目標(biāo)建模不準(zhǔn)確的情況下，低復(fù)雜度自適應(yīng)CKF算法能夠保持和強(qiáng)跟蹤C(jī)KF算法基本一致的濾波精度和數(shù)值穩(wěn)定性，有效提高了CKF算法的自適應(yīng)性，同時(shí)也驗(yàn)證了所提算法的有效性。

2）使用等效浮點(diǎn)操作數(shù)進(jìn)行了算法復(fù)雜度計(jì)算，低復(fù)雜度自適應(yīng)CKF算法相比較強(qiáng)跟蹤C(jī)KF算法具有更低的算法復(fù)雜度，且隨著狀態(tài)向量、量測(cè)向量維度的增加，所提算法的優(yōu)勢(shì)會(huì)更大。

3）仿真結(jié)果表明，在算法運(yùn)行時(shí)間上，相比較CKF算法，強(qiáng)跟蹤C(jī)KF算法運(yùn)行時(shí)間增加了29.53%，而低復(fù)雜度自適應(yīng)CKF算法僅增加了1.29%，具有明顯優(yōu)勢(shì)。仿真運(yùn)行時(shí)間結(jié)果與復(fù)雜度計(jì)算結(jié)果保持一致。

為了實(shí)現(xiàn)相關(guān)參數(shù)的自動(dòng)選取，需要進(jìn)一步考慮與智能尋優(yōu)算法的結(jié)合或者相關(guān)理論的推導(dǎo)。