新的自適應(yīng)漸消擴展卡爾曼濾波在GPS定位中的應(yīng)用*

胡　輝，彭雄明，楊德進，李陽達

（1.華東交通大學(xué)信息工程學(xué)院，南昌　330013；2.中國科學(xué)院國家授時中心，西安　710000）

?

新的自適應(yīng)漸消擴展卡爾曼濾波在GPS定位中的應(yīng)用*

胡輝1，彭雄明1，楊德進2，李陽達1

（1.華東交通大學(xué)信息工程學(xué)院，南昌330013；2.中國科學(xué)院國家授時中心，西安710000）

摘要：當(dāng)實際數(shù)據(jù)出現(xiàn)突變時，基于最小二乘、擴展卡爾曼濾波的GPS定位解算存在定位結(jié)果精度低和穩(wěn)定性差的問題。提出一種自適應(yīng)漸消擴展卡爾曼濾波算法，通過自適應(yīng)漸消迭代系統(tǒng)噪聲協(xié)方差，來實現(xiàn)抑制數(shù)據(jù)突變影響。試驗結(jié)果表明：該算法相比最小二乘、擴展卡爾曼濾波，其定位精度有所提高;相比傳統(tǒng)漸消擴展卡爾曼濾波，其收斂速度、穩(wěn)定性有所提高。

關(guān)鍵詞：GPS，自適應(yīng)漸消，擴展卡爾曼濾波，定位精度，穩(wěn)定性

0　引言

標(biāo)準的卡爾曼濾波是在假設(shè)系統(tǒng)噪聲和觀測噪聲已知的情況下進行濾波，只要噪聲的特性描述正確，濾波就會產(chǎn)生優(yōu)化的系統(tǒng)狀態(tài)估計值。實際情況，受實驗樣本的限制，噪聲的先驗統(tǒng)計特性未知或不準確，即使已知噪聲的先驗統(tǒng)計特性，但系統(tǒng)處于實時的運行環(huán)境，受內(nèi)外不確定因素的影響，噪聲統(tǒng)計特性容易發(fā)生變化，而經(jīng)典卡爾曼不具有應(yīng)對噪聲統(tǒng)計變化的自適應(yīng)能力，不準確的噪聲模型會導(dǎo)致濾波器不穩(wěn)定和精度降低，甚至發(fā)散。

在導(dǎo)航定位中，系統(tǒng)模型都有一定程度的非線性，為了解決非線性濾波的問題，用的最廣泛的濾波方法是擴展卡爾曼濾波（EKF），當(dāng)模型參數(shù)存在較大的誤差時，擴展卡爾曼濾波的估計精度會大大降低，甚至發(fā)散。為了克服現(xiàn)有EKF的缺點，有很多學(xué)者對EKF進行了自適應(yīng)研究，見文獻［1-11］。針對實際情況中數(shù)據(jù)突變，導(dǎo)致收斂速度、收斂穩(wěn)定性差等問題，本文提出一種新的自適應(yīng)漸消系統(tǒng)噪聲算法來實現(xiàn)GPS接收機的定位，并與傳統(tǒng)LS、EKF、漸消擴展卡爾曼濾波算法比較，選擇最優(yōu)的定位算法。

1定位解算模型

將EKF應(yīng)用于GPS定位解算中，首要是建立狀態(tài)模型和觀測模型。定義Xk為系統(tǒng)狀態(tài)向量，Zk為系統(tǒng)觀測向量。

式（1）中系統(tǒng)狀態(tài)向量由接收機的位置（xk，yk，zk）、速度（vxk，vyk，vzk）、接收機的鐘差δtk和鐘漂δ˙tk構(gòu)成，式（2）中觀測向量由偽距ρik和多普勒測量值dik構(gòu)成。其中i表示第i顆衛(wèi)星，i=1，2，3，…，n，n為可見衛(wèi)星數(shù)，k為采樣時刻。

1.1狀態(tài)方程

在PV（Position，Velocity）模型中，狀態(tài)量包括3個正交方向上的位置和速度、鐘差、鐘漂，其中每個方向上的位置和速度所含噪聲是相互獨立的，相關(guān)系數(shù)為0。系統(tǒng)的狀態(tài)轉(zhuǎn)移方程:

式（3）中F是離散時間域的狀態(tài)轉(zhuǎn)移系數(shù)矩陣，其推導(dǎo)是由連續(xù)時間域F（t）推導(dǎo)，F(xiàn)（t）滿足

F系數(shù)矩陣滿足［13］

將式（4）和式（6）帶入式（5）化簡得到F系數(shù)矩陣為：

因在［tk，tk+1］時間內(nèi)噪聲可以定義為

式（9）中G（λ）是轉(zhuǎn)化矩陣，F(xiàn)（t）是連續(xù)時間域的狀態(tài)轉(zhuǎn)移矩陣，在［tk，tk+1］時間內(nèi)近似為常量。

將式（9）帶入式（8）中化簡得

由式（4）可以推得

化簡式（12）得到離散化的Qk如下:

1.2觀測方程

系統(tǒng)的觀測方程，偽矩和系統(tǒng)狀態(tài)參數(shù)的關(guān)系為：

式（14）中，ρik是第i顆衛(wèi)星k時刻的偽矩，rrk是接收機k時刻的位置，rik是第i顆衛(wèi)星k時刻衛(wèi)星的位置，εik是第i顆衛(wèi)星k時刻總的測量誤差，εik用白噪聲進行建模。其偽矩方程中有4個未知的變量，接收機的位置（xrk，yrk，zrk）以及接收機時鐘偏差δtk；其k時刻第i顆衛(wèi)星的位置為（xik，yik，zik），其由導(dǎo)航電文提取后求得。

多普勒頻率測量值的計算：

式（15）中vxk，vyk，vzk分別為接收機在X、Y、Z方向的速度。vxik，vyik，vzik分別為第i顆衛(wèi)星X、Y、Z方向的速度。

對觀測方程式（14）和式（15）而言，可以寫成矩陣的形式：

式（16）中Xk是k時刻點的狀態(tài)向量，Vk是觀測噪聲矩陣，Zk是在k時刻的觀測值，Hk是觀測方程矩陣。式（3）中狀態(tài)噪聲Wk和式（16）中量測噪聲Vk的期望和協(xié)方差矩陣滿足：

E為數(shù)學(xué)期望符號，Qk為n×n維的半正定矩陣，Rk是m×m維正定對稱矩陣。

衛(wèi)星與接收機幾何距離

其觀測方程矩陣Hk滿足：

2自適應(yīng)漸消擴展卡爾曼濾波

上述已建立GPS定位系統(tǒng)的狀態(tài)方程和觀測方程。現(xiàn)將建立的模型應(yīng)用于EKF中，傳統(tǒng)漸消擴展卡爾曼（FEKF）對系統(tǒng)的狀態(tài)協(xié)方差以及系統(tǒng)噪聲進行實時漸消，從而達到降低系統(tǒng)模型誤差、抑制濾波發(fā)散、提高濾波的精度。因其漸消因子涉及的參數(shù)較多，漸消的針對性難以區(qū)分。為此，提出針對系統(tǒng)噪聲進行自適應(yīng)漸消的擴展卡爾曼濾波（AFEKF）方法。

2.1自適應(yīng)漸消擴展卡爾曼濾波模型

和EKF估計算法過程一樣，AFEKF主要由預(yù)測和更新兩個過程構(gòu)成，傳統(tǒng)EKF流程如下：

2.1.1系統(tǒng)狀態(tài)和狀態(tài)協(xié)方差矩陣的時間更新

式（19）中X^k|k-1是k-1時刻的時間更新狀態(tài)向量，F(xiàn)為狀態(tài)轉(zhuǎn)移矩陣，X^k-1是k-1時刻的狀態(tài)向量；式（20）中Pk|k-1是狀態(tài)誤差的協(xié)方差時間更新矩陣，Pk-1是k-1時刻的狀態(tài)誤差的協(xié)方差矩陣，Qk-1為k-1時刻的系統(tǒng)噪聲方差矩陣。

2.1.2觀測量更新

卡爾曼增益計算：

式（21）中Rk是觀測量的方差矩陣，Kk是卡爾曼增益矩陣，Hk是量測矩陣。

狀態(tài)估計和均方誤差估計：

