李春輝，馬健，楊永建，肖冰松，鄧有為

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

容積卡爾曼濾波[1-2]（cubature Kalman filter，CKF）是Arasaratnam 等學(xué)者提出的一種非線性濾波算法。CKF 基于三階球面徑向原則進(jìn)行數(shù)值運(yùn)算，逼近最優(yōu)濾波框架中的狀態(tài)后驗(yàn)分布[3]，能以至少二階泰勒精度逼近任何非線性系統(tǒng)的后驗(yàn)協(xié)方差和均值[4]，在目標(biāo)跟蹤[5]、導(dǎo)航[6]等領(lǐng)域應(yīng)用廣泛。為了獲得更高的數(shù)值穩(wěn)定性，正交三角分解被引入CKF 迭代過(guò)程，構(gòu)成了平方根CKF（square-root CKF，SRCKF）算法。

但是，CKF/SRCKF 算法的自適應(yīng)性和魯棒性并不總能滿足實(shí)際應(yīng)用場(chǎng)景的需求，如機(jī)動(dòng)目標(biāo)的跟蹤。從濾波的角度出發(fā)，影響SRCKF 狀態(tài)估計(jì)性能的主要因素有2 類：①目標(biāo)建模與實(shí)際不匹配、目標(biāo)機(jī)動(dòng)后狀態(tài)發(fā)生突變等造成的模型誤差；②量測(cè)缺失、量測(cè)偏差等造成的異常量測(cè)值[7]。

目前，主要有2 種途徑克服模型誤差：①?gòu)奶岣吣Ｐ妥赃m應(yīng)性和準(zhǔn)確性出發(fā)對(duì)目標(biāo)進(jìn)行再建模[8-10]；②構(gòu)建和改進(jìn)各種自適應(yīng)濾波算法[11-13]。其中，SRCKF 與強(qiáng)跟蹤濾波器（strong tracking filter，STF）架構(gòu)的結(jié)合可以有效克服模型誤差的影響[14-17]，但是也存在求解計(jì)算量大[17]等局限，因此，文獻(xiàn)[18]提出了基于新息修正的低復(fù)雜度自適應(yīng)CKF（low complexity adaptive cubature Kalman filter, LCACKF）算法，可以作為STF 的有效替代算法。另外，針對(duì)異常量測(cè)值也有各種魯棒技術(shù)[19-21]。但是上述方法難以克服模型誤差和異常量測(cè)值同時(shí)發(fā)生帶來(lái)的不良影響。

實(shí)際上，也有一些魯棒濾波器可以解決上述問(wèn)題，如魯棒M-M 濾波器[22]、H∞濾波器[23]和重尾分布噪聲濾波器[24-25]等。但是這些濾波算法存在理論推導(dǎo)復(fù)雜、迭代次數(shù)多的局限性。此外，構(gòu)建自適應(yīng)魯棒濾波器[26-30]也可以有效克服這一問(wèn)題。Yang 等[26-27]建立了一套自適應(yīng)魯棒卡爾曼濾波（adaptively robust Kalman filter, ARKF）理論，利用魯棒估計(jì)方法控制測(cè)量異常值，并引入自適應(yīng)漸消因子克服模型誤差，成功運(yùn)用于動(dòng)態(tài)導(dǎo)航定位[28]。但是當(dāng)在無(wú)法獲得全局狀態(tài)的魯棒估計(jì)解時(shí)，ARKF就可能失效[29]，因此文獻(xiàn)[30]基于ARKF 架構(gòu)提出了一種魯棒多漸消因子容積卡爾曼濾波（robust multiple fading factors cubature Kalman filter, RMCKF），克服了ARKF 的局限性，且迭代次數(shù)少。RMCKF結(jié)合了STF 架構(gòu)及基于Huber 函數(shù)的魯棒估計(jì)，但是RMCKF 為次優(yōu)算法的結(jié)合，濾波精度不夠高，而且當(dāng)新息出現(xiàn)較大偏差時(shí)，均是基于新息構(gòu)建的漸消因子和修正的量測(cè)誤差協(xié)方差矩陣就難以正確有效的獲得。

