基于雙因子抗差卡爾曼濾波在動(dòng)態(tài)導(dǎo)航中的應(yīng)用

龍仁波,高井祥,王 堅(jiān)

(1.中國礦業(yè)大學(xué)國土環(huán)境與災(zāi)害監(jiān)測(cè)國家測(cè)繪局重點(diǎn)實(shí)驗(yàn)室,江蘇徐州221116 2.中國礦業(yè)大學(xué)江蘇省資源環(huán)境信息工程重點(diǎn)實(shí)驗(yàn)室,江蘇徐州221116)

0 引 言

卡爾曼濾波理論是一種對(duì)動(dòng)態(tài)系統(tǒng)進(jìn)行數(shù)據(jù)處理的有效方法,它利用觀測(cè)向量來估計(jì)隨時(shí)間不斷變化的狀態(tài)向量。由于其在對(duì)狀態(tài)向量進(jìn)行估計(jì)時(shí),不需要存儲(chǔ)大量的歷史觀測(cè)數(shù)據(jù),利用新的觀測(cè)值,通過不斷的預(yù)測(cè)和修正,即可估計(jì)出系統(tǒng)新的狀態(tài)。因此,卡爾曼濾波被廣泛地應(yīng)用于各種動(dòng)態(tài)測(cè)量系統(tǒng)中,如GPS實(shí)時(shí)動(dòng)態(tài)定位等。但是,標(biāo)準(zhǔn)卡爾曼濾波理論對(duì)動(dòng)態(tài)系統(tǒng)提出了嚴(yán)格的要求,要求系統(tǒng)噪聲和觀測(cè)噪聲為零均值白噪聲。這一條件在實(shí)踐中難以滿足,致使濾波結(jié)果失真。因此,人們提出了克服這一缺陷的許多方法,如抗差濾波方法[1]、粗差探測(cè)方法[2]等。由非隨機(jī)誤差引起的模型誤差,也可采用文獻(xiàn)[3]中的方法加以解決,即首先識(shí)別模型誤差的類型,然后進(jìn)行相應(yīng)的補(bǔ)償。近來有學(xué)者提出利用自適應(yīng)因子控制Kalman濾波中擾動(dòng)異常對(duì)狀態(tài)估值的影響[4-5],并已在GPS導(dǎo)航、GIS道路修測(cè)、航空導(dǎo)航及低軌衛(wèi)星定軌中得到成功應(yīng)用[6]。自適應(yīng)濾波根據(jù)自適應(yīng)因子作用范圍不同又分為單因子自適應(yīng)濾波和多因子自適應(yīng)濾波[7-8]。在此,將雙因子抗差卡爾曼濾波引入到動(dòng)態(tài)導(dǎo)航中,通過實(shí)測(cè)數(shù)據(jù)模擬粗差進(jìn)行處理,取得了良好的結(jié)果。

1 標(biāo)準(zhǔn)卡爾曼濾波模型

動(dòng)力學(xué)系統(tǒng)可以由狀態(tài)方程和觀測(cè)方程表示,分別為

其中xk為k(k=1,2,3,……)歷元時(shí)刻的nk×1狀態(tài)參數(shù)向量,Φk/k-1為狀態(tài)轉(zhuǎn)移矩陣,ωk為高斯白噪聲過程控制誤差向量;而zk為觀測(cè)向量,Hk為觀測(cè)矩陣,Vk為觀測(cè)噪聲向量,其服從高斯分布,且滿足:

其中矩陣Q0,Qk,Rk已知,Q0>0,Rk>0;i,j,k=1,2,…,and i≠j.為了估計(jì)狀態(tài)參數(shù)xk,不僅需要觀測(cè)值,還需要的預(yù)測(cè)值,這里可以有狀態(tài)模型(1),通過下式可得xk;

通過綜合觀測(cè)值z(mì)k和狀態(tài)參數(shù)的預(yù)測(cè)值,即可得狀態(tài)參數(shù)的最優(yōu)估值。鑒于卡爾曼濾波流程很多文章都已經(jīng)提到,這里就不一一列出。

2 抗差卡爾曼濾波模型

標(biāo)準(zhǔn)濾波與抗差濾波的狀態(tài)參數(shù)向量估值表達(dá)形式一致,不同之處使用的權(quán)陣不同[9],標(biāo)準(zhǔn)卡爾曼濾波解使用原始的觀測(cè)信息和預(yù)報(bào)信息的權(quán)矩陣;雙因子抗差濾波解使用觀測(cè)信息和預(yù)報(bào)信息的等價(jià)權(quán)矩陣。

在抗差濾波中,觀測(cè)權(quán)陣元素的確定是整個(gè)算法的關(guān)鍵問題,觀測(cè)等價(jià)權(quán)元素直接反應(yīng)了抗差濾波算法對(duì)觀測(cè)異常值的控制能力。

雙因子抗差濾波等價(jià)元素的計(jì)算方法,先有觀測(cè)信息獲得位置參數(shù)的抗差估計(jì)值

為保持權(quán)矩陣的對(duì)稱性和相關(guān)性不變,我們將使用相關(guān)抗差估計(jì)理論[10-11]來獲取觀測(cè)向量的等價(jià)權(quán)元素,即

rii和rjj為降權(quán)因子,其計(jì)算過程可見文獻(xiàn)[10],可定義為

