修正擴維無跡卡爾曼濾波算法研究

任曉斌，聶桂根，李連艷，陳祖岸，雷浩川

(1. 武漢大學(xué)衛(wèi)星導(dǎo)航定位技術(shù)研究中心，湖北 武漢 430079； 2. 武漢大學(xué)土木建筑工程學(xué)院，湖北 武漢430079； 3. 恩施州城鄉(xiāng)規(guī)劃管理局， 湖北 巴東 444300； 4. 青海大學(xué)地質(zhì)工程系，青海 西寧 810016)

X射線脈沖星是一種高速自轉(zhuǎn)的中子星，毫秒脈沖星由于其自轉(zhuǎn)周期穩(wěn)定性極佳，常常用于導(dǎo)航中。X射線脈沖星導(dǎo)航作為新興的導(dǎo)航方式，具有良好的發(fā)展前景。在導(dǎo)航定位中，如何選擇合適的濾波器，建立恰當(dāng)?shù)脑肼暷Ｐ蛯⒅苯犹岣邽V波效果，這對于高精度X射線脈沖星導(dǎo)航精度的提高具有重要意義。

目前，研究人員作了大量的濾波處理工作。文獻[1]采用最小二乘(least square,LS)算法和KF算法，在星表誤差與時間誤差遞增的條件下，仿真了X、Y、Z方向的位置誤差，誤差在186.1～2 571.1 m之間。文獻[2]運用EKF算法仿真后，得到在1 μs的TOA估計誤差下且不考慮其他誤差時的位置誤差為40.63 m，速度誤差為0.019 m/s；運用無跡卡爾漫濾波(unscented Kalman filter，UKF)在上述條件下的位置誤差為28.48 m，速度誤差為0.012 4 m/s。文獻[3]運用擴維無跡卡爾漫濾波(augmented state unscental Kalman filter，ASUKF)算法進行仿真，位置誤差為199.4 m，速度誤差為0.16 m/s。文獻[4]應(yīng)用CDKF算法進行仿真，位置誤差為145.2 m。

本文主要針對脈沖星導(dǎo)航的基本原理，建立脈沖星導(dǎo)航的狀態(tài)方程和測量方程；然后基于傳統(tǒng)ASUKF，建立修正擴維無跡卡爾漫濾波(MASUKF)；最后將該算法與X射線脈沖星導(dǎo)航相結(jié)合，進行航天器的位置和速度仿真。

1 基于UKF、ASUKF的濾波算法

1.1 脈沖星導(dǎo)航的原理

X射線脈沖星導(dǎo)航是利用3顆及3顆以上的脈沖星，測量脈沖星到達(dá)航天器的時間與到達(dá)太陽系質(zhì)心的時間之差，并通過航天器沿脈沖星與太陽系位置的幾何關(guān)系，分別確定航天器沿脈沖星至太陽系視線方向的距離，經(jīng)過處理后得到航天器在以太陽系質(zhì)心為原點的坐標(biāo)系的三維坐標(biāo)。

脈沖星導(dǎo)航的航天器動力學(xué)方程為

2n·rn·b-2b·r]

(1)

式中，tSSB為脈沖到達(dá)SSB的時間；tSC為脈沖到達(dá)航天器的時間，即TOA(time of arrival)；D0為脈沖星到SSB的相對距離；c代表光速；b為SSB相對于太陽系中心的位置矢量,b代表該矢量的數(shù)值大??；n為單位矢量，近似認(rèn)為是常量；r為航天器在SSB中位置矢量，r代表該矢量的數(shù)值大?。籺SSB與tSC之差代表航天器在沿脈沖星的視線距離方向的投影。右側(cè)表達(dá)式中：第1項為一階Doppler延遲，第2項由周年視差引起，前兩項統(tǒng)稱Roamer延遲，第3項為Shapiro延遲。

在脈沖星導(dǎo)航中，主要誤差源有行星星歷誤差、角位置誤差、脈沖星距離誤差等。而所有的誤差項都與TOA有關(guān)，因此，一個合適的濾波方法可以更為準(zhǔn)確地確定TOA。

1.2 UKF的基本理論

UKF是一種將無損變換(UT)和標(biāo)準(zhǔn)Kalman濾波體系相結(jié)合的算法，它通過無損變換使非線性系統(tǒng)方程適用于在線性假設(shè)下的標(biāo)準(zhǔn)Kalman濾波體系。UKF算法的核心是UT變換，UT變換可以通過構(gòu)建Sigma采樣點獲得樣本的方差與均值，進而較準(zhǔn)確地估計整體的方差和均值，從而可以大幅降低位置誤差、速度誤差。UKF濾波可以消除部分Roamer延遲、Shapiro效應(yīng)、Einstein效應(yīng)所帶來的位置誤差與速度誤差。

1.2.1 UKF算法

