GPS動態(tài)定位自適應(yīng)卡爾曼濾波算法研究

李勇軍，左 娟

（1.江西省測繪應(yīng)急保障服務(wù)中心，江西 南昌 330046；2.江西省第一測繪院，江西南昌 330046）

GPS動態(tài)定位自適應(yīng)卡爾曼濾波算法研究

李勇軍1，左 娟2

（1.江西省測繪應(yīng)急保障服務(wù)中心，江西 南昌 330046；2.江西省第一測繪院，江西南昌 330046）

Sage－Husa濾波和強跟蹤濾波是2種常規(guī)的自適應(yīng)卡爾曼濾波，有各自的優(yōu)缺點。綜合2種濾波的特點，給出一種抗粗差的修正算法，實驗效果比較好，能有效抑制少部分粗差帶來的影響，計算結(jié)果表明效果比較理想。

動態(tài)GPS定位；卡爾曼濾波；自適應(yīng)濾波；Sage－Husa濾波；強跟蹤濾波

卡爾曼濾波是一個不斷預(yù)測、修正的遞推過程，求解時不需要存儲大量的觀測數(shù)據(jù)，并且當(dāng)?shù)玫叫碌挠^測數(shù)據(jù)時，可隨時算得新的參數(shù)濾波值，便于實時地處理觀測結(jié)果，因此，越來越多地應(yīng)用于動態(tài)定位數(shù)據(jù)處理中，尤其是GPS動態(tài)數(shù)據(jù)處理［1－3］。然而，在一般情況下，Kalman濾波所基于的動力學(xué)模型和隨機模型不可能準(zhǔn)確無誤。特別是帶有粗差的樣本數(shù)據(jù)常常會使卡爾曼濾波器對系統(tǒng)的狀態(tài)預(yù)報進行錯誤的修正，使得濾波結(jié)果發(fā)生偏移，甚至發(fā)散。因此，在卡爾曼濾波中加入抗粗差處理是非常必要的。

1 常見的自適應(yīng)卡爾曼濾波模型

在GPS動態(tài)單點定位中，一般采用常加速模型（CA模型［4］），卡爾曼濾波系統(tǒng)的狀態(tài)方程和觀測方程可表示為

式中：X為狀態(tài)矢量，X＝（xyzt˙x˙y˙z ˙t¨x¨y¨z¨t）T，Φk，k－1是狀態(tài)轉(zhuǎn)移矩陣，Γk，k－1是噪聲輸入矩陣，Wk是過程噪聲矢量，Zk是觀測矢量，Hk是偽距觀測線性化后的觀測矩陣，Vk是觀測方程的誤差矢量。

1.1Sage－Husa自適應(yīng)濾波

Sage自適應(yīng)濾波［5－7］利用歷史信息的平均來估計當(dāng)前歷元的動態(tài)噪聲協(xié)方差矩陣和觀測噪聲協(xié)方差矩陣，從而使動態(tài)噪聲協(xié)方差矩陣和觀測噪聲協(xié)方差矩陣自適應(yīng)于當(dāng)前的動態(tài)信息和觀測信息。線性系統(tǒng)的Sage－Husa自適應(yīng)濾波可以描述為

式中：Qk是系統(tǒng)過程噪聲，Rk是系統(tǒng)觀測噪聲，Kk為增益矩陣，Pk為預(yù)測值方差陣，＾qk、＾Qk、＾r(nóng)k、＾Rk由時變噪聲統(tǒng)計估計器［8］遞推獲得，其中：

在應(yīng)用Sage自適應(yīng)濾波處理動態(tài)GPS觀測數(shù)據(jù)時，由于動態(tài)定位中的狀態(tài)變量維數(shù)比較高，Sage－Husa算法對于階次較高的系統(tǒng)也不能保證完全可靠，有可能失去半正定性和正定性而出現(xiàn)濾波發(fā)散現(xiàn)象，穩(wěn)定性和收斂性不能完全保證［2］。

1.2 強跟蹤kalman濾波

強跟蹤算法［9－11］，即采用漸消因子限制Kalman濾波器的記憶長度，以便充分利用現(xiàn)時的觀測數(shù)據(jù)。與正常Kalman濾波解的顯著區(qū)別在于將驗前狀態(tài)協(xié)方差矩陣膨脹λk倍，使歷史狀態(tài)信息的試用效率降低，從而達到重用現(xiàn)時測量信息的目的，如式（12）所示。

式中：λk＝diag［λ1kλ2k… λnk］，λk為漸消因子，有時候也稱為遺忘因子，且λk≥1。

最佳遺忘因子λk可由下式求取：

式中：tr［·］是矩陣求跡符號，Nk和Mk的表達式為

式中：vk＝Zk－Hk

2 一種改進的自適應(yīng)濾波

