逼近積分點數(shù)下限的五階容積卡爾曼濾波定軌算法?

李兆銘 楊文革 丁丹 廖育榮

1)(裝備學院研究生院,北京 101416)

2)(裝備學院光電裝備系,北京 101416)

1 引 言

對低軌衛(wèi)星進行在軌監(jiān)視需要實時確定衛(wèi)星的軌道狀態(tài),地基雷達由于具備全天時、全天候的工作能力而成為空間監(jiān)視系統(tǒng)中的一類重要的傳感器[1].由于衛(wèi)星軌道動力學模型具有較強的非線性,因此,在數(shù)據(jù)處理層面,實時定軌問題的本質便是在考慮軌道攝動的影響下,利用雷達輸出的帶噪聲的測距、測速和測角數(shù)據(jù),通過非線性濾波算法得到軌道狀態(tài)的最優(yōu)估計[2],具有重要的研究價值.

非線性卡爾曼濾波算法采用遞歸方法實現(xiàn)對狀態(tài)向量的估計,其假設后驗概率密度服從高斯分布,從時間更新與量測更新的五個關鍵積分可以得到其核心是計算形如“非線性函數(shù)×高斯概率密度函數(shù)”的多維向量積分[3],一般難以求得該積分的解析解,因此研究中經(jīng)常采用數(shù)值容積準則對其進行逼近.作為非線性濾波的一種重要方法,容積卡爾曼濾波[3,4](cubature Kalman f i lter,CKF)采用三階球面-徑向容積準則對非線性函數(shù)的高斯加權積分進行近似,具有比無跡卡爾曼濾波[5,6]更高的濾波精度和數(shù)值穩(wěn)定性,已經(jīng)在許多領域得到了廣泛的應用[7?10].為了進一步提高非線性濾波精度,Jia等[11,12]利用任意階全對稱球面插值準則和矩匹配原理,提出了五階容積卡爾曼濾波(f i fth-degree cubature Kalman f i lter,5-CKF),將CKF的精度從三階提高到五階,該算法目前已經(jīng)應用于目標跟蹤、慣性導航初始對準和多源數(shù)據(jù)融合等領域[13,14].文獻[15]將強跟蹤濾波與5-CKF相結合,利用漸消因子在線調整協(xié)方差矩陣,有效提高了算法的魯棒性.文獻[16]利用統(tǒng)計線性回歸模型近似非線性量測模型,提出了基于Huber的5-CKF算法.文獻[17]利用矩陣對角化變換代替5-CKF中的Cholesky分解,提高了濾波計算的穩(wěn)定性.上述研究雖然改進了5-CKF算法,但并沒有在本質上改進算法所采用的五階容積準則.Wang等[18]利用球面單純形變換群計算球面積分,提出了五階球面單純形-徑向容積卡爾曼濾波(f i fth-degree spherical simplex-radial cubature Kalman f i lter,5-SSRCKF),并指出在高維條件下,5-SSRCKF具有比5-CKF更高的濾波精度.Singh和Bhaumik[19]指出5-CKF和5-SSRCKF均是采用矩匹配法計算徑向積分,而該方法無法保證徑向準則的最優(yōu)性,進而選擇采用高階高斯-拉蓋爾公式計算徑向積分,提出了高階容積求積分卡爾曼濾波(high-degree cubature quadrature Kalman filter,HDCQKF).然而,上述濾波算法在提高精度的同時帶來了積分點個數(shù)的增多,且積分點個數(shù)隨著系統(tǒng)維數(shù)呈多項式增長,一旦算法應用于高維系統(tǒng),或者處理器性能較低時,可能會造成較重的計算負擔,進而降低算法的實時性.衛(wèi)星軌道的實時確定需要在一個濾波周期內實現(xiàn)對量測信號的解析并輸入濾波算法執(zhí)行,對算法的計算效率和實時應用能力具有較高的要求.因此,研究如何降低五階濾波定軌算法的計算量是十分必要的.