因此，為了克服模型誤差和異常量測(cè)值的影響，一方面受到自適應(yīng)魯棒濾波器結(jié)構(gòu)的啟發(fā)，另一方面結(jié)合多傳感器信息融合技術(shù)精度高和互補(bǔ)性強(qiáng)等優(yōu)點(diǎn)[31]，本文提出一種多傳感器融合自適應(yīng)魯棒算法。首先，基于新息協(xié)方差匹配原則設(shè)計(jì)了魯棒子系統(tǒng)以控制異常量測(cè)值；然后，將文獻(xiàn)[18]中提出的自適應(yīng)算法與SRCKF 結(jié)合構(gòu)成LCASRCKF，并用其代替STF 設(shè)計(jì)了自適應(yīng)子系統(tǒng)，以此克服模型誤差；最后，結(jié)合2 種子系統(tǒng)的特點(diǎn)和算法設(shè)計(jì)缺陷提出了一種基于分布式的融合架構(gòu)，使得全局輸出可以充分平衡并利用先驗(yàn)的模型預(yù)測(cè)值信息和后驗(yàn)的量測(cè)值信息。仿真結(jié)果驗(yàn)證了本文所提融合架構(gòu)和融合算法的有效性，在存在模型誤差和異常量測(cè)值的情況下，融合算法有著明顯更優(yōu)的狀態(tài)估計(jì)性能。

1 平方根容積卡爾曼濾波

考慮如下非線性離散加性噪聲系統(tǒng)：

式中：f( )和h( )分別為非線性狀態(tài)轉(zhuǎn)移函數(shù)和量測(cè)函數(shù)；xk+1和zk+1分 別為n維 和l維的狀態(tài)向量和量測(cè)向量；wk和vk+1分別為過(guò)程噪聲和量測(cè)噪聲，兩者相互獨(dú)立且對(duì)應(yīng)的噪聲協(xié)方差矩陣分別為Qk和Rk+1，均為高斯白噪聲。

對(duì)于式（1）和式（2）所建模的系統(tǒng)，假設(shè)k時(shí)刻系統(tǒng)的狀態(tài)估計(jì)值x?k|k和 估計(jì)誤差協(xié)方差Pk|k已知，則SRCKF 算法在k+1時(shí)刻的狀態(tài)估計(jì)步驟可以總結(jié)如1.1 節(jié)、1.2 節(jié)所示。

1.1 時(shí)間更新

1） 使用k時(shí)刻狀態(tài)估計(jì)值x?k|k和估計(jì)誤差協(xié)方差Pk|k估計(jì)容積點(diǎn)：

1.2 量測(cè)更新

2 子系統(tǒng)設(shè)計(jì)與融合架構(gòu)

2.1 基于新息協(xié)方差匹配的魯棒SRCKF 子系統(tǒng)

對(duì)于多傳感器融合算法，子系統(tǒng)的魯棒性是保證系統(tǒng)整體性能的關(guān)鍵因素。如果子系統(tǒng)對(duì)異常量測(cè)值不具有魯棒性且不能及時(shí)識(shí)別和隔離誤差，融合效果就會(huì)產(chǎn)生偏差甚至發(fā)散。因此，文獻(xiàn)[7,32]都是先設(shè)計(jì)魯棒濾波算法，再對(duì)全局融合進(jìn)行設(shè)計(jì)。

協(xié)方差匹配原則是一種理論簡(jiǎn)單且計(jì)算量小的魯棒技術(shù)[12]，可以通過(guò)對(duì)量測(cè)噪聲進(jìn)行修正和調(diào)整，以此削弱異常量測(cè)值在濾波過(guò)程中的作用。因此，基于新息協(xié)方差匹配來(lái)建立魯棒SRCKF 子系統(tǒng)。當(dāng)系統(tǒng)濾波正常時(shí)，理論和實(shí)際的新息協(xié)方差矩陣相匹配：

