基于SRCKF 的多傳感器融合自適應(yīng)魯棒算法

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

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

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

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

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

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

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

1 平方根容積卡爾曼濾波

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

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

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

1.1 時間更新

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

1.2 量測更新

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.1 仿真條件設(shè)置

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

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

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

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

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

3.2 仿真場景1：模型誤差

在仿真場景1 中，4 種算法在濾波過程中均一直使用FCV，以此在30～60 s 及90～120 s 兩個時間段引入由于模型不匹配造成的模型誤差；其次在80 s 時引入[0，30 m/s，0，30 m/s]T的速度突變，以此引入由于狀態(tài)突變造成的模型誤差。其中一次仿真的目標真實軌跡及4 種算法的濾波軌跡如圖2 所示。可以看出，4 種算法都能跟蹤目標實際運動，但是目標2 次轉(zhuǎn)彎階段即存在模型不匹配時，SRCKF 算法性能下降，估計出的軌跡和真實軌跡不能很好契合。

圖2 目標運動軌跡（場景1）Fig. 2 Target trajectory (scenario 1)

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

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

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

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

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

表1 平均RMSE 對比（場景1）Table 1 Comparison of mean RMSEs (scenario 1)

3.3 仿真場景2：異常量測值

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

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

表2 平均RMSE 對比（場景2）Table 2 Comparison of mean RMSEs (scenario 2)

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

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

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

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

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

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

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

4 結(jié) 論

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

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

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