帶有粗差的樣本數(shù)據(jù)常常會使Kalman濾波器對系統(tǒng)的狀態(tài)預(yù)報進行錯誤的修正，使得濾波結(jié)果發(fā)生偏移，甚至發(fā)散。因此，在Kalman濾波中，加入必要的抗粗差處理是非常必要的。

2.1 抗粗差修正算法

當(dāng)測量數(shù)據(jù)中含少量粗差時，能夠克服其不利影響，或者能將其影響控制在某預(yù)先給定的界限內(nèi)，以確保濾波估計值盡可能接近系統(tǒng)真實狀態(tài)。由此構(gòu)造了自適應(yīng)Kalman濾波的抗粗差修正算法如下：

式中：Yk（·）為適當(dāng)選取的按段光滑的函數(shù)，稱為壓縮影響函數(shù)；Gk為權(quán)矩陣。如果選取權(quán)矩陣序列

則使濾波誤差均方差最小的Y函數(shù)為

式中：λk為KkGk矩陣的最大特征值，Dk為選取的門限常數(shù)序列，Dk的合理取值為

式中：cα（n）為以χ2（n）分布置信度為（1－α）×100%的上分位點，通常α取0.05或0.025。

2.2 濾波器收斂性判據(jù)

當(dāng)濾波器發(fā)散時，誤差協(xié)方差陣無界，這時實際的誤差往往比理論預(yù)計的誤差大很多倍。可以根據(jù)這一特點，利用新息序列的性質(zhì)來構(gòu)造濾波器的收斂判據(jù)。

式中：γ≥1為可調(diào)系數(shù)。

2.3 算法流程

當(dāng)式（24）成立時，測量值Zk為正常測量值，濾波器處于正常工作狀態(tài)，這時可用Sage－Husa自適應(yīng)濾波算法進行狀態(tài)最優(yōu)值估計。反之，說明測量值Zk為粗差或濾波發(fā)散，此時如果僅采用強跟蹤濾波算法，濾波的最優(yōu)條件將被破壞，導(dǎo)致濾波精度降低；如果僅采用抗粗差修正算法處理，又無法保證濾波的穩(wěn)定性和準(zhǔn)確性。實際上，GPS觀測時，成片粗差的個數(shù)是有限的。因此，本文設(shè)置了一個連續(xù)粗差個數(shù)門限，當(dāng)連續(xù)粗差的個數(shù)小于門限值時，將當(dāng)前測量值視為粗差，采用抗粗差修正算法處理，即進行狀態(tài)估計時利用壓縮影響函數(shù)Yk（）抑制由粗差帶來的異常信息，使?fàn)顟B(tài)估計保持正常；否則，認(rèn)為濾波發(fā)散，采用強跟蹤濾波算法處理，即利用加權(quán)系數(shù)λk調(diào)整Pk，k－1，從而抑制濾波發(fā)散。

3 實例分析

本文算例數(shù)據(jù)選自丹麥國家空間研究中心（DNSC）在格陵蘭地區(qū)進行航空Lidar測量所采集的動態(tài)GPS數(shù)據(jù)［12］，采用武漢大學(xué)測繪學(xué)院開發(fā)的動態(tài)定位軟件TriP2.0計算的結(jié)果作為“已知值”（動態(tài)定位精度：3cm～1dm）。實驗選用了前2000個歷元作為實驗的數(shù)據(jù)，在這2000個歷元當(dāng)中，飛機完成了從“靜止→起飛→升空→平穩(wěn)”的一系列機動突變的過程，具有代表性，能比較好地分析其動態(tài)定位。

實驗采用如下5種方案（見圖1～圖5）進行計算比較：方案A：單歷元最小二乘平差解；方案B：標(biāo)準(zhǔn)Kalman濾波解；方案C：Sage－Husa自適應(yīng)濾波解；方案D：強跟蹤自適應(yīng)濾波解；方案E：改進自適應(yīng)濾波解。為節(jié)省空間，在此僅選擇各種方案在X軸方向與參考值的差值圖。

從上面5個方案計算的結(jié)果（見表1），可以看出：

1）飛機起飛的時候，標(biāo)準(zhǔn)的kalman濾波和Sage－Husa自適應(yīng)濾波的結(jié)果受到明顯影響，尤其是Sage－Husa自適應(yīng)濾波，出現(xiàn)了發(fā)散。最小二乘估計基本上不受飛機擾動的影響，強跟蹤自適應(yīng)濾波受飛機擾動影響不大，而改進的自適應(yīng)濾波對飛機的擾動能夠自適應(yīng)，效果明顯，具有抵抗非平穩(wěn)的能力。

2）當(dāng)觀測值存在粗差時，Sage－Husa自適應(yīng)濾波比標(biāo)準(zhǔn)的kalman濾波還要差，甚至發(fā)散（本例即為發(fā)散）。觀測值粗差歪曲了當(dāng)前歷元的結(jié)果，而將預(yù)測信息與觀測信息的不符合全部歸咎于預(yù)報信息，于是過分降低了預(yù)報信息的權(quán)重，使自適應(yīng)濾波的結(jié)果擬合于錯誤的觀測結(jié)果。