本文提出一種新的逼近積分點個數(shù)下限的5-CKF定軌算法,該算法在近似非線性函數(shù)的高斯加權積分時所需的積分點個數(shù)僅比五階容積準則積分點個數(shù)的理論下限多一個積分點,從而有效地降低了算法的計算量.仿真實驗結果表明,本文所提算法在定軌精度方面與已有五階濾波算法相當,但所需的積分點最少,實時性最高,從而驗證了本文算法的有效性.

2 逼近積分點個數(shù)下限的五階容積準則

考慮如下向量函數(shù)的積分,

(1)式所示的積分一般難以求得解析解,因此在研究中經(jīng)常采用被積函數(shù)在某些確定點上的函數(shù)值的加權和來對該積分進行數(shù)值逼近,構成如下容積準則,

式中,xi為積分點,ωi為積分權值,N為積分點個數(shù).研究中經(jīng)常采用代數(shù)精度來描述積分準則(2)的逼近程度,為使容積準則達到五階代數(shù)精度,需要的積分點個數(shù)的理論下限為[20]

目前在5-CKF中常見的容積準則有兩種,分別為五階球面-徑向容積準則和五階球面單純形-徑向容積準則,其積分點個數(shù)分別為2n2+1和n2+3n+3.可以看出,該兩種準則的積分點個數(shù)與理論下限尚有差距.為了進一步逼近積分點個數(shù)的下限,降低數(shù)值運算量,提高算法的實時性,文獻[20]給出了如下容積準則:

式中,μ,γ和η滿足如下等式:

該準則所需的積分點個數(shù)為n2+n+2,僅比(3)式中的理論下限多一個積分點,但該準則僅對2≤n≤7成立.該積分準則包含8個參數(shù),分別是μ,γ,η,λ,ξ,A,B和C,文獻[20]中給出了這些參數(shù)的具體值,本文僅針對實時定軌系統(tǒng)給出n=6時的參數(shù)值,列于表1.

為了將(4)式化簡為便于應用的形式,定義e為如下n階單位矩陣:

表1 容積準則參數(shù)表(n=6)Table 1.Parameters of cubature rule(n=6).

用矩陣下標i表示矩陣的第i列,利用單位矩陣e可以將(4)式寫成如下形式:

進一步地,定義如下變量

則(9)式可以簡化為如下形式:

相比較(4)式的形式,通過線性變換改寫成的(10)式的形式更便于應用于非線性濾波算法.

3 逼近積分點個數(shù)下限的5-CKF算法

考慮如下加性噪聲的非線性系統(tǒng):

式中,xk∈Rn為系統(tǒng)狀態(tài)向量,zk∈Rp為量測向量,f(xk)和h(xk)是已知的非線性函數(shù),噪聲wk∈Rn和vk∈Rp是不相關的高斯白噪聲,其協(xié)方差矩陣分別為Qk和Rk.貝葉斯濾波理論采用遞歸方法實現(xiàn)對狀態(tài)向量xk的估計,其關鍵是計算貝葉斯濾波框架中以下幾個積分[19]:

式中,N(x;)為高斯概率密度函數(shù),且隨機變量x的均值為,協(xié)方差矩陣為Px;為k時刻的先驗狀態(tài)估計,為先驗誤差協(xié)方差矩陣,為量測預測值,Pz為量測預測協(xié)方差矩陣,Pxz為交叉協(xié)方差矩陣.從(12)—(16)式可以看出,非線性卡爾曼濾波的關鍵是計算形如的多維向量函數(shù)積分,其中,g(x)為任意非線性函數(shù).又因為該積分具有如下等價表示[3]:

結合積分準則(10),可以得到非線性函數(shù)的高斯加權積分為

步驟1 濾波初始化

循環(huán)k=1,2,···,完成以下步驟.

步驟2 時間更新

計算容積點

計算容積點的非線性傳遞

計算先驗狀態(tài)估計

計算先驗誤差協(xié)方差矩陣

步驟3 量測更新

計算容積點

計算容積點的非線性傳遞

計算量測預測值

計算量測誤差協(xié)方差矩陣

計算交叉協(xié)方差矩陣

步驟4 狀態(tài)更新

計算卡爾曼濾波增益

計算后驗狀態(tài)估計