式（22）中X^k是估計的狀態(tài)向量，式（23）Pk是估計的狀態(tài)協(xié)方差矩陣

估計的殘差向量：

EKF系統(tǒng)中，實際數(shù)據(jù)出現(xiàn)突變時估計的殘差Vk會增大，而增益陣Kk因離線計算的原因，未能根據(jù)實時的狀態(tài)在線調(diào)整增益矩陣Kk，Kk仍將趨于極小值，不會隨殘差Vk的增大而變大，導(dǎo)致EKF濾波精度下降，甚至發(fā)散。

傳統(tǒng)FEKF［3］在式（20）中引入λk

其中未進行式（26）處理的系統(tǒng)噪聲為未自適應(yīng)系統(tǒng)噪聲漸消擴展卡爾曼濾波算法。

式（25）中λk為k時刻的漸消因子。本文處理過程中，提出利用自適應(yīng)漸消因子對時變系統(tǒng)噪聲Qk進行自適應(yīng)，并同擴展卡爾曼濾波、傳統(tǒng)漸消擴展卡爾曼濾波方法進行比較。

2.2自適應(yīng)漸消因子的確定

漸消子的選取，主要是用漸消因子抑制卡爾曼濾波的記憶長度，利用當(dāng)前的觀測數(shù)據(jù)，提高濾波器的動態(tài)性能。如下針對系統(tǒng)噪聲進行自適應(yīng)漸消，并推導(dǎo)漸消因子的計算方法。

將式（20）和式（21）中Pk|k-1、Kk的表達式代入式（28）中，化簡整理得：

式（29）中:

對式（31）兩邊求跡，得到如下表達式：

上求漸消因子的步驟，相比現(xiàn)有求漸消因子的方法，沒有標(biāo)量因子的計算，其計算簡單且易實現(xiàn)，實時性效果也能得到保證。在狀態(tài)突變時，估計的觀測誤差Vk會相應(yīng)增大，會引起誤差方差矩陣Ck增大，則λ0會相應(yīng)地增大，則能確保式（28）滿足。

3試驗結(jié)果分析

本文進行了如下3組實驗，一組是靜態(tài)未突變場景實驗，一組勻速圓周運動實驗，一組數(shù)據(jù)突變場景實驗；設(shè)置初始化狀態(tài)協(xié)方差，P0=diag［106，106，106，106，106，106，106，106］，觀測量偽矩協(xié)方差和多普勒速度協(xié)方差分別為R=diag［9，0.25，9，0.25…］，采樣時間間隔T=0.1 s。第1組靜態(tài)實驗，鐘差的噪聲功率譜密度取值為σb=0，鐘漂的噪聲功率譜密度取值為σd=0.25，位置的噪聲功率譜密度σp=0，速度的噪聲功率譜密度σv=1-4；第2組實驗，設(shè)置載體以1 m/s恒定速度做勻速圓周運動，運動半徑為10 m，其參數(shù)設(shè)置和靜態(tài)場景設(shè)置相同；第3組數(shù)據(jù)突變實驗，突變的數(shù)據(jù)來源于加州大學(xué)河濱分校實際采集的數(shù)據(jù)［13］，采樣時間長度是51 s時長的數(shù)據(jù)，前0～7.3 s數(shù)據(jù)為穩(wěn)定起始收斂時間段，在6 s時數(shù)據(jù)較大跳動，在7.3 s開始收斂，其參數(shù)設(shè)置如下σb= 0，σd=0.25，σp=0，σv=9；自適應(yīng)漸消擴展卡爾曼濾波的漸消因子初始取值為λ=1。

3.1 LS、EKF、AFEKF靜態(tài)未突變場景比較

數(shù)據(jù)從開始定位，LS、EKF和自適應(yīng)漸消擴展卡爾曼濾波（AFEKF）在靜態(tài)場景情況下，分別是其X、Y、Z方向上的定位結(jié)果誤差見圖1～圖3所示。

圖1　x方向的定位結(jié)果誤差　圖2　y方向的定位結(jié)果誤差

圖3　z方向的定位結(jié)果誤差

從圖1～圖3中可以看出，LS的定位誤差有較大的擾動，AFEKF定位結(jié)果和EKF相比，定位效果很接近。