UKF算法是以UT變換為基礎(chǔ)，采用卡爾曼濾波器的框架，并運用對稱性采樣法進行濾波的算法。其核心思想為：獲取采樣點(即Sigma點)并計算其均值、方差和權(quán)，然后根據(jù)采樣值模擬計算整體的均值與方差。UKF算法的步驟如下：

(1) 構(gòu)建狀態(tài)方程和測量方程，分別如下

xk+1=f(xk)+wk

(2)

yk=h(xk)+vk

(3)

在確定狀態(tài)方程和測量方程的誤差項wk、vk時，需要假定系統(tǒng)狀態(tài)的噪聲滿足高斯白噪聲。

(2) 初始化，即

(4)

(3) 計算2n+1個Sigma點及其權(quán)值，即

(5)

(4) 計算Sigma點通過非線性函數(shù)的傳播結(jié)果，即

Yi=f(Xi)i=0,1,…,2n

(6)

(7)

1.2.2 基于UKF的X射線脈沖星導(dǎo)航算法

基于UKF的脈沖星導(dǎo)航，即將式(2)和式(3)的狀態(tài)方程和觀測方程轉(zhuǎn)為脈沖星的狀態(tài)與觀測方程，脈沖星的測量方程如式(1)所示，狀態(tài)方程為

f(x(t),t)=

(8)

基于UKF的X射線脈沖星導(dǎo)航算法修正了部分因TOA引起的系統(tǒng)誤差，在下面的仿真試驗中，也可得到位置誤差與速度誤差的估計。但UKF算法假設(shè)狀態(tài)方程與量測方程均為高斯白噪聲，且并未考慮噪聲的其他特性，也未針對噪聲本身單獨進行建模分析，因此針對誤差項加以修正是降低誤差的下一步任務(wù)。

1.3 擴維無味卡爾曼濾波(ASUKF)

1.3.1ASUKF原理

ASUKF在傳統(tǒng)的脈沖星信號獲取中，在視線方向ni時不能準(zhǔn)確獲得視線方向，因此脈沖星的方向誤差、角位置誤差等會大量積累。ASUKF在UKF的基礎(chǔ)上，將變量的個數(shù)繼續(xù)擴大，可基于多個測量值在不同維度下進行，可以大大減少由于方向誤差帶來的偏差，從而使其更加貼近實際情況。

1.3.2ASUKF算法

ASUKF算法是在UT變換的基礎(chǔ)上，通過修改誤差項獲取更接近真值的采樣點，然后運用對稱性采樣的方法進行濾波的算法。其根本思路也是通過采樣值模擬計算整體的均值與方差；而最大的不同點在于加入誤差項后狀態(tài)方程與測量方程發(fā)生了變化，從而導(dǎo)致采樣點發(fā)生改變。

(1) 狀態(tài)方程：在原UKF的基礎(chǔ)上增加了3個誤差項B1、B2、B3，即

在微課內(nèi)容上，要保證知識的系統(tǒng)性并做到有始有終。在課程末尾進行一個簡明扼要的總結(jié)，使知識結(jié)構(gòu)更加立體。同時幫助學(xué)生進行知識點的回顧總結(jié)，加深所學(xué)知識印象。教學(xué)者在課程內(nèi)容安排上還要做到由淺入深、循序漸進，使學(xué)習(xí)者能夠接受課程安排。針對重點內(nèi)容，可以合理創(chuàng)設(shè)問題情景并確定啟發(fā)性論題，建立為理解而教的目標(biāo)，使學(xué)生對內(nèi)容產(chǎn)生濃厚的興趣，主動做課堂的參與者。

X(t)=[xyzvxvyvzB1B2B3]

(9)

(10)

(2) 觀測方程考慮系統(tǒng)偏差后方程變?yōu)?/p>

z(t)=h(X(t))+Bi

(11)

(3) 誤差項：該誤差項主要修正方向誤差和由于Roamer延遲引起的誤差

(12)

1.4 修正擴維無味卡爾曼濾波(MASUKF)

1.4.1MASUKF原理

MASUKF在ASUKF的基礎(chǔ)上修改了誤差項，考慮到Roamer延遲的高階項，同時在ASUKF的基礎(chǔ)上提高了算法的運行效率，縮短了算法的運行時間。

1.4.2 MASUKF算法

MASUKF算法的基本思路與ASUKF算法一致，也將系統(tǒng)偏差看作系統(tǒng)狀態(tài)的3個變量。因此狀態(tài)方程(系統(tǒng)軌道動力學(xué)方程)和觀測方程中均含有系統(tǒng)偏差。在實際中通常需要將方向誤差展開至由于赤經(jīng)赤緯測量誤差所帶來的角位置誤差

(13)

(14)

2 仿真試驗

UKF與ASUKF算法在濾波中精度最高，利用兩種方法分別進行位置誤差的模擬與航天器位置的仿真。在澳大利亞國家天文臺官網(wǎng)下載并獲取脈沖星的數(shù)據(jù)。

