鄒 敏，王國棟，劉 超

(1.安徽理工大學(xué) 測繪學(xué)院，安徽 淮南232001；2.浙江有色測繪院，浙江 紹興312000)

?

抗差自適應(yīng)Kalman濾波及其在GNSS導(dǎo)航中的應(yīng)用

鄒 敏1，王國棟2，劉 超1

(1.安徽理工大學(xué) 測繪學(xué)院，安徽 淮南232001；2.浙江有色測繪院，浙江 紹興312000)

針對Kalman濾波易受粗差影響而導(dǎo)致結(jié)果失真的問題，提出一種抗差自適應(yīng)Kalman濾波方法，該方法結(jié)合自適應(yīng)濾波與抗差Kalman濾波的優(yōu)點(diǎn)，同時設(shè)計(jì)自適應(yīng)因子和抗差因子，采用改進(jìn)的兩段Huber函數(shù)與2～3倍的觀測噪聲中誤差來充當(dāng)抗差因子與粗差判別標(biāo)準(zhǔn)。并對Kalman濾波和抗差自適應(yīng)濾波(Adaptive Robust Kalman Filtering, ARKF)結(jié)果進(jìn)行比較。車載實(shí)驗(yàn)結(jié)果表明，ARKF可以有效抵制觀測異常對狀態(tài)估值的影響，同時在系統(tǒng)先驗(yàn)信息不能精確給出的情況下，顯著改善了濾波估值的穩(wěn)定性和可用性。

Kalman濾波；粗差；抗差自適應(yīng)Kalman濾波；GNSS

Kalman濾波(Kalman Filtering，KF)在GPS導(dǎo)航領(lǐng)域已得到了廣泛的應(yīng)用，只要給出正確的噪聲特性，KF將給出濾波狀態(tài)的最優(yōu)估計(jì)。事實(shí)上，動力學(xué)模型給出的先驗(yàn)信息不足以表征載體真實(shí)的運(yùn)動狀態(tài)。這樣給出的KF估計(jì)值不再是最優(yōu)估計(jì)值，甚至可能導(dǎo)致濾波發(fā)散[1-2]。同時，隨著導(dǎo)航衛(wèi)星系統(tǒng)諸如GLONASS、Galileo、我國北斗二代等的不斷加入，粗差發(fā)生的幾率必將不斷增加。這些都是導(dǎo)致KF結(jié)果失真的原因，無法滿足高精度GNSS導(dǎo)航的需要。針對KF存在的問題，很多學(xué)者對其做了一些改進(jìn)，主要集中在以下幾方面：自適應(yīng)濾波[3-5](Adaptive Kalman Filtering，AKF)、抗差Kalman濾波[6-9](Robust Kalman Filtering，RKF)[10-12]、抗差自適應(yīng)Kalman濾波[13-14]( Adaptive Robust Kalman Filtering，ARKF)。此外，為了獲得正確的系統(tǒng)狀態(tài)估計(jì)，許多學(xué)者還提出了以擴(kuò)展Kalman濾波[15]、無跡Kalman濾波[16]、小子域?yàn)V波[17]等為例的非線性濾波方法。現(xiàn)有的ARKF的文獻(xiàn)中抗差和自適應(yīng)因子較多采用三段函數(shù)，粗差判斷標(biāo)準(zhǔn)也是根據(jù)經(jīng)驗(yàn)值，這有可能導(dǎo)致觀測噪聲或狀態(tài)噪聲協(xié)方差陣奇異，得不到最優(yōu)的濾波估值。因此，本文采用改進(jìn)的兩段Huber函數(shù)來設(shè)計(jì)抗差因子，粗差判別標(biāo)準(zhǔn)采用2～3倍的觀測噪聲中誤差，充分利用當(dāng)前觀測信息。對不同速度譜密度情況下的Kalman濾波和抗差自適應(yīng)濾波進(jìn)行比較，同時對加入粗差前后的KF和ARKF也進(jìn)行比較。

1　Kalman濾波

給定離散化的線性動力學(xué)系統(tǒng)方程和觀測方程：

(1)

式中，xk表示第k歷元n×1階狀態(tài)向量；φk,k-1為n×n階狀態(tài)轉(zhuǎn)移矩陣；wk為n×1階系統(tǒng)隨機(jī)過程噪聲向量；zk表示m×1階觀測向量；Hk是m×n階設(shè)計(jì)矩陣；vk為m×1階隨機(jī)觀測噪聲向量。

設(shè)Pk為殘差向量的協(xié)方差矩陣，Qk和Rk分別為噪聲向量wk和vk的協(xié)方差陣，則Kalman濾波的遞歸過程如圖1所示。其中，+表示濾波估計(jì)值；-表示預(yù)測值。

2　抗差自適應(yīng)Kalman濾波

2.1抗差因子模型