計算后驗誤差協(xié)方差矩陣

4 低軌衛(wèi)星實時定軌數(shù)學模型

4.1 衛(wèi)星軌道動力學模型

對于運行在近地軌道上的衛(wèi)星,地球J2項非球形攝動和大氣阻力攝動是衛(wèi)星所受到的最主要的攝動力.在J2000地心慣性坐標系(O-XY Z)中,考慮上述兩種攝動影響,衛(wèi)星的軌道動力學模型為

式中,cd為大氣阻力系數(shù);為衛(wèi)星面質比;ρd為大氣密度;vrel為衛(wèi)星與大氣間的相對速度,假設大氣隨著地球轉動,則有

4.2 地面雷達量測模型

雷達測量模型建立在雷達地平坐標系(OXhYhZh)中,而軌道模型建立在O-XY Z中,因此需要利用WGS84地球固連坐標系(O-XeYeZe)實現(xiàn)從O-XY Z到O-XhYhZh的轉換.假設衛(wèi)星在OXY Z中的軌道狀態(tài)為,在O-XeYeZe中的軌道狀態(tài)為T,在O-XhYhZh中的軌道狀態(tài)為T,分兩步完成軌道狀態(tài)的轉換.

步驟1 從O-XY Z到O-XeYeZe的轉換

轉換矩陣為MJW=MPwMRoMNuMPr,其中,MPr為歲差矩陣,MNu為章動矩陣,MRo為地球自轉矩陣,MPw為極移矩陣,進而可以得到

將(36)和(37)式寫成矩陣形式為

步驟2 從O-XeYeZe到O-XhYhZh的轉換轉換矩陣MHW為

式中,λ為雷達地心經(jīng)度,φ為雷達地心緯度,同時可以換算成雷達在O-XeYeZe中的地心坐標T,于是得

寫成矩陣形式為

將(40)式代入(44)式可得

于是,便得到軌道狀態(tài)從O-XY Z到OXhYhZh的轉換,如(45)式所示. 為了得到OXhYhZh中的量測值與軌道狀態(tài)的關系,將Xh和h寫成具體向量的形式為Xh=T,,則雷達的測距值R、測速值、方位角A和俯仰角E與軌道狀態(tài)之間的幾何關系為

(46)式可以寫成如下離散量測方程的形式:

5 仿真實驗與分析

首先,用兩個非線性系統(tǒng)濾波算例來驗證本文算法的性能.

算例1 考慮如下包含三角函數(shù)、乘方運算以及指數(shù)運算的三維強非線性系統(tǒng):

式中,Q=0.1,R=1,濾波初值為0=將本文提出的算法與標準CKF,5-CKF,5-SSRCKF和HDCQKF算法相對比,仿真步長為1,仿真步數(shù)為100,采用均方根誤差(root mean square error,RMSE)來描述估計精度,運行1000次Monte Carlo仿真.仿真結果如圖1所示,統(tǒng)計RMSE的平均值和算法運行時間列于表2.

圖1 (網(wǎng)刊彩色)五種算法濾波RMSE值Fig.1.(color online)RMSE of f i ve f i ltering algorithms.

表2 五種算法濾波平均RMSE值和運行耗時Table 2.Mean RMSE and execution time of f i ve f i ltering algorithms.

算例2 考慮如下非線性系統(tǒng)模型,該模型是驗證非線性濾波算法性能的常用模型,系統(tǒng)狀態(tài)模型和量測模型為

其中k=1,2,···,N.濾波初值為x0=0n×1,考慮系統(tǒng)維數(shù)n=5和n=7兩種情況,系統(tǒng)噪聲與量測噪聲均為零均值單位協(xié)方差高斯白噪聲.仿真步長為1,步數(shù)為100,執(zhí)行1000次Monte Carlo仿真,結果如圖2所示,統(tǒng)計平均RMSE和算法運行耗時,列于表3.

