基于平方根容積卡爾曼濾波的目標狀態與傳感器偏差擴維聯合估計算法

劉 瑜，何 友，王海鵬，董 凱

(海軍航空工程學院 信息融合技術研究所，山東 煙臺264001)

0 引 言

多傳感器信息融合系統通過組網方式利用多部傳感器對同一目標的觀測信息進行有效融合處理，可以避免單基地雷達的一些缺點與局限性，獲取全面、可靠的敵我軍事態勢信息，理論上能夠獲得更為精確的目標狀態估計，從而為進一步的環境態勢推理提供更為準確可靠的依據，目前已經在空中交通管制、國土防空、空間監視與衛星遙感等領域中得到了廣泛應用［1－2］。但是，人們在使用多傳感器組網系統時往往發現其融合效果并不如預期那么理想，原因之一即是傳感器量測不僅含有隨機誤差，且具有系統偏差［3－4］。如果這種偏差沒有得到妥善處理，將直接導致目標航跡跟蹤質量的下降，甚至可能出現虛假目標。因此，考慮到傳感器量測與數據互聯、航跡建立、航跡關聯、航跡濾波及跟蹤、航跡管理等后續目標跟蹤問題具有緊密聯系，為了提高分布式多傳感器網絡的數據融合質量，必須消除或降低傳感器量測中的系統偏差對融合的影響。

現有的系統偏差估計算法可分為三類［5］:①離線估計法，主要代表算法有Burke［6］提出的實時質量控制(RTQC)誤差配準算法、Dana［7］提出的廣義最小二乘(GLS)誤差配準算法、Zhou Yifeng 等［8］提出的精確極大似然(EML)誤差配準算法等;②在線估計法，主要有Zhou Lin 等［9］提出的SA－PSO 法、Lin 等［10］提出的EX 法以及宋強等［11］提出的基于傅立葉變換的航跡對準關聯算法;③目標狀態與系統偏差聯合估計法，即將系統誤差做為狀態向量中的擴維部分，進行系統誤差和目標狀態的聯合濾波，主要有Nabaa 等［12］提出的基于EKF 的擴維方法ASEKF、宋強等［13］提出的基于 ASUKF(Augmented state unscented Kalman filter)的實時誤差配準算法及Li 等［14］提出的基于EM－KF 的聯合估計算法。

以上算法中，ASUKF 利用不敏卡爾曼濾波(Unscented Kalman filter，UKF)實現了目標狀態和系統誤差的聯合實時估計，取得了比ASEKF 更為優良的估計性能。其中，UKF［15］選擇2n+1(n為狀態維數)個具有權值的Sigma 點來近似狀態變量的均值，Sigma 點經非線性函數傳播后捕獲的均值和方差能夠達到非線性函數真實值的三階精度，因此其精度高于EKF，同時克服了EKF 易于發散以及只適用于弱非線性的缺點。但是，UKF 需要合理調節系統參數才能達到理想濾波效果，且在高維系統中UKF 容易出現數值不穩定現象，其應用遇到了困難［16］。

近年來，Arasaratnam 同樣從分布近似的角度推導出一種3 階球面－徑向(Spherical－radial)容積規則，提出了容積卡爾曼濾波(Cubature Kalman filter，CKF)［17］。CKF 通過2n 個等權值容積點來傳播系統狀態的均值和方差，能獲得較高的濾波精度。CKF 具備UKF 的優點，且無需像UKF 一樣調節各參數因子，其容積點及其權值僅由狀態維數唯一確定，可以預先計算和存儲，算法設計與實現更為簡單，且在高維狀態濾波中的優勢更為明顯，因而受到了廣大學者的高度重視。

基于以上分析，本文針對帶有固定傳感器偏差的非線性狀態估計問題，研究并提出一種基于平方根CKF 的目標狀態與傳感器偏差擴維聯合估計算法(Augmented state squared－root cubature Kalman filter，ASSRCKF)。ASSRCKF 將目標的運動模型和傳感器偏差配準模型有機地結合起來，形成擴維的狀態空間模型，即將傳感器偏差作為目標狀態向量中的分量，然后基于平方根CKF 濾波，實現狀態與偏差聯合實時估計。