部分?jǐn)?shù)據(jù)如下：航天器位于繞地的MEO衛(wèi)星，質(zhì)量為1403 kg，天線相位中心偏差分別為0.641、-0.005、1.177 m。起始航天器在J2000.0坐標(biāo)系下的位置X、Y、Z分別為-16 242 041.827、-5 242 312.694、22 071 892.826 m，速度分別為-2669、-3 676.998、-887.52 m/s。長半軸a=27 908 632.497 m，e=0.000 5，軌道的傾角i=55.521°，升交點赤經(jīng)Ω=80.526°，近地點角距ω=165.303°，平近點角M=301.079°。

2.1 ASUKF與UKF算法比較

仿真時間為2008年2月13:00—2008年2月14:00，間隔為1 min。根據(jù)上文中設(shè)計的UKF與ASUKF濾波器分別仿真航天器的坐標(biāo)值。其中，在t時刻航天器在地心慣性坐標(biāo)系J2000.0內(nèi)的坐標(biāo)值可由精密星歷擬合得到，通過仿真試驗，即可得到UKF與ASUKF的位置誤差與速度誤差。

2.2 仿真結(jié)果分析

選取上述脈沖星進行航天器導(dǎo)航，分別將狀態(tài)方程與測量方程進行速度誤差與位置誤差的估計，模擬在150 s的步長下UKF與ASUKF算法的誤差。結(jié)果如圖1所示。灰色表示UKF，黑色表示ASUKF。結(jié)果顯示，UKF算法在x方向的定位精度約為172 m,ASUKF算法在x方向的定位精度約為102 m。ASUKF較UKF算法估計的位置誤差與速度誤差均較小，雖然位置誤差與速度誤差存在突變點與極值點，但二者均可收斂，在一定精度要求下可滿足條件。

原來位置為：-16 242 041.827、-5 242 312.694、22 071 892.826 m，經(jīng)脈沖星動力學(xué)方程導(dǎo)航后確定的航天器位置為-16 241 258.631、-5 242 826.321、22 072 760.739 m，兩種導(dǎo)航方法的位置誤差及速度誤差分別見表1、表2。

表1 兩種導(dǎo)航方法的位置誤差 m

表2 兩種導(dǎo)航方法的速度誤差 (m/s)

將航天器的真實狀態(tài)(-16 242 041.827，-5 242 312.694，22 071 892.826)加入UKF與ASUKF算法中，可以直觀進行濾波前后航天器位置的對比。模擬在航天器x方向速度1 km/s，y方向速度1 km/s，z方向1 km/s的情況下的濾波軌跡。結(jié)果如圖2—圖5所示。

在航天器運動狀態(tài)確定的情況下，ASUKF均能較好地模擬航天器的運動狀態(tài)，其精度遠(yuǎn)遠(yuǎn)大于UKF算法。在構(gòu)建誤差項時考慮角位置誤差，因而運用ASUKF算法進行估計航天器真實位置的精度較高。

2.3 ASUKF與MASUKF算法比較

在加入誤差項Bi后，對航天器進行仿真分析，模擬航天器在3方向初始運動為勻速運動的狀態(tài)。為提高仿真精度，仿真步長為10 000 s。選x方向分析其位置誤差(y、z方向類似)，加入誤差項后的模擬結(jié)果如圖6、圖7所示。

由圖6、圖7可知，在仿真步長10 000 s時，MASUKF算法的位置誤差均值與方差小于ASUKF算法。在勻速運動下，MASUKF的位置誤差均值約為99.43 m，略小于ASUKF算法的100.21 m；MASUKF的速度誤差均值約0.001 25 m/s，略小于ASUKF算法的0.001 29 m/s。而ASUKF算法的均方差也小于MASUKF算法，因此MASUKF算法在模擬過程中的性能更佳。

2.4 3種算法的運行時間對比

在Matlab中對3種算法分別設(shè)置起止時間，查看3種算法的運行時間，如圖8所示。

由于UKF算法沒有考慮誤差項，精度相對較低、計算量較小，因此其運行時間最短。而MASUKF相較ASUKF算法而言顯著地提高了0.975 s，大大節(jié)省了程序的運行時間。

3 結(jié) 語

本文主要研究了具有脈沖星方向誤差的ASUKF導(dǎo)航濾波算法的仿真實現(xiàn)，并自行設(shè)計了MASUKF算法進行試驗。首先分析了脈沖星方向誤差產(chǎn)生的原因及其對導(dǎo)航性能的影響，比較了UKF、ASUKF和MASUKF算法的原理，并在近地軌道上對該方法進行了仿真驗證。仿真結(jié)果表明，ASUKF算法較UKF算法而言精度較高。而與ASUKF濾波算法相比，MASUKF算法的導(dǎo)航定位、測速精度明顯更高,運行時間更短，是一種更好的算法。