實(shí)際上，魯棒算法應(yīng)該在異常量測(cè)值存在的情況下再去影響濾波器特性，否則就會(huì)對(duì)濾波最優(yōu)性造成影響。因此需要建立切換準(zhǔn)則，根據(jù)數(shù)據(jù)實(shí)際情況來(lái)選擇標(biāo)準(zhǔn)或者魯棒SRCKF?；诩僭O(shè)檢驗(yàn)理論建立的服從 χ2分布的質(zhì)量檢測(cè)統(tǒng)計(jì)函數(shù)是一種有效的方法[8-9,17]，但是隨著量測(cè)向量維數(shù)的增加，統(tǒng)計(jì)函數(shù)的計(jì)算負(fù)載也隨之增大，實(shí)時(shí)性會(huì)受到影響。

基于門限判決的切換準(zhǔn)則和基于 χ2分布的質(zhì)量檢測(cè)統(tǒng)計(jì)函數(shù)實(shí)質(zhì)上都是概率論的運(yùn)用，兩者的容錯(cuò)率都可以得到保證。

2.2 基于修正的自適應(yīng)SRCKF 子系統(tǒng)

目標(biāo)機(jī)動(dòng)或者目標(biāo)建模不匹配造成的模型誤差也會(huì)使得濾波器性能下降甚至濾波發(fā)散，這是因?yàn)橹暗臄?shù)據(jù)在濾波過(guò)程中起到了過(guò)強(qiáng)的牽制作用[33]，需要加強(qiáng)新的量測(cè)數(shù)據(jù)的作用來(lái)使系統(tǒng)及時(shí)跟蹤真值。當(dāng)模型誤差存在時(shí)，狀態(tài)預(yù)測(cè)值就會(huì)與真實(shí)目標(biāo)狀態(tài)產(chǎn)生偏差，進(jìn)而使新息增大。從式（20）可以看到，若子系統(tǒng)仍然運(yùn)行魯棒SRCKF 算法，修正后的Rk+1會(huì)增大，反而導(dǎo)致量測(cè)信息在濾波過(guò)程中比重進(jìn)一步減小。顯然，算法設(shè)計(jì)的基本思想導(dǎo)致魯棒子系統(tǒng)難以克服模型誤差。

這就需要設(shè)計(jì)自適應(yīng)子系統(tǒng)來(lái)解決這一問(wèn)題。LCACKF 算法通過(guò)設(shè)置與新息關(guān)聯(lián)的判定門限和修正準(zhǔn)則，直接對(duì)狀態(tài)預(yù)測(cè)值x?k+1|k進(jìn)行修正，使得預(yù)測(cè)值可以及時(shí)“跟上”目標(biāo)真實(shí)狀態(tài)，減小預(yù)測(cè)誤差，進(jìn)而減小狀態(tài)估計(jì)誤差[18]。文獻(xiàn)[18]通過(guò)誤差分析和仿真驗(yàn)證表明該算法不僅有較高的濾波精度，還具有低算法復(fù)雜度和快速收斂等優(yōu)勢(shì)，將其與SRCKF 結(jié)合構(gòu)成LCASRCKF 后可以作為STF-SRCKF 的有效替代算法，用以克服模型誤差帶來(lái)的不良影響。但是該算法還存在一些問(wèn)題，需要改進(jìn)。

