用卡爾曼濾波進行GPS動態(tài)定位

1、6 測 繪 通 報 2006年 第7期 文章編號:0494 0911(2006)07 0006 03中圖分類號:P228.4 文獻標識碼:B用卡爾曼濾波進行GPS動態(tài)定位蔡艷輝1,2,程鵬飛,李夕銀11(1.中國測繪科學研究院,北京100039;2.遼寧工程技術(shù)大學地理空間信息技術(shù)與應用實驗室,遼寧阜新123000)KinematicPointPositioningwithKalmanFilteringCAIYan hui,CHENGPeng fei,LIXi yin摘要:采用卡爾曼濾波進行GPS單點定位,必須合理地構(gòu)造系統(tǒng)動態(tài)特性,才能使濾波平穩(wěn)進行。介紹兩種確定GPS觀測數(shù)據(jù)方差的方法,然

2、后針對卡爾曼濾波的發(fā)散問題進行討論,并給出一種避免濾波發(fā)散的漸消因子改進算法,實現(xiàn)濾波的自適應。最后結(jié)合算例驗證了該方法的有效性。關(guān)鍵詞:卡爾曼濾波;單點定位;發(fā)散一、引 言對于動態(tài)GPS數(shù)據(jù)單點定位一般采用勻加速模型13,其離散化的卡爾曼濾波系統(tǒng)的狀態(tài)方程和觀測方程可表示為Xk+1= k+1,kXk+1.依據(jù)高度角進行映射由于衛(wèi)星信號在高度角低時容易產(chǎn)生多路徑效應,同時低高度角產(chǎn)生的大氣折射也相應增強,故假設(shè)衛(wèi)星在天頂時,相應的偽距觀測方差為 0,衛(wèi)星的高度角為 時的偽距觀測方差為202i=sin2k+1,k k1Vk+1=Bk+1 k+1+ k+Xk+1=Xk+ Xk+(1)(7)當衛(wèi)星的

3、高度角為 小于10 , =E+6.25,不同的衛(wèi)星之間的觀測值認為相互獨立,則偽距觀測值的協(xié)方差矩陣為21Rk=000 222式中,X為狀態(tài)矢量,X=(xyzt x y z t x y z t)T; 為觀測方程線性化后狀態(tài)矢量在當前時刻的改正值;Bk+1為偽距觀測線性化后的設(shè)計矩陣; k+1,k為狀態(tài)轉(zhuǎn)移矩陣;(8)Vk+1為觀測方程的誤差矢量; k+結(jié)果為Xk+Xk+1,k為噪聲驅(qū)動矩陣。則卡爾曼濾波的濾波1,k+1=1,k=Xk+1,k+Kk+1(Lk+1-Bk+1Xk+Bk+1Pk+ Tk+1,k+T1,kBk+1+1,k)(2)(3)k+k1,kXRk+1)-2.采用高度角函數(shù)Jin于

4、1996年給出偽距觀測方差經(jīng)驗公式11(ti)=a0+a1e-E/a2Kk+1=Pk+Pk+Pk+1,k=T1,kBk+1(9)k+1,kPk,k k+T1,kQk k+1,k(4)(5)(6)其中,a0,a1分別是固定因子和比例因子;a2是高度角變化系數(shù)。對于特定的接收機,a0,a1,a2是固定值,可預先測定。1,k+1=(E-Kk+1Bk+1)Pk+1,k二、觀測值方差的確定在進行卡爾曼濾波時,需要確定觀測量的方差矩陣Rk,根據(jù)GPS的觀測值合理地給出觀測值的方差和協(xié)方差矩陣,將有助于真實反映系統(tǒng)狀態(tài),獲得滿意的濾波結(jié)果。觀測值方差矩陣的確定,采用的方法如下。三、濾波發(fā)散和濾波飽和的處理從