3.2 EKF、AFEKF勻速圓周運動場景比較

在勻速圓周運動情況下，EKF和AFEKF其X、Y、Z方向上的定位結(jié)果誤差如圖4所示。

圖4 x、y、z方向的定位結(jié)果誤差

從圖4可以看出，EKF波動較大，不穩(wěn)定性強，而AFEKF穩(wěn)定性強。在勻速圓周運動中，載體速度大小不會變化，而速度方向在時刻變化，此時EKF建立的靜態(tài)模型難以復(fù)合復(fù)雜運動的場景，而AFEKF通過自適應(yīng)系統(tǒng)噪聲能穩(wěn)定工作。

3.3 LS、EKF、AFEKF動態(tài)突變場景比較

數(shù)據(jù)從開始定位，EKF和自適應(yīng)漸消擴展卡爾曼濾波（AFEKF）起始收斂時間是從第7.3 s開始，圖5～圖7分別是X、Y、Z方向上的定位結(jié)果誤差圖。

圖5 x方向的定位結(jié)果誤差圖6 y方向的定位結(jié)果誤差

圖7 z方向的定位結(jié)果誤差

從圖5～圖7中可以看出，LS的定位誤差有較大的擾動，在有突變的情況下，EKF和AFEKF處理效果，比LS效果要好；AFEKF在收斂性上，較EKF更能收斂到更加準確的值。結(jié)合圖5～圖7，可以看出，起始時間0 s～3.8 s內(nèi)，數(shù)據(jù)未發(fā)生跳變其EKF和 AFEKF效果相同，其穩(wěn)定性比最小二乘好；結(jié)合圖5～圖7及表1中，12 s～51 s內(nèi)AFEKF、EKF誤差均值比LS效果要好，AFEKF定位誤差均值比EKF好。

表1　數(shù)據(jù)穩(wěn)定后（12 s～51 s）的LS、EKF和AFEKF比較

3.4傳統(tǒng)FEKF、未自適應(yīng)系統(tǒng)噪聲FEKF、自適應(yīng)

系統(tǒng)噪聲AFEKF比較

下面是在數(shù)據(jù)突變的場景，傳統(tǒng)FEKF、未自適應(yīng)系統(tǒng)噪聲FEKF、自適應(yīng)系統(tǒng)噪聲AFEKF算法定位結(jié)果對比圖，圖8～圖10分別是X、Y、Z方向上的定位結(jié)果誤差圖。

圖8　x方向的定位結(jié)果誤差　圖9　y方向的定位結(jié)果誤差

圖10　z方向的定位結(jié)果誤差

從圖8～圖10中可以看出，在起始時間段傳統(tǒng)FEKF、未自適應(yīng)系統(tǒng)噪聲FEKF、自適應(yīng)系統(tǒng)噪聲AFEKF算法定位效果相差不大。在6 s時刻，數(shù)據(jù)出現(xiàn)突變，此時傳統(tǒng)FEKF、未自適應(yīng)系統(tǒng)噪聲FEKF出現(xiàn)較大的突變，收斂穩(wěn)定后出現(xiàn)較大振動，而AFEKF抑制突變效果明顯，其穩(wěn)定性高于傳統(tǒng)FEKF、未自適應(yīng)系統(tǒng)噪聲FEKF。相比傳統(tǒng)漸消擴展卡爾曼，該算法穩(wěn)定性更強，其整體定位精度也有所提高。

4　結(jié)論

在衛(wèi)星導(dǎo)航定位解算中，本文提出一種自適應(yīng)漸消擴展卡爾曼濾波方法應(yīng)用于數(shù)據(jù)突變或狀態(tài)突變，通過自適應(yīng)漸消系統(tǒng)噪聲來實現(xiàn)突變的抑制。根據(jù)實際的數(shù)據(jù)進行試驗，結(jié)果表明在沒有突變的情況下，自適應(yīng)漸消擴展卡爾曼濾波定位效果和漸消FEKF、EKF效果一致，比最小二乘定位效果更好；在數(shù)據(jù)出現(xiàn)突變時，該算法定位精度比擴展卡爾曼濾波要好；相比傳統(tǒng)漸消、未自適應(yīng)系統(tǒng)噪聲漸消擴展卡爾曼濾波，其穩(wěn)定性和整體定位精度都有所提高。因此，本文提出的自適應(yīng)系統(tǒng)噪聲漸消擴展卡爾曼濾波方法針對數(shù)據(jù)出現(xiàn)突變的情況，能為衛(wèi)星導(dǎo)航定位中實時快速穩(wěn)定收斂提供一定參考。