文獻(xiàn)[18]中對(duì)修正判決門限的設(shè)置基于經(jīng)驗(yàn)和具體仿真場(chǎng)景。本文將判決門限縮放因子 λ區(qū)間調(diào)整為[2,3)，調(diào)整結(jié)合了設(shè)計(jì)魯棒系統(tǒng)時(shí)用到的3σ準(zhǔn)則。之所以比3 稍小一些是為了增加修正區(qū)域?qū)δＰ驼`差的敏感性，減小濾波過(guò)程中積累誤差的影響。

LCASRCKF 算法并不能有效控制異常量測(cè)值，這也是由算法設(shè)計(jì)的基本思想造成的：對(duì)于狀態(tài)預(yù)測(cè)值的修正是基于新息的，當(dāng)新息偏差源自異常量測(cè)值而非目標(biāo)狀態(tài)的突變時(shí)，新息就不能反映出預(yù)測(cè)值偏離實(shí)際狀態(tài)的趨勢(shì)，此時(shí)再使用修正算法反而會(huì)進(jìn)一步增大預(yù)測(cè)誤差。實(shí)際上，這也是很多克服模型誤差的自適應(yīng)算法都有的缺陷，如STF 通過(guò)實(shí)時(shí)調(diào)整增益矩陣，加重量測(cè)數(shù)據(jù)在濾波中的作用，可以有效克服模型誤差，但是在異常量測(cè)值出現(xiàn)時(shí)其濾波性能顯然會(huì)受到較大影響。

2.3 融合準(zhǔn)則和融合架構(gòu)

由上述分析可知，單獨(dú)的自適應(yīng)子系統(tǒng)和魯棒子系統(tǒng)難以同時(shí)克服模型誤差和異常量測(cè)值的影響，這是2 種算法設(shè)計(jì)時(shí)的基本思想造成的，也就使得直接進(jìn)行簡(jiǎn)單的量測(cè)融合或者狀態(tài)融合并不能達(dá)到預(yù)期的效果和精度。因此，本文中對(duì)融合架構(gòu)進(jìn)行改進(jìn)，主要考慮以下幾點(diǎn)因素：

1） 魯棒SRCKF 子系統(tǒng)的魯棒量測(cè)值和高量測(cè)精度是自適應(yīng)SRCKF 子系統(tǒng)所需要的。

2） 對(duì)于自適應(yīng)SRCKF 子系統(tǒng)，修正后的狀態(tài)預(yù)測(cè)值貼近目標(biāo)真實(shí)狀態(tài)，一方面可以為全局融合提供更多信息，另一方面也是難以自適應(yīng)跟隨目標(biāo)狀態(tài)變化的魯棒SRCKF 子系統(tǒng)所需要的。

通過(guò)設(shè)計(jì)一種基于分布式的融合架構(gòu)可以實(shí)現(xiàn)：在所有系統(tǒng)濾波正常時(shí)，融合使用了更多的信息，因此全局輸出的精度會(huì)明顯提升；當(dāng)存在模型誤差和量測(cè)偏差時(shí)，通過(guò)融合使得全局輸出可以有效充分地利用先驗(yàn)的模型信息和后驗(yàn)的量測(cè)信息；當(dāng)突變型不良量測(cè)甚至量測(cè)缺失的情況出現(xiàn)時(shí)，可以避免自適應(yīng)子系統(tǒng)對(duì)全局輸出的“抵消”作用。

3 數(shù)值仿真與分析

3.1 仿真條件設(shè)置

融合系統(tǒng)由1 個(gè)高量測(cè)精度的魯棒SRCKF 子系統(tǒng)和2 個(gè)自適應(yīng)SRCKF 子系統(tǒng)組成，按照?qǐng)D1中的架構(gòu)進(jìn)行融合輸出。在濾波過(guò)程中，3 個(gè)子系統(tǒng)的過(guò)程噪聲協(xié)方差矩陣均為Q=diag(1,1,1,1)，魯棒子系統(tǒng)和自適應(yīng)子系統(tǒng)的量測(cè)噪聲協(xié)方差矩陣分別為RR= diag(50,50)，RA=diag(100,100)。

圖1 融合架構(gòu)Fig. 1 Fusion architecture

為驗(yàn)證本文所提融合算法在模型誤差和異常量測(cè)值存在時(shí)的有效性，將本文算法與SRCKF 算法、LCASRCKF 算法、RMCKF 算法進(jìn)行對(duì)比。融合算法中的門限調(diào)節(jié)參數(shù) β為9，同時(shí)為了對(duì)比有效，LCASRCKF 和RMCKF 所使用的量測(cè)噪聲協(xié)方差矩陣也設(shè)置為RR。

使用x和y方向上的位置均方根誤差（root mean square error, RMSE）對(duì)濾波算法的狀態(tài)估計(jì)性能進(jìn)行量化評(píng)價(jià)，并進(jìn)行200 次蒙特卡羅實(shí)驗(yàn)，得到統(tǒng)計(jì)平均結(jié)果，RMSE 通過(guò)式（32）計(jì)算：

式中：N為仿真總次數(shù)；j和k指第j次仿真的k時(shí)刻；Xi(k) 和X?ij(k|k) 分 別為狀態(tài)向量第i維分量的真實(shí)值和狀態(tài)估計(jì)值。每次蒙特卡羅仿真中，濾波初始值和目標(biāo)初始狀態(tài)保持一致。

3.2 仿真場(chǎng)景1：模型誤差

在仿真場(chǎng)景1 中，4 種算法在濾波過(guò)程中均一直使用FCV，以此在30～60 s 及90～120 s 兩個(gè)時(shí)間段引入由于模型不匹配造成的模型誤差；其次在80 s 時(shí)引入[0，30 m/s，0，30 m/s]T的速度突變，以此引入由于狀態(tài)突變?cè)斐傻哪Ｐ驼`差。其中一次仿真的目標(biāo)真實(shí)軌跡及4 種算法的濾波軌跡如圖2 所示?？梢钥闯?，4 種算法都能跟蹤目標(biāo)實(shí)際運(yùn)動(dòng)，但是目標(biāo)2 次轉(zhuǎn)彎階段即存在模型不匹配時(shí)，SRCKF 算法性能下降，估計(jì)出的軌跡和真實(shí)軌跡不能很好契合。