1 帶有系統偏差的非線性狀態估計

以兩部同步雷達A、B 構成的雷達網為例，其量 測 值 分 別 為 RA(k)，θA(k)，ηA(k( )) 和。假設A、B 的距離系統誤差分別為ΔRA、ΔRB，方位角系統誤差為ΔθA、ΔθB，俯仰角系統誤差為ΔηA、ΔηB。雷達A、B 在公共坐標系中的坐標分別為(0，0，0)、(u，v，w)。設定k 時刻目標的位置為 ( x( k)，y(k)，z(k))，速度為，則可以定義k 時刻系統的狀態向量為

以勻速運動的目標為例，其運動狀態方程可以描述為:

式中:

一般情況下，認為傳感器偏差為恒定量或長時間內緩慢變化量，故其狀態方程可以描述為:

式中:I 為單位矩陣，且

因此，結合式(2)和式(7)，整個離散系統的動態方程可以表示為:

式中:

系統的量測方程可以定義為:

式中:

式(9)～(13)中:V1(k)、V(k)、W(k)為白色高斯隨機噪聲，其方差陣分別為Q1(k)、Q(k)、R(k)。

2 平方根容積卡爾曼濾波原理

2.1 基于求容積規則的數值積分近似法

考慮如下具有“非線性函數×高斯概率密度”形式的n 維積分［17－18］:

式中:f(x)為任意函數;Rn為積分區域。

貝葉斯理論求解非線性高斯域濾波的關鍵問題就在于計算求解式(14)所示的積分。

令x=ry，yTy=1，r ∈［0，∞)，則式(14)可表示為:

式中:Un={y ∈Rn|yTy=1}為半徑為1 的超球面;σ(·)為球面度量單位或Un的面積微元。將式(15)化簡得:

這樣，n 維積分式(14)變換為如式(16)和(17)所示的球面－徑向積分形式，可采用球面積分原理及徑向原理求解，如式(18)和(19)所示，從而形成CKF 濾波算法。

式(18)顯示了球面積分原理下一種三階球面積分結構，其中，［u］i表示生成算子的第i 個元素。式(19)展示了球面徑向原理，即m 點高斯積分可以等價為(2m－1)個多項式求和形式，其中，w(x)為一個在積分區間［a，b］上非負的權重函數。

更為具體地，CKF 利用球面徑向準則選取2n(n 為狀態維度)個具有相應權值的點集(wi，ξi)，用于逼近非線性狀態的后驗均值和協方差，如下所示:

式中:

式中:［1］i表示集合［1］的第i 列。

若n=2，則有

按照以上方法計算出容積點集之后，就可以執行CKF 中的時間更新和量測更新步驟。

2.2 平方根容積卡爾曼濾波

原理上，CKF 濾波過程與UKF 類似，但其理論推導更加嚴謹［17－18］。CKF 采用了一種全新的點集近似分布方法，即根據Cubature 準則，利用2 n 個同等權值的Cubature 點經非線性系統方程轉換后產生新的點，進而結合權值計算狀態的預測，無需對非線性模型線性化。

CKF 同樣具備UKF 的優良特性，可以較好地處理非線性系統的估計問題，由于其使用更少的采樣點，進一步降低了計算代價，且在三維以上非線性系統中的優勢更為明顯，具有數值精度更高、濾波穩定性更高和可采用平方根策略的優良特性［19］。然而，在CKF 的濾波遞推過程中，每一步都要計算狀態估計協方差矩陣Pk/k以及一步預測協方差矩陣Pk/k－1的平方根，具有計算量較大且數值不穩定的缺點。而平方根CKF(Squaredroot CKF，SRCKF)在CKF 的基礎上以Cholesky 分解的形式直接傳播和更新狀態協方差矩陣的平方根，降低了計算負擔，從而獲得更高的計算效率，同時能保證協方差矩陣的非負定性，避免了濾波器的發散，提高了濾波的收斂速度和數值穩定性［18］。SRCKF 的時間更新及量測更新遞推步驟可參考文獻［17］。

