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

李勇軍，左 娟

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

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

李勇軍1，左 娟2

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

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

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

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

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

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

式中：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是過(guò)程噪聲矢量，Zk是觀測(cè)矢量，Hk是偽距觀測(cè)線性化后的觀測(cè)矩陣，Vk是觀測(cè)方程的誤差矢量。

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

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

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

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

1.2 強(qiáng)跟蹤kalman濾波

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

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

最佳遺忘因子λk可由下式求?。?/p>

式中：tr［·］是矩陣求跡符號(hào)，Nk和Mk的表達(dá)式為

式中：vk＝Zk－Hk

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

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

2.1 抗粗差修正算法

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

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

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

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

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

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

當(dāng)濾波器發(fā)散時(shí)，誤差協(xié)方差陣無(wú)界，這時(shí)實(shí)際的誤差往往比理論預(yù)計(jì)的誤差大很多倍?？梢愿鶕?jù)這一特點(diǎn)，利用新息序列的性質(zhì)來(lái)構(gòu)造濾波器的收斂判據(jù)。

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

2.3 算法流程

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

3 實(shí)例分析

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

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

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

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

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

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

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

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

4 結(jié) 論

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

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

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

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

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

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

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

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

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

［4］楊元喜.自適應(yīng)動(dò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］王永剛，王順宏.改進(jìn)Sage－Husa濾波及在GPS／INS容錯(cuò)組合制導(dǎo)中的應(yīng)用［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，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］.自動(dòng)化學(xué)報(bào)，1990，16（3）：210－216.

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

［12］張小紅.動(dòng)態(tài)精密單點(diǎn)定位（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é)任編輯:劉文霞］