圖2 目標(biāo)運(yùn)動(dòng)軌跡（場(chǎng)景1）Fig. 2 Target trajectory (scenario 1)

4 種算法在x和y方向上位置的RMSE 曲線對(duì)比如圖3 和圖4 所示，對(duì)應(yīng)的平均RMSE 值如表1所示。

從圖3 和圖4 可以看出，在模型準(zhǔn)確的情況下，本文所提融合算法的濾波精度明顯高于其他3 種算法，而由于RMCKF 和LCASRCKF 都是次優(yōu)算法，量測(cè)精度比SRCKF 算法還要差一些。另外，在模型不匹配和狀態(tài)突變?cè)斐赡Ｐ驼`差時(shí)，SRCKF算法的RMSE 曲線會(huì)發(fā)生較大跳變，RMCKF 和LCASRCKF 算法都能克服模型誤差的不良影響，值得注意的是，RMCKF 算法精度低于LCASRCKF 算法，這是因?yàn)榇藭r(shí)偏差的主要來(lái)源是模型誤差，RMCKF-算法對(duì)Rk+1的魯棒修正會(huì)對(duì)漸消因子的作用產(chǎn)生一定的“削弱”。

圖3 x 方向位置均方根誤差（場(chǎng)景1）Fig. 3 RMSE of position in x axis (scenario 1)

圖4 y 方向位置均方根誤差（場(chǎng)景1）Fig. 4 RMSE of position in y axis (scenario 1)

結(jié)合表1 可以看到，融合算法有著明顯更優(yōu)的濾波精度和收斂速度，這是因?yàn)槿诤霞軜?gòu)中不僅包含更多魯棒子系統(tǒng)的量測(cè)信息，還融入了更多自適應(yīng)子系統(tǒng)修正后的狀態(tài)預(yù)測(cè)信息。

表1 平均RMSE 對(duì)比（場(chǎng)景1）Table 1 Comparison of mean RMSEs (scenario 1)

3.3 仿真場(chǎng)景2：異常量測(cè)值

在場(chǎng)景2 中，模型誤差（同場(chǎng)景1）及異常量測(cè)值同時(shí)發(fā)生。將10 ～12 s 的量測(cè)值置零以引入量測(cè)缺失的情況，同時(shí)對(duì)80 s 和105 s 的量測(cè)值分別引入[400 m，400 m/s，400 m，400 m]T和[300 m，300 m/s，300 m，300 m]T的量測(cè)偏差。

4 種算法在x和y方向上的位置RMSE 曲線對(duì)比如圖5 和圖6 所示，圖7 和圖8 為對(duì)應(yīng)RMSE 曲線的放大圖；表2 為4 種算法平均RMSE 值的對(duì)比。

表2 平均RMSE 對(duì)比（場(chǎng)景2）Table 2 Comparison of mean RMSEs (scenario 2)

圖5 x 方向位置均方根誤差（場(chǎng)景2）Fig. 5 RMSE of position in y axis (scenario 2)

圖6 y 方向位置均方根誤差（場(chǎng)景2）Fig. 6 RMSE of position in y axis (scenario 2)

圖7 x 方向位置RMSE 細(xì)節(jié)（場(chǎng)景2）Fig. 7 Details of RMSE of position in x axis (scenario 2)

圖8 y 方向位置RMSE 細(xì)節(jié)（場(chǎng)景2）Fig. 8 Details of RMSE of position in y axis (scenario 2)

從圖5 和圖6 可以看到，在異常量測(cè)值引入的時(shí)刻，SRCKF、RMCKF 和LCASRCKF 這3 種算法的RMSE 曲線都出現(xiàn)很大的跳變，而RMCKF 算法能以快速的收斂速度迅速降低誤差，LCASRCKF 算法收斂速度稍慢但也優(yōu)于SRCKF 算法。值得注意的是，在異常量測(cè)值和模型誤差同時(shí)發(fā)生時(shí)，LCASRCKF 和RMCKF 算法甚至出現(xiàn)了比SRCKF 算法更高的峰值，這也符合之前的分析：此時(shí)新息的偏差主要來(lái)自于異常量測(cè)值，LCASRCKF 算法在此時(shí)修正狀態(tài)預(yù)測(cè)值顯然難以克服其不良影響甚至使誤差進(jìn)一步增大；而對(duì)于RMCKF 算法而言，此時(shí)基于較大新息得到的漸消因子會(huì)對(duì)改進(jìn)的量測(cè)誤差協(xié)方差矩陣的魯棒作用產(chǎn)生“抵消”，這種仿真現(xiàn)象也出現(xiàn)在了文獻(xiàn)[30]中。

相比之下，本文所提融合算法在誤差存在時(shí)有著理想的收斂速度，同時(shí)保持了較高的濾波精度和穩(wěn)定性。融合架構(gòu)中的融合輸出系數(shù)S使得全局輸出可以平衡并充分利用先驗(yàn)預(yù)測(cè)值和后驗(yàn)量測(cè)值在狀態(tài)估計(jì)過(guò)程中所提供的信息，因此融合算法的RMSE 曲線始終保持相對(duì)平穩(wěn)，沒(méi)有出現(xiàn)其他算法那樣的跳變。結(jié)合表2 可以看到，融合算法在濾波精度上的優(yōu)勢(shì)明顯。

從圖7 和圖8 可以進(jìn)一步看到本文融合算法對(duì)10 ～12 s 之間連續(xù)性的量測(cè)缺失也能保持魯棒性；在80 s時(shí)狀態(tài)突變和異常量測(cè)值的同時(shí)發(fā)生對(duì)融合算法的精度影響不大；但是在105 s 模型不匹配和異常量測(cè)值的同時(shí)發(fā)生使得融合算法產(chǎn)生了一個(gè)稍高的峰值。

4 結(jié) 論

1) 為保持全局融合時(shí)的魯棒性，基于新息協(xié)方差匹配原則建立了魯棒子系統(tǒng)，為克服模型誤差，基于LCASRCKF 算法建立了自適應(yīng)子系統(tǒng)。通過(guò)融合架構(gòu)的改進(jìn)，使得全局輸出可以充分平衡并利用先驗(yàn)的模型預(yù)測(cè)值信息和后驗(yàn)的量測(cè)值信息。

2) 仿真結(jié)果驗(yàn)證了所提融合算法及融合架構(gòu)的有效性。在模型誤差存在時(shí)，融合算法有著比RMCKF 算法、LCASRCKF 算法及SRCKF 算法更高的濾波精度和收斂速度；在異常量測(cè)值同時(shí)被引入時(shí)，所提融合算法具有更優(yōu)的穩(wěn)定性和收斂速度，同時(shí)有著比其他3 種算法明顯更高的濾波精度。

在融合架構(gòu)的設(shè)計(jì)及全局輸出的融合方式上主要考慮的是利用更多的信息，采用了分布式結(jié)構(gòu)下無(wú)反饋的融合方式。下一步考慮結(jié)合聯(lián)邦濾波器的信息分配等特點(diǎn)，對(duì)融合架構(gòu)進(jìn)一步優(yōu)化，使之應(yīng)用場(chǎng)景更廣泛和多變。