抗差因子的確定基于等價權(quán)原理，即通過權(quán)函數(shù)構(gòu)造抗差因子，對存在粗差的觀測值進(jìn)行降權(quán)處理，降低異常觀測在參數(shù)估計(jì)中的份額。為了克服判別條件采用經(jīng)驗(yàn)值的盲目性，充分利用當(dāng)前觀測信息，對Huber權(quán)函數(shù)的判別條件進(jìn)行改進(jìn)：

(2)

式中，c取2σ～3σ0(σ0為觀測噪聲的中誤差)。而

(3)

(4)

2.2自適應(yīng)因子模型

自適應(yīng)因子模型[18]分為歸零函數(shù)和非歸零降函數(shù)。所謂歸零函數(shù)，即當(dāng)判別統(tǒng)計(jì)量大于某個極值時，自適應(yīng)因子為零；而非歸零降函數(shù)指的是，自適應(yīng)因子隨著判別統(tǒng)計(jì)量增大而下降，但不為零。為了防止自適應(yīng)因子取零情況下預(yù)測狀態(tài)協(xié)方差陣奇異，本文選取了類似于Huber函數(shù)的兩段函數(shù)自適應(yīng)因子：

(5)

式中，k為常數(shù)，k=1.0～2.5；Δxk為狀態(tài)不符值統(tǒng)計(jì)量。

式中，Q′為速度的譜密度。

將位置的初始方差固定為0.2m2，分別選取不同的速度譜密度0.01、0.02、0.05、0.1和0.2m2/s2，比較Kalman濾波和抗差自適應(yīng)濾波的濾波效果，如圖3、4所示；給GPS的17號衛(wèi)星的100、200和300歷元的C/A碼觀測值分別人為加入20、40和60m的粗差，檢驗(yàn)Kalman濾波和抗差自適應(yīng)濾波的濾波效果，如圖5、6所示。由于Y軸和Z軸的濾波結(jié)果與X軸的類似，因此討論不同濾波結(jié)果在X軸方向上的變化即可說明問題。

從圖3可以看出，隨著速度譜密度的不斷增大，KF的效果越來越好，這說明只要給出較正確的系統(tǒng)狀態(tài)噪聲，KF可以給出較高的濾波估值精度，由于初值賦值的盲目性，這種濾波估值精度很難保證。在未能正確給出系統(tǒng)狀態(tài)噪聲(如Q′=0.01m2/s2)的情況下，曲線變化比較劇烈，濾波殘差大部分都超過了2m，有的甚至達(dá)到了5m，這已經(jīng)不能滿足高精度實(shí)時GNSS導(dǎo)航的需要。

從圖4可以看出，隨著速度譜密度的不斷增大，ARKF的效果比較穩(wěn)定，殘差曲線變化也越來越和緩，但總體變化不大，這說明ARKF已經(jīng)不那么依賴系統(tǒng)狀態(tài)噪聲初值的精度，真正做到了自適應(yīng)，同時也有效抵制了系統(tǒng)模型誤差對濾波結(jié)果的不良影響。同時，濾波殘差都在0.5m以內(nèi)，可以滿足高精度GNSS導(dǎo)航的需要。

由圖5可知，當(dāng)觀測值含有粗差時，KF結(jié)果在100、200、300歷元嚴(yán)重失真，已無法進(jìn)行正常的導(dǎo)航。粗差越大，濾波結(jié)果越遠(yuǎn)離真實(shí)值。且由于模型誤差的存在，KF的效果并不好，殘差曲線波動范圍較大，說明濾波器并不處于穩(wěn)定狀態(tài)。

相較于圖5而言，在加粗差前后，圖6的殘差曲線波動范圍較小，殘差都在0.5m以內(nèi)，有效地抑制了粗差觀測值對狀態(tài)估值的影響，說明該濾波器具有較好的穩(wěn)定性和良好的適應(yīng)性，能滿足高精度實(shí)時GNSS導(dǎo)航的需要。同時，ARKF有效抑制了系統(tǒng)模型誤差對濾波結(jié)果的影響，提高了濾波的精度。

4　結(jié)論

與經(jīng)典的Kalman濾波結(jié)果進(jìn)行實(shí)例對比分析，驗(yàn)證了本文推導(dǎo)的抗差自適應(yīng)Kalman濾波及其算法的有效性與優(yōu)越性。抗差自適應(yīng)Kalman濾波可以有效地抑制系統(tǒng)模型誤差，以及觀測粗差對狀態(tài)估值的影響，降低不良觀測影響，提高濾波精度。

[1]李增科，王 堅(jiān)，高井祥，等.利用SVM的GPS/INS組合導(dǎo)航濾波發(fā)散抑制方法研究[J].武漢大學(xué)學(xué)報：信息科學(xué)版，2013，38(10)：1216-1220.

