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

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

0 引 言

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

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

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

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

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

1 帶有系統(tǒng)偏差的非線性狀態(tài)估計

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

以勻速運動的目標(biāo)為例，其運動狀態(tài)方程可以描述為:

式中:

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

式中:I 為單位矩陣，且

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

式中:

系統(tǒng)的量測方程可以定義為:

式中:

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

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

2.1 基于求容積規(guī)則的數(shù)值積分近似法

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

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

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

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

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

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

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

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

式中:

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

若n=2，則有

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

2.2 平方根容積卡爾曼濾波

原理上，CKF 濾波過程與UKF 類似，但其理論推導(dǎo)更加嚴(yán)謹(jǐn)［17－18］。CKF 采用了一種全新的點集近似分布方法，即根據(jù)Cubature 準(zhǔn)則，利用2 n 個同等權(quán)值的Cubature 點經(jīng)非線性系統(tǒng)方程轉(zhuǎn)換后產(chǎn)生新的點，進(jìn)而結(jié)合權(quán)值計算狀態(tài)的預(yù)測，無需對非線性模型線性化。

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

3 本文算法

首先將目標(biāo)的運動模型和傳感器偏差配準(zhǔn)模型組合在一起，形成擴(kuò)維的狀態(tài)空間模型，即將傳感器偏差作為狀態(tài)向量中的分量，然后利用平方根容積卡爾曼濾波技術(shù)實現(xiàn)目標(biāo)狀態(tài)和傳感器偏差的聯(lián)合估計。因此，基于SRCKF 的目標(biāo)狀態(tài)和傳感器偏差的擴(kuò)維聯(lián)合估計可以稱為ASSRCKF。

考慮如下n 維非線性離散狀態(tài)空間模型:

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

(1)初始化

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

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

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

②計算容積點經(jīng)過非線性狀態(tài)轉(zhuǎn)移函數(shù)的預(yù)測值(i=1 ∶2n):

③結(jié)合權(quán)值與容積點預(yù)測值，估計預(yù)測狀態(tài)(SRCKF 采用相等權(quán)值):

④估計預(yù)測誤差協(xié)方差矩陣的平方根因子:

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

(3)量測更新

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

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

③估計預(yù)測量測:

④估計新息協(xié)方差矩陣:

⑤估計互協(xié)方差矩陣:

式中:

⑥估計SRCKF 濾波增益:

⑦基于k 時刻新的量測zk，更新系統(tǒng)狀態(tài):

⑧更新誤差協(xié)方差矩陣的平方根因子:

4 仿真及分析

4.1 仿真設(shè)置

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

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

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

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

4.2 仿真結(jié)果及分析

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

由圖1 可見，3 種實時擴(kuò)維估計算法均能進(jìn)行有效的目標(biāo)狀態(tài)估計。其中，基于確定性采樣濾波的兩種算法ASUKF 與ASSRCKF 的擴(kuò)維估計精度相對較高，而ASSRCKF 算法精度比ASUKF算法略優(yōu)，且仿真中更為穩(wěn)定，這也說明了SRCKF 相對UKF 更適合于高維系統(tǒng)濾波。

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

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

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

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

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

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

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

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

5 結(jié)束語

本文主要研究了傳感器存在系統(tǒng)偏差條件下的三維目標(biāo)跟蹤問題?；诟咚骨蠓e規(guī)則與三階球面－徑向容積規(guī)則，設(shè)計了基于平方根容積卡爾曼濾波的目標(biāo)狀態(tài)與傳感器系統(tǒng)偏差擴(kuò)維聯(lián)合估計算法ASSRCKF。仿真分析表明:ASSRCKF在估計精度、算法實時性、濾波穩(wěn)定性等方面均優(yōu)于已有的擴(kuò)維估計算法，為帶有系統(tǒng)誤差的非線性狀態(tài)估計問題提供了一種新的可行的解決方法。

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

［2］何友，王國宏，關(guān)欣.信息融合理論及應(yīng)用［M］.北京:電子工業(yè)出版社，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 的目標(biāo)狀態(tài)與系統(tǒng)偏差的聯(lián)合估計算法［J］.彈箭與制導(dǎo)學(xué)報，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 的水下航行器動基座初始對準(zhǔn)技術(shù)［J］.華中科技大學(xué)學(xué)報:自然科學(xué)版，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］宋強，何友，董云龍.一種目標(biāo)狀態(tài)與系統(tǒng)偏差的聯(lián)合估計算法［J］.彈箭與制導(dǎo)學(xué)報，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.