3）強跟蹤算法雖然在對突變情況進行估計的時候，精度不是很好，但是比較穩(wěn)定，增大了系統(tǒng)的過程噪聲和觀測噪聲的方差陣，保證了濾波可靠收斂，但是犧牲一定精度而換取濾波的穩(wěn)定性。

4）改進的自適應(yīng)濾波不僅能抵制機動目標(biāo)的突變情況，而且還能抵御小部分粗差的影響，能合理平衡動力學(xué)模型預(yù)報信息與觀測信息的權(quán)比，對狀態(tài)擾動的影響具有較強的控制能力。

表1 5種不同方案計算的X、Y、 Z方向坐標(biāo)與參考值之差統(tǒng)計情況m

4 結(jié) 論

綜合以上分析，可以得出：

1）標(biāo)準(zhǔn)kalman濾波能夠充分利用預(yù)報信息來提高濾波估值的精度，但不能控制載體運動擾動異常誤差的影響；

2）最小二乘估計可以有效抵制載體運行異常誤差的影響，但沒有充分利用載體平穩(wěn)運行的可靠預(yù)報信息；

3）Sage－Husa自適應(yīng)濾波能夠合理利用預(yù)報信息和當(dāng)前歷元信息來對濾波進行估值，但前提是要前一歷元能為當(dāng)前歷元提供可靠估值，否則容易發(fā)散；

4）強跟蹤算法雖然能夠防止濾波發(fā)散，但是增大系統(tǒng)的過程噪聲和觀測噪聲方差陣，這樣將許多未建模的誤差包含進去，使濾波穩(wěn)定，但降低了部分精度；

5）改進的自適應(yīng)濾波不僅能夠有效抵制觀測值粗差的影響，而且能夠合理平衡模型預(yù)報信息和當(dāng)前歷元觀測信息。

［1］秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理［M］.西安：西北工業(yè)大學(xué)出版社，1998.

［2］付夢印，鄧志紅，張繼偉.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用［M］.北京：科學(xué)出版社，2003.

［3］劉基余.GPS衛(wèi)星導(dǎo)航定位原理與方法［M］.北京：科學(xué)出版社，2003.

［4］楊元喜.自適應(yīng)動態(tài)導(dǎo)航定位［M］.北京：科學(xué)出版社，2006.

［5］Sage A P，Husa G W.Adaptive Filtering With unknown Prior Statistic.Joint Automatic Control Conference，1969：760－769.

［6］Mohamed A H，Schwarz K P.Adaptive Kalman filter for INS／GPS［J］.Journal of Geodesy，1999，73（4）：193－203.

［7］Wang Y J，Stewart M P，Tsakiri M.Adaptive Kalman filtering for integration of GPS with GLONASS and INS［C］.IUGG／IAG Birmingham，August：18－31.

［8］王永剛，王順宏.改進Sage－Husa濾波及在GPS／INS容錯組合制導(dǎo)中的應(yīng)用［J］.中國慣性技術(shù)學(xué)報，2003，11（5）：29－32.

［9］Fagin S L.Recursive Linear regession theory，optimal filter theory and error analysis of optimal system［J］.IEEE Int.Convent，Record，.1964，12：216－240.

［10］夏啟軍，孫優(yōu)賢，周春暉.漸卡爾曼濾波器的最佳自適應(yīng)算法及其應(yīng)用［J］.自動化學(xué)報，1990，16（3）：210－216.

［11］蔡艷輝，程鵬飛，李夕銀.用卡爾曼濾波進行GPS動態(tài)定位［J］.測繪通報，2006（7）：6－8.

［12］張小紅.動態(tài)精密單點定位（PPP）的精度分析［J］.全球定位系統(tǒng)，2006（1）：7－11.

Research on adaptive Kalman Filtering algorithm in GPS kinematic positioning

LI Yong－jun1，ZUO Juan2
（1.Jiangxi Emergency Guarantee Service Center of Surveying and Mapping，Nanchang 330046，China；2.Jiangxi First Surveying and Mapping Institute，Nanchang 330046，China）

Sage－Husa adaptive filtering and strong tracking adaptive filtering are two general adaptive Kalman filters，but they both have advantages and disadvantages.The paper proposes an approach to antigross error correction algorithm by combination of the filers above based on the filter divergence analysis.Experimental results were quite good and can inhibit the small impact of gross errors.

dynamic GPS positioning；Kalman filtering；adaptive filtering；Sage－Husa adaptive filtering；strong tracking adaptive filtering

P228

A

1006－7949（2012）04－0029－04

2011－07－12

李勇軍（1984－），男，助理工程師，碩士.

[責(zé)任編輯:劉文霞］