[2]李 沖，黃觀文，譚 理，等.抗差自適應(yīng)卡爾曼濾波在GPS精密單點(diǎn)定位中的應(yīng)用[J].測繪科學(xué)，2011，36(4)：22-23.

[3]MOHAMED A H，SCHWARZ K P.Adaptive Kalman filtering for INS/GPS[J].Journal of geodesy，1999，73(4)：193-203.

[4]GE Linlin，CHEN Hongyue，HAN Shaowei，et al.Adaptive filtering of continuous GPS results[J].Journal of Geodesy，2000，74(7-8)：572-580.

[5]YANG Yuanxi，GAO Weiguang.An optimal adaptive Kalman filter[J].Journal of Geodesy，2006，80(4)：177-183.

[6]余學(xué)祥，呂偉才.GPS監(jiān)測網(wǎng)動態(tài)數(shù)據(jù)處理抗差Kalman濾波模型[J].中國礦業(yè)大學(xué)學(xué)報，2000，29(6)：553-557.

[7]許長輝，高井祥，胡 洪，等.精密單點(diǎn)定位的抗差卡爾曼濾波研究[J].中國礦業(yè)大學(xué)學(xué)報，2012，41(5)：857-862.

[8]苗岳旺，周 巍，田 亮，等.基于新息χ2檢測的擴(kuò)展抗差卡爾曼濾波及其應(yīng)用[J].武漢大學(xué)學(xué)報：信息科學(xué)版，2016，41(2)：1-5.

[9]聶建亮，張雙成，徐永勝，等.基于抗差Kalman 濾波的精密單點(diǎn)定位[J].地球科學(xué)與環(huán)境學(xué)報，2010，32(2)：218-220.

[10]HUBER P J.Robust statistics[M].New York：John Wiley，1981.

[11]HAMPEL F R，RONCHETTI E M，ROUSSEEUW P J，et al.Robust statistics[M].New York：John Wiley，1986.

[12]楊元喜.抗差估計(jì)理論及其應(yīng)用[M].北京：八一出版社，1993.

[13]YANG Yuanxi，HE Haibo，XU Guochang.Adaptively robust filtering for kinematic geodetic positioning[J].Journal of Geodesy，2001，75(2-3)：109-116.

[14]吳富梅，楊元喜.一種兩步自適應(yīng)抗差Kalman濾波在GPS/INS 組合導(dǎo)航中的應(yīng)用[J].測繪學(xué)報，2010，39(5)：522-527.

[15]謝先明.一種基于擴(kuò)展卡爾曼濾波的多基線相位噪聲抑制及展開方法[J].測繪科學(xué)，2015，40(2)：43-47.

[16]王 仁，趙長勝，夏志浩，等.無跡卡爾曼濾波衰減記憶算法研究[J].測繪通報，2015(12)：20-22.

[17]蔡 鐘，倪小東.重力異常處理中小子域?yàn)V波法的改進(jìn)[J].河北工程大學(xué)學(xué)報：自然科學(xué)版，2015，32(4)：47-51.

[18]楊元喜，任 夏，許 艷.自適應(yīng)抗差濾波理論及應(yīng)用的主要進(jìn)展[J].導(dǎo)航定位學(xué)報，2013，1(1)：9-15.

[19]SCHWARZ K P，CANNON M E，WONG R V C.A comparison of GPS kinematic models for the determination of position and velocity along a trajectory[J].Manuscripta Geodaetica，1989，14(6)：345-353.

(責(zé)任編輯王利君)

Adaptive robust Kalman filtering and its application in GNSS

ZOU Min1，WANG Guodong2，LIU Chao1

(1.School of Geodesy and Geomatics,Anhui University of Science and Technology,Anhui Huainan,232001,China;2.The Youse Surveying and Mapping Institute of Zhejiang,Zhejiang Shaoxing,312000,China)

The Kalman filtering is easily affected by the gross error and it will cause larger distortion of the result. To overcome this problem, an adaptive robust Kalman filtering was proposed. It combines the advantages of adaptive Kalman filtering and robust Kalman filtering by using the adaptive factor and the robust factor. And the improved two segments Huber function was designed as the robust factor, the two to three times observation noise error was designed as the gross error determination standard respectively. Compared the result of Kalman filtering and adaptive robust Kalman filtering, the latter could effectively resist the influence of abnormal observation to the state estimation, and can improve the stability and availability of the filtering estimation significantly.

KF;gross error;ARKF;GNSS

2016-04-20

國家自然科學(xué)基金資助項(xiàng)目(41404004)；安徽省自然科學(xué)基金資助項(xiàng)目(1408085QD72)；安徽理工大學(xué)科研啟動基金資助項(xiàng)目(11152)

鄒敏(1992-)，女，安徽滁州人，碩士，從事GNSS數(shù)據(jù)處理的研究。

1673-9469(2016)03-0089-05

10.3969/j.issn.1673-9469.2016.03.019

P228

A