3 本文算法

首先將目標的運動模型和傳感器偏差配準模型組合在一起，形成擴維的狀態空間模型，即將傳感器偏差作為狀態向量中的分量，然后利用平方根容積卡爾曼濾波技術實現目標狀態和傳感器偏差的聯合估計。因此，基于SRCKF 的目標狀態和傳感器偏差的擴維聯合估計可以稱為ASSRCKF。

考慮如下n 維非線性離散狀態空間模型:

式中:uk為已知的控制輸入;過程噪聲wk－1為零均值、方差為Qk－1的高斯白噪聲;量測噪聲vk為零均值、方差為Rk的高斯白噪聲。

(1)初始化

設置狀態初值x0|0，協方差矩陣初值P0|0，協方差矩陣平方根因子的初始值S0|0，其中P0|0=。狀態初值x0|0及協方差矩陣初值P0|0的初始化采用文獻［20］中所述的方法二。

(2)時間更新(k=1，2，…)

①計算當前狀態的2n 個容積點(i=1 ∶2n):

②計算容積點經過非線性狀態轉移函數的預測值(i=1 ∶2n):

③結合權值與容積點預測值，估計預測狀態(SRCKF 采用相等權值):

④估計預測誤差協方差矩陣的平方根因子:

需要說明的是，算法S=Tria(A)意為:先對矩陣A 進行QR 分解，得到一個正規正交矩陣B 與一個上三角矩陣C，令S=CT，得到的S 為上三角矩陣。

(3)量測更新

①計算更新狀態容積點(i=1 ∶2n):

②計算預測量測容積點(i=1 ∶2n):

③估計預測量測:

④估計新息協方差矩陣:

⑤估計互協方差矩陣:

式中:

⑥估計SRCKF 濾波增益:

⑦基于k 時刻新的量測zk，更新系統狀態:

⑧更新誤差協方差矩陣的平方根因子:

4 仿真及分析

4.1 仿真設置

為驗證本文算法ASSRCKF 的有效性，采用蒙特卡洛仿真，將本文算法與ASEKF、ASUKF 進行比較與分析。為仿真比較更為全面，將基于容積卡爾曼濾波的擴維估計算法(Augmented state cubature Kalman filter，ASCKF)也進行了仿真，其算法過程與ASSRCKF 類似，這里不再贅述。

設定蒙特卡洛仿真次數為20 次，仿真步數設置為1000 步。設置仿真環境如下:采用兩部同步三坐標傳感器A、B 進行組網融合，采樣間隔均為T=1 s。設定A、B 傳感器的笛卡爾坐標分別為(0，0，0)km、(185.2，0，0)km;各傳感器的距離、方位角、俯仰角量測精度均分別設置為50 m、0.3°、0.2°;而測距系統誤差分別為2000 m、1500 m，測方位角系統誤差分別為0.4°、0.3°，俯仰角系統誤差分別為0.3°、0.2°。

假設傳感器公共探測區中的目標做勻速直線運動，目標初始狀態分別設置為:X(0)=［50 km，250 m/s;－50 km，－200 m/s;10 km，10 m/s］。

各濾波器的初始化均采用文獻［20］中所述的方法二進行設定。各算法的估計精度由均方根誤差(Root mean square error，RMSE)曲線進行評價:

4.2 仿真結果及分析

傳感器存在偏差的情況下，圖1 給出了三種算法在X、Y 和Z 軸方向上的目標狀態估計RMSE曲線。需要說明的是，從原理上分析，ASCKF 與ASSRCKF 的估計精度一致，實際仿真結果也是如此，故ASCKF 的RMSE 曲線不必在圖中顯示。

由圖1 可見，3 種實時擴維估計算法均能進行有效的目標狀態估計。其中，基于確定性采樣濾波的兩種算法ASUKF 與ASSRCKF 的擴維估計精度相對較高，而ASSRCKF 算法精度比ASUKF算法略優，且仿真中更為穩定，這也說明了SRCKF 相對UKF 更適合于高維系統濾波。