從以上算例1和2的結果可以看出,CKF算法執(zhí)行所需的時間最短,這是因為CKF采用三階球面-徑向容積準則,容積點個數(shù)少,但該準則僅具有三階精度,在系統(tǒng)具有強非線性時的濾波精度要低于五階濾波算法.以本文算例為例,在算例1中,相比CKF,本文算法將濾波精度提高了12.26%.在算例2中,相比CKF,本文算法將濾波精度分別提高了10.91%和5.72%.算例中對比的四種五階算法的濾波精度相當,而由于本文采用的五階容積準則逼近容積點個數(shù)的下限,因此本文算法在保持五階濾波精度的同時具有最高的計算效率.

在通過上述兩個算例驗證本文算法性能之后,將本文算法應用于地基實時定軌之中,圖3所示為衛(wèi)星地面模擬器,其上運行高精度軌道預報算法,該算法考慮了地球高階非球形引力、大氣阻力、太陽光壓、三體引力和潮汐等攝動影響.其中,考慮21階地球模型,大氣阻力系數(shù)cd=2.2,衛(wèi)星面質比為0.02 m2/kg,大氣密度模型采用Jacchia-Roberts模型,太陽光壓系數(shù)cr=1.實驗中考慮衛(wèi)星的軌道歷元為2016年9月1日16:00:00(UTC),軌道六根數(shù)分別為

圖2 (網(wǎng)刊彩色)五種算法濾波RMSE值 (a)五維系統(tǒng);(b)七維系統(tǒng)Fig.2.(color online)RMSE of f i ve f i ltering algorithms:(a)Five dimension system;(b)seven dimension system.

表3 五種算法濾波平均RMSE值和運行耗時Table 3.Mean RMSE and execution time of f i ve f i ltering algorithms.

圖3 衛(wèi)星模擬器Fig.3.Satellite simulator.

雷達經(jīng)緯度和衛(wèi)星過境時間如圖4所示,雷達最低測量仰角為10°,假設雷達測距誤差為60 m,測速誤差為0.1 m/s,測角誤差為0.02°.濾波初值為

前述算例已經(jīng)表明,五階算法的濾波精度要高于標準CKF,故此處不再考慮CKF算法,而將本文提出的算法與5-CKF,5-SSRCKF和HDCQKF算法進行對比,用RMSE來評價定軌精度,運行200次Monte-Carlo仿真,結果如圖5—圖8所示,統(tǒng)計300—530 s內平均定位RMSE和平均定速RMSE并列于表4,可以看出,四種算法的收斂速度基本一致,定軌精度基本相同.

對比四種算法的積分點個數(shù)和運行200次Monte-Carlo仿真所需的時間列于表5,可見在同等定軌精度的條件下,本文提出的算法所需的積分點個數(shù)最少,算法運行所需的時間最短,因此實時性最高.

圖4 實時定軌示意圖Fig.4.Schematic diagram of orbit determination.

表4 300—530 s平均定軌RMSETable 4.Mean RMSE of orbit determination during 300–530 s.

表5 三種算法積分點個數(shù)與執(zhí)行時間Table 5.The number of quadrature points and execution time of three algorithm.

圖5 (網(wǎng)刊彩色)1—10 s定軌RMSE (a)位置RMSE;(b)速度RMSEFig.5.(color online)RMSE of orbit determination during 1–10 s:(a)Position RMSE;(b)velocity RMSE.

圖6 (網(wǎng)刊彩色)150—200 s定軌RMSE (a)位置RMSE;(b)速度RMSEFig.6.(color online)RMSE of orbit determination during 150–200 s:(a)Position RMSE;(b)velocity RMSE.

圖7 (網(wǎng)刊彩色)300—350 s定軌RMSE (a)位置RMSE;(b)速度RMSEFig.7.(color online)RMSE of orbit determination during 300–350 s:(a)Position RMSE;(b)velocity RMSE.

圖8 (網(wǎng)刊彩色)450—500 s定軌RMSE (a)位置RMSE;(b)速度RMSEFig.8.(color online)RMSE of orbit determination during 450–500 s:(a)Position RMSE;(b)velocity RMSE.

6 結 論