通常k0=1.0～1.5,k1=2.5～8.0,該模型與IGG3模型的三段法類似,但是該模型的優(yōu)點(diǎn)是保證了相關(guān)觀測(cè)值的對(duì)稱性,簡(jiǎn)化了參數(shù)抗差解的求取,使得驗(yàn)后協(xié)方差矩陣的表示相對(duì)簡(jiǎn)單。

在卡爾曼濾波中,當(dāng)觀測(cè)值中含有粗差時(shí),可以根據(jù)上面確定的等價(jià)權(quán)原理,通過分析增益矩陣,選取適當(dāng)?shù)臋?quán)函數(shù)代替觀測(cè)噪聲協(xié)方差陣,以減小或消除粗差對(duì)估計(jì)結(jié)果的影響。當(dāng)?shù)葍r(jià)權(quán)確定之后,重新利用最小二乘原理,解算參數(shù)估值。

3 實(shí)例分析

本文采用的數(shù)據(jù)采集于2005年某大學(xué)的校園,使用Leica 500 GPS接收機(jī),1秒采樣GPS偽距單點(diǎn)定位序列進(jìn)行算法測(cè)試,取其中500個(gè)歷元的運(yùn)動(dòng)軌跡作為測(cè)試數(shù)據(jù)(圖1),建立的動(dòng)力學(xué)模型為

觀測(cè)方程:

為了檢驗(yàn)抗差估計(jì)的效果,在100,200,400歷元處衛(wèi)星的粗捕獲碼(C/A)觀測(cè)值加入20 m的粗差,其位置、速度、C/A和P2的初始方差分別為0.2 m2,9×10-6m2s-2、1 m2和1m2,然后分別采用標(biāo)準(zhǔn)卡爾曼濾波及抗差卡爾曼濾波進(jìn)行解算,表1列出了兩種方案的不同指標(biāo)量。

圖1 運(yùn)動(dòng)軌跡圖

表1 兩種方案的不同指標(biāo)比較/m

圖2與圖3分別給出X方向標(biāo)準(zhǔn)Kalman濾波與抗差Kanman濾波模型的濾波前后X方向軌跡及殘差序列。圖2可以看出粗差對(duì)濾波序列與殘差序列都存在明顯的影響。而圖3中,抗差模型已經(jīng)消除了粗差對(duì)濾波結(jié)果的影響,殘差序列表明粗差對(duì)參數(shù)估計(jì)的影響已經(jīng)被消除,充分說明抗差模型的有效性,Y與Z方向結(jié)果類似,這里就不一一列出。

從表1可以看出,采用抗差卡爾曼濾波導(dǎo)航精度得到了提高,這說明抗差卡爾曼濾波不僅消除了粗差對(duì)導(dǎo)航的影響,而且可以降低系統(tǒng)的偶然誤差對(duì)參數(shù)估值的影響。

4 結(jié) 論

構(gòu)造了基于雙因子抗差權(quán)的Kalman濾波模型,此模型在考慮相關(guān)觀測(cè)值的情況下,可有效消除粗差對(duì)導(dǎo)航結(jié)果的影響,并且可以降低系統(tǒng)偶然對(duì)參數(shù)估值的影響;實(shí)驗(yàn)數(shù)據(jù)研究表明該方法可行;在研究中發(fā)現(xiàn),初始參數(shù)的確定對(duì)于模型的應(yīng)用效果影響較大。

[1] Koch K R,Yang Yuan-xi.Robust Kalman filter for rank deficient observation models[J].Journal of Geodesy,1998,72(8):436-441.

[2] Liu Xiang-lin,Liu Jin-nan,Du Dao-shang,et al.Reliability analysis for Kalman filtering and its application in kinematic GPS positioning[J].Journal of Wuhan Technical University of Surveying and Mapping(WTUSM),1997,22(3):234-236..

[3] Tao Ben-zao.Identification of Kalman filtering model errors[J].Crustal Deformation and Earthquake,1999,19(4):33-235.

[4] 楊元喜,何海波,徐天河.論動(dòng)態(tài)自適應(yīng)濾波[J].測(cè)繪學(xué)報(bào),2001,30(4):293-298.

[5] Yang Yuan-xi,He Hai-bo,Xu Guo-chang.Adaptively robust filtering for kinematic geodetic Positioning[J].Journal of geodesy,2001,75(2/3):109-116.

[6] 楊元喜,文援蘭.衛(wèi)星精密軌道綜合自適應(yīng)抗差濾波技術(shù)[J].中國科學(xué):D輯,2003,32(11):1112-1119.

[7] Yang Yuan-xi,Gao Wei-guang.A new learning statistic for adaptive filter based on predicted residuals[J].Progress in Natural Science,2006,16(5):833-837.

[8] Cui Xian-qiang,Yang Yuan-xi.Adaptively robust filtering with classified adaptive factors[J].Progress in Natural Science,2006,16(8):841-846.

[9] 崔先強(qiáng),楊元喜.分類因子自適應(yīng)抗差濾波[J].自然科學(xué)進(jìn)展,2006,16(4):490-494.

[10] 楊元喜,宋力杰,徐天河.大地測(cè)量相關(guān)觀測(cè)抗差估計(jì)理論[J].測(cè)繪學(xué)報(bào),2002,31(2):95-99.

[11] Yang Yuan-xi,Song Li-jie,Xu Tian-he.Robust estimator for correlated observations based on bifactors equivalent weights[J].Journal of Geodesy,2002(76):353-358.