圖1 X、Y、Z 方向各算法的RMSE 隨時間變化曲線Fig.1 Time variation of RMSE in X，Y and Z direction

圖2 雷達A 的距離、方位角、俯仰角系統偏差估計Fig.2 RMSE in range bias，azimuth bias and elevation bias(Radar A)

為進一步檢測本文算法對傳感器系統偏差的估計效果，圖2、圖3 分別給出了雷達A、雷達B在距離、方位角與俯仰角的偏差估計均方根誤差隨仿真步數變化圖。由圖2、圖3 可見，3 種擴維算法均能夠在仿真步數大于200 時基本實現估計收斂，且精度較高，說明了擴維思想在目標狀態與系統偏差聯合估計中的可行性。其中基于確定性采樣濾波的兩種算法具有相對較快的收斂速度，且相對于ASEKF 提高了偏差估計精度。尤其是ASSRCKF 具有更佳的聯合濾波性能，在目標狀態及偏差估計精度方面相對于ASUKF 平均提高了2%～10%。

ASEKF、ASUKF、ASCKF、ASSRCKF 算法平均每次蒙特卡洛仿真的耗時為0.5753 s、0.2834 s、0.2226 s、0.2015 s(本文仿真中每次循環仿真具有1000 步濾波過程)。

可以看出，ASCKF 與ASSRCKF 的計算耗時明顯低于前兩種算法，在算法實時性方面具有一定優勢。從原理上分析，CKF 與UKF 的計算復雜度在同一個量級，均為，其中nx為狀態維數，nz為量測維數。UKF 單次濾波需要采樣2nx+1 個不敏點，而CKF 只需采樣2nx個容積點，故CKF 的計算代價略低于UKF。另一方面，ASSRCKF 以平方根分解的形式降低了協方差矩陣運算的維度，進一步節省了計算開銷，提高了算法的實時性。

圖3 雷達B 的距離、方位角、俯仰角系統偏差估計Fig.3 RMSE in range bias，azimuth bias and elevation bias(Radar B)

此外，ASEKF 基于擴展卡爾曼濾波理論，其對系統模型的線性化誤差往往會嚴重影響最終的估計精度，甚至導致濾波發散，且其需要計算雅各比矩陣，在高維非線性系統中的計算耗時較大;ASUKF 利用了確定性采樣濾波技術UKF 的優良特性，可以避免濾波估計時非線性系統線性化所帶來的影響，但是其濾波性能是基于預先優化設置的系統參數，影響了算法的可擴展性，且UKF在高維系統中容易出現數值不穩定現象;本文算法ASSRCKF 基于CKF 的濾波性能解決了UKF在高維系統中數值不穩定的缺點，并采用協方差平方根的形式進行濾波計算，保證了方差矩陣的正定性，提高了算法的穩定性。

綜合以上仿真與分析，從估計精度、算法實時性、濾波穩定性等幾個主要方面來看，ASSRCKF相對于已有的擴維算法具有明顯的優勢。

5 結束語

本文主要研究了傳感器存在系統偏差條件下的三維目標跟蹤問題。基于高斯求積規則與三階球面－徑向容積規則，設計了基于平方根容積卡爾曼濾波的目標狀態與傳感器系統偏差擴維聯合估計算法ASSRCKF。仿真分析表明:ASSRCKF在估計精度、算法實時性、濾波穩定性等方面均優于已有的擴維估計算法，為帶有系統誤差的非線性狀態估計問題提供了一種新的可行的解決方法。

［1］Bar－Shalom Y，Li X R.Multitarget－Multisensor Tracking:Principles and Technique［M］.Stors C T:YBS Publishing，1995.

［2］何友，王國宏，關欣.信息融合理論及應用［M］.北京:電子工業出版社，2010.

［3］Farina A.Radar Data Processing(Vol.II)［M］.New York:Research Studies Press Ltd，1985.