5、理論上講,隨著觀測數(shù)據(jù)的增多,卡爾曼濾波可以逐漸變得平穩(wěn),狀態(tài)估計逐漸變得更為精確,但有時由濾波所得的狀態(tài)估計與實際狀態(tài)估計之間的誤差遠遠超過所能容許的范圍,理論計算的方差可以逐漸趨于穩(wěn)定,而實際估計誤差方差可能趨于無收稿日期:2004 11 30基金項目:遼寧工程技術(shù)大學地理空間信息技術(shù)與應用實驗室基金資助項目(2004005)作者簡介:蔡艷輝( ),男,GLONASS2006年 第7期 測 繪 通 報 7 窮大,這樣產(chǎn)生濾波發(fā)散。其產(chǎn)生的原因一般有:對物理系統(tǒng)了解不全面,動態(tài)方程描述不完整;對系統(tǒng)噪聲和觀測噪聲的統(tǒng)計特性缺乏了解,噪聲模型不恰當;機器計算時舍入誤差影響。常用的克服濾波發(fā)散的

6、方法有限定下界法、擴充狀態(tài)法、漸消記憶法、限定記憶法、非線性濾波法等。本文對漸消因子法進行了改進,避免卡爾曼濾波器的發(fā)散。對一步預測方差增加漸消因子 (k+1),即Pk+1,k=21(k+1)=其中0=j=0i=0+b( 0<a) 0( 0 a)(17)fabs(kij)33(18)式中,fabs()為取絕對值;kij為增益矩陣的i行j列元素;a為根據(jù)系統(tǒng)特性給定的門限值;b為補償數(shù)?？柭鼮V波器實際是一個低通濾波器,門限值a與一定的帶寬相關(guān),a變小,濾波帶寬變大,a變(k+1) k+ k+T1,kPk,k k+1,k+T1,kQk k+1,k(10)式中, (k+1)的確定算法如下:當

7、系統(tǒng)滿足下述條件:1.Qk是半正定對稱陣,Rk和Pk都是正定對稱矩陣;2.Hk+1,k大,濾波帶寬變小。b是一個補償系數(shù),用來調(diào)整濾波效率,b選得過大或過小都將導致濾波發(fā)散。所以 在進行靜態(tài)數(shù)據(jù)處理時,采用恒速度模型,一般可取a=0.5,b=-1; 對于動態(tài)數(shù)據(jù),采用恒加速度模型,a,b的取值一般根據(jù)運動的特性適當調(diào)整,保證 (k+1)能夠起到增大增益的作用,對于較低速運行的系統(tǒng),可取a=2,b=0.5。由于觀測方程是坐標的直接函數(shù),所以增益矩陣中坐標分量對應的矩陣元素如果變小,則新數(shù)據(jù)對系統(tǒng)的修正作用就會減弱,采用增益矩陣的元素變化特性為指標,來控制系統(tǒng)對動態(tài)模型的依賴性,實現(xiàn)濾波的自適應。

8、(Xk+1,k)是非奇異陣時,漸消因子 01( 0 1)( 0<1)(k+1)可由下式得到(k+1)=式中0=trN(k+1)/trM(k+1)N(k+1)=V0(k+1)-Hk+M(k+1)=Hk+Rk+1,k k+1,kPkT1,kQkHk+1,k-(11)(12)Rk+1(13)Tk+T1,kHk+1,k四、算 例該算例采用機載GPS航空攝影測量的GPS觀測數(shù)據(jù),選取其中一段飛機由靜止到開始起飛到進入航線的試驗數(shù)據(jù),采用恒加速度模型,估計機載GPS天線的位置、速度和加速度。飛機飛行最大速度每小時360km,數(shù)據(jù)采用間隔1s。當卡爾曼濾波參數(shù)a=2,b=0.5,狀態(tài)矢量中位置方差(3

9、,3,3,3),速度方差(1,1,1,1),加速度方差(0.5,0.5,0.5,0.5),模型噪聲為(0.5,0.5,0.5,0.5)時,計算飛機運行軌跡與商用差分軟件Graf Nav計算結(jié)果比較如圖1所示。(14)(15)V0(k+1)=E(vk+1vTk+1)=Hk+11,kT1,kPk+1,kHk+1,k+vk+1=Lk+1-Bk+1Xk+(16)其中,tr()為求跡算子。另外,由于系統(tǒng)的特殊要求,常常出現(xiàn)數(shù)據(jù)飽和(即實際估計誤差方差逐漸趨近于零), (k+1)的取值也可以由增益矩陣Kk+1的元素自動設(shè)定,以實現(xiàn)濾波的穩(wěn)態(tài)進行, (k+1)定義如下18 測 繪 通 報 2006年 第7期