參考文獻：

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

［2］楊柳慶，肖前貴，牛妍，等.基于漸消卡爾曼濾波器的定位系統(tǒng)設(shè)計［J］.南京航空航天大學(xué)學(xué)報，2012，44（1）:134-138.

［3］徐定杰，賀瑞，沈鋒，等.基于新息協(xié)方差的自適應(yīng)漸消卡爾曼濾波器［J］.系統(tǒng)工程與電子技術(shù)，2011，33（12）:2696-2699.

［4］馬龍，李曉明.基于GPS/INS組合導(dǎo)航的改進自適應(yīng)漸消卡爾曼濾波算法［J］.科學(xué)技術(shù)與工程，2013，13（33）:9973-9977.

［5］徐天河，楊元喜.改進的Sage自適應(yīng)濾波方法［J］.測繪科學(xué)，2000，25（3）:22-24+2-1.

［6］王沖，曾慶軍.基于Sage-husa自適應(yīng)濾波算法的AUV組合導(dǎo)航系統(tǒng)設(shè)計［J］.電子設(shè)計工程，2013，21（15）:75-77+80.

［7］魏偉，秦永元，張曉冬，等.對Sage-Husa算法的改進［J］.中國慣性技術(shù)學(xué)報，2012，20（6）:678-686.

［8］MEHRA R.Approaches to adaptive filtering［J］.IEEE Trans.On Automatic Control.1972，17（5）：693-698.

［9］趙琳.非線性系統(tǒng)濾波理論［M］.北京：國防工業(yè)出版社，2012.

［10］GENG Y，WANG J.Adaptive estimation of multiple fading factors in Kalman filter for navigation applications［J］.GPS Solutions，2008，12（4）:273-279.

［11］KIM K H，LEE J G，PARK C G.Adaptive two-stage extended Kalman filter for a fault-tolerant INS-GPS loosely coupled system［J］.Aerospace and Electronic Systems，IEEE Transactions on，2009，45（1）:125-137.

［12］MAO X C，WADA M，HASHIMOTO H.Investigation on nonlinear filtering algorithms for GPS［C］// Proc.of Intelligent Vehicle Symposium，Versailles，F(xiàn)rance:IEEE，2002:64-70.

［13］魯郁.GPS全球定位接收機—原理與軟件實現(xiàn)［M］.北京：電子工業(yè)出版社，2009.

Based on Adaptive Extended Kalman Filter Positioning Module

HU Hui1，PENG Xiong-ming1，YANG De-jin2，LI Yang-da1
（1.School of Information Engineering，East China Jiaotong University，Nanchang 330013，China；2.National Time Service Center，Chinese Academy of Sciences，Xi’an 710000，China）

Abstract：In GPS positioning computation，when the actual data is changed，the Least Squares，the Extended Kalman Filter positioning results have low precision and poor stability problems.This paper proposes an Adaptive Fading Extended Kalman Filtering algorithm，through adaptive iterative system noise covariance，the modification to achieve less data mutation.Experimental results show that the algorithm is compared with the Least Squares，the Extended Kalman Filter，the positioning accuracy is improved.Compared with the traditional Fading Extended Kalman Filter，its convergence speed and stability are improved.

Key words：GPS，adaptive fading，extended kalman filter，positioning accuracy，stability

作者簡介：胡輝（1970-），男，江西南昌人，博士，教授。研究方向：衛(wèi)星導(dǎo)航定位，并行算法與分析處理，機器視覺。

*基金項目：江西省自然科學(xué)基金（20142BAB207001）；江西省教育廳科學(xué)技術(shù)基金資助項目（GJJ14369）

收稿日期：2015-03-16修回日期：2015-05-11

文章編號：1002-0640（2016）03-0177-06

中圖分類號：TN96

文獻標(biāo)識碼：A