［4］Dana A M P.Registration:a prerequisite for multiple sensor tracking［C］∥Multitarget－multisensor Tracking，Advanced Applications，Bar－shalom Y Ed，Norwood:Artech House，1990.

［5］Zhou Lin，Pan Quan，Liang Yan.Joint estimation of state and sensor systematic error in hybrid system［C］∥Proceedings of International Conference on Information Fusion，Singapore，2012:969－975.

［6］Burke J J.The SAGE real time quality control function and its interface with BUIC Ⅱ/BUIC Ⅲ［R］.MITRE Corporation Technical Report，No.308，1996.

［7］Dana M P.Registration:a prerequisite for multiple sensor tracking［C］∥Multitarget－Multisensor Tracking，Advanced Applications，Bar－Shalom Y Ed，Norwood:Artech House，1990.

［8］Zhou Yi－feng，Henry L.An exact maximum likelihood registration algorithm for data fusion［J］.IEEE Trans on Signal Processing，1997，45(6):1560－1572.

［9］Zhou L，Pan Q，Liang Y.Application of improved SA－PSO in the system error registration［J］.Opto－Electronic Engineering，2010，37(9):27－31，38.

［10］Lin X D，Bar－shalom Y.Multisensor－multitarget bias estimaiton for general asynchronous sensors［J］.IEEE Transactions on Aerospace and Electronic Systems，2006，41(3):899－919.

［11］He Y，Song Q，Xiong W.A track registration correlation algorithm based on Fourier transform［J］.Acta Aeronautica et Astronautica Sinica，2010，31(2):362－365.

［12］Nabaa N，Bishop R H.Solution to a multisensor tracking problem with sensor registration errors［J］.IEEE Trans on Aerospace and Electronic Systems，1999，35(1):354－363.

［13］宋強，何友，董云龍.基于UKF 的目標狀態與系統偏差的聯合估計算法［J］.彈箭與制導學報，2007，27(3):311－313，316.Song Qiang，He You，Dong Yun－long.An augmented state target tracking algorithm with systematic errors based on the unscented Kalman filter［J］.Journal of Projectiles，Rockets，Missiles and Guidance，2007，27(3):311－313，316.

［14］Li Z H，Chen S Y，Leung H.Joint data association，registration，and fusion using EM－KF［J］.IEEE Transactions on Aerospace and Electronic Systems，2010，46(2):496－507.

［15］Julier S J，Uhlmann J K.Unscented filtering and nonlinear estimation［J］.IEEE Review，2004，92(3):401－421.

［16］Duník J，?imandl M，Straka O.Performance analysis of derivative－free filters［C］∥Proceedings of the 44th IEEE Conference on Decision and Control，and European Control Conference，Seville，2005:1941－1946.

［17］Arasaratnam I，Haykin S.Cubature Kalman filters［J］.IEEE Transactions on Automatic Control，2009，54(6):1254－1269.

［18］孫楓，唐李軍.Cubature 卡爾曼濾波與Unscented 卡爾曼濾波估計精度比較［J］.控制與決策，2013，28(2):303－308.Sun Feng，Tang Li－jun.Estimation precision comparison of cubature Kalman filter and unscented Kalman filter［J］.Control and Decision，2013，28(2):303－308.

［19］郝燕玲，楊峻巍，陳亮，等.基于SRCKF 的水下航行器動基座初始對準技術［J］.華中科技大學學報:自然科學版，2012，40(2):123－127.Hao Yan－ling，Yang Jun－wei，Chen Liang，et al.Initial alignment method of the dynamic based for underwater vehicles using SRCKF［J］.Journal of Huazhong University of Science and Technology(Natural Science Edition)，2012，40(2):123－127.

［20］宋強，何友，董云龍.一種目標狀態與系統偏差的聯合估計算法［J］.彈箭與制導學報，2007，27(4):312－315，322.Song Qiang，He You，Dong Yun－long.A joint estimation algorithm for state and systematic error［J］.Journal of Projectiles，Rockets，Missiles and Guidance，2007，27(4):312－315，322.