10、 從圖2可以看出,在緯度和經(jīng)度方向都表現(xiàn)出很大的差值,這種大差值出現(xiàn)時飛機都處于轉(zhuǎn)彎狀態(tài),由于飛機轉(zhuǎn)彎是變加速度運動,所以用恒定加速度模型來表示就存在模型誤差。在高度方向差值明顯優(yōu)于經(jīng)緯度方向,主要是在高度方向,沒有大的起落。但是高度方向存在一定的系統(tǒng)偏差,因為在高度方向飛機一直處于加速上升狀態(tài),系統(tǒng)偏差包括模型在高度方向的響應延遲以及各種改正不完全的因素影響。不同的濾波參數(shù)的影響:當狀態(tài)矢量中位置方差(3,3,3,3),速度方差(1,1,1,1),加速度方差(0.5,0.5,0.5,0.5)時,模型噪聲分別為(0.5,0.5,0.5,0.5),濾波參數(shù)分別為a=0.5,b=-0.5;a=2,

11、b=0.5時,計算結(jié)果如圖2所示。圖2 不同門限值單點定位與差分定位飛機位置差值五、結(jié) 論卡爾曼濾波器實際是一個低通濾波器,門限值與一定濾波帶寬相關(guān)。從圖2中可以看出,小的門限導致大的誤差。這是因為,小門限值使得系統(tǒng)模型作用增強,濾波帶寬變大,抗外界干擾能力增強,但系統(tǒng)的模型誤差、系統(tǒng)的噪聲干擾同時增大,導致誤差增大,系統(tǒng)不穩(wěn)定;大的門限值使得濾波帶寬變小,濾波效果差,抗外界干擾能力減弱,系統(tǒng)的模型誤差、系統(tǒng)的噪聲干擾減小,系統(tǒng)模型作用減弱,但系統(tǒng)穩(wěn)定,不易發(fā)散。從而驗證本文提出抑止濾波發(fā)散和濾波飽和方法的合理性。同時也說明,應該根據(jù)實際情況選取合適的濾波參數(shù),才能使濾波達到最佳效果。另外,用

12、卡爾曼濾波器構(gòu)造高動態(tài)的系統(tǒng),需要有高頻率的數(shù)據(jù)輸出。從圖1可以看出,由于飛機速度過快,濾波器存在響應延遲,即使選用合理的濾波門限值和補償系數(shù),濾波結(jié)果仍然會出現(xiàn)大的偏差。因此,要提高濾波結(jié)果精度,只有增大GPS數(shù)據(jù)輸出頻率,進一步完善系統(tǒng)模型。參考文獻:1 JINXin xiang.TheoryofCarrierAdjustedDGPSPositioningApproachandSomeExperimentalResultsM.Delft:TheDelftUniversityPress,1996.2 萬德鈞,房建成,王 慶,等.GPS動態(tài)濾波的理論、方法及其應用M.南京:江蘇科學技術(shù)出版社,2000.3 崔希璋,於宗儔,陶本藻,等.廣義測量平差(第二版)M.北京:測繪出版社,2001.4 CHENGPeng fei.InvestigationontheEstablishmentofDGPSServicesinChinaM.Graz:TheGrazTechnicalUniversityPress,1998.5 劉基余,李征航,王躍虎,等.全球定位系統(tǒng)原理及其應用M.北京:測繪出版社,1993.6 CAIYan hui,CHENGPeng fei,LIXi yin.TraversingtheAmbiguity VectorSpacebyRecursiveFun