基于SRCKF 的多傳感器融合自適應魯棒算法

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

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

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

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

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

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

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

1 平方根容積卡爾曼濾波

考慮如下非線性離散加性噪聲系統：

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

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

1.1 時間更新

1） 使用k時刻狀態估計值x?k|k和估計誤差協方差Pk|k估計容積點：

1.2 量測更新

2 子系統設計與融合架構

2.1 基于新息協方差匹配的魯棒SRCKF 子系統

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

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

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

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

2.2 基于修正的自適應SRCKF 子系統

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

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

文獻[18]中對修正判決門限的設置基于經驗和具體仿真場景。本文將判決門限縮放因子 λ區間調整為[2,3)，調整結合了設計魯棒系統時用到的3σ準則。之所以比3 稍小一些是為了增加修正區域對模型誤差的敏感性，減小濾波過程中積累誤差的影響。

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

2.3 融合準則和融合架構

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

1） 魯棒SRCKF 子系統的魯棒量測值和高量測精度是自適應SRCKF 子系統所需要的。

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

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

3 數值仿真與分析

3.1 仿真條件設置

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

圖1 融合架構Fig. 1 Fusion architecture

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

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

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

3.2 仿真場景1：模型誤差

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

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

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

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

圖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)

結合表1 可以看到，融合算法有著明顯更優的濾波精度和收斂速度，這是因為融合架構中不僅包含更多魯棒子系統的量測信息，還融入了更多自適應子系統修正后的狀態預測信息。

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

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

在場景2 中，模型誤差（同場景1）及異常量測值同時發生。將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 為對應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 細節（場景2）Fig. 7 Details of RMSE of position in x axis (scenario 2)

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

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

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

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

4 結 論

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

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

在融合架構的設計及全局輸出的融合方式上主要考慮的是利用更多的信息，采用了分布式結構下無反饋的融合方式。下一步考慮結合聯邦濾波器的信息分配等特點，對融合架構進一步優化，使之應用場景更廣泛和多變。