本文提出了一種新的逼近積分點個數(shù)下限的5-CKF定軌算法,采用一種數(shù)值容積準則對非線性函數(shù)的高斯加權積分進行近似,并在貝葉斯濾波算法框架下推導出算法的更新步驟.該算法所需的積分點個數(shù)為n2+n+2,比五階代數(shù)精度容積準則的積分點個數(shù)理論下限僅多一個,相比已有的五階容積濾波算法,比5-CKF少n2?n?1,比5-SSRCKF少2n+1,可見該算法降低了計算的復雜度,有效提高了算法的實時性,且隨著系統(tǒng)狀態(tài)維數(shù)的增加,該算法的優(yōu)勢更加明顯.在地基雷達對低軌衛(wèi)星實時定軌的仿真實驗中,本文算法的定軌精度與5-CKF,5-SSRCKF算法定軌精度相當,定位精度均為22 m左右,定速精度均為0.11 m/s左右,但算法的運行時間最短,運行200次Monte-Carlo仿真相比5-CKF少了113.91 s,相比5-SSRCKF少了46.35 s,從而驗證了該算法的有效性.雖然該算法在保持五階濾波精度的同時具有相比其他算法更高的計算效率,但僅針對系統(tǒng)維數(shù)2≤n≤7時有效,然而該維數(shù)范圍已經(jīng)可以滿足大部分的應用需求.

[1]Ning X,Ye C M,Yang J,Shen B 2014Chin.J.Radio29 27(in Chinese)[寧夏,葉春茂,楊健,沈彬2014電波科學學報29 27]

[2]Abhinoy K S,Shovan B 2014IEEE International Symposium on Signal Processing and Information TechnologyNoida,India,December 15–17,2014 p114

[3]Arasaratnam I,Haykin S 2009IEEE Trans.Autom.Control54 1254

[4]Jafar Z,Ehsan S 2015IET Sci.Meas.Technol.9 294

[5]Julier S J,Uhlmann J K,Whyte H F D 2000IEEE Trans.Autom.Control45 477

[6]Xiong K,Zhang H Y,Chan C W 2006Automatica42 261

[7]Zhang L J,Yang H B,Lu H P,Zhang S F,Cai H,Qian S 2014Acta Astronaut.105 254

[8]Chen J G,Wang N,Ma L L,Xu B G 2015IET Radar Sonar Navig.9 324

[9]Lu Z Y,Wang D M,Wang J H,Wang Y 2015Acta Phys.Sin.64 150502(in Chinese)[逯志宇,王大鳴,王建輝,王躍2015物理學報64 150502]

[10]Wu H,Chen S X,Yang B F,Chen K 2015Acta Phys.Sin.64 218401(in Chinese)[吳昊,陳樹新,楊賓峰,陳坤2015物理學報64 218401]

[11]Jia B,Xin M,Cheng Y 2012Automatica49 510

[12]Jia B,Xin M,Cheng Y 2012IEEE Conference on Decision and ControlMaui Hawaii,USA,December 10–13,2012 p4095

[13]Huang X Y,Tang X Q,Wu M 2015Syst.Eng.Electron.37 633(in Chinese)[黃湘遠,湯霞清,武萌2015系統(tǒng)工程與電子技術37 633]

[14]Zhang X C 2014Circuits Syst.Signal Process33 1799[15]Zhang L,Cui N G,Wang X G,Yang F,Lu B G 2015Acta Aeronaut.Astronaut.Sin.36 3885(in Chinese)[張龍,崔乃剛,王小剛,楊峰,盧寶剛2015航空學報36 3885]

[16]Zhang W J,Wang S Y,Feng Y L,Feng J C 2016Acta Phys.Sin.65 088401(in Chinese)[張文杰,王世元,馮亞麗,馮久超2016物理學報65 088401]

[17]Zhao L Q,Chen K Y,Wang J L,Yu T 2016Control Decis.31 1080(in Chinese)[趙利強,陳坤云,王建林,于濤2016控制與決策31 1080]

[18]Wang S Y,Feng J C,Tse C K 2014IEEE Signal Process.Lett.21 43

[19]Singh A K,Bhaumik S 2015Int.J.Control Autom.Syst.13 1097

[20]Lu J,Darmofal D L 2004SIAM J.Sci.Comput.26 613