胡　輝，彭雄明，楊德進(jìn)，李陽達(dá)

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

?

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

胡輝1，彭雄明1，楊德進(jìn)2，李陽達(dá)1

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

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

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

0　引言

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

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

1定位解算模型

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

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

1.1狀態(tài)方程

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

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

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

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

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

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

將式（9）帶入式（8）中化簡(jiǎn)得

由式（4）可以推得

化簡(jiǎn)式（12）得到離散化的Qk如下:

1.2觀測(cè)方程

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

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

多普勒頻率測(cè)量值的計(jì)算：

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

對(duì)觀測(cè)方程式（14）和式（15）而言，可以寫成矩陣的形式：

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

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

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

其觀測(cè)方程矩陣Hk滿足：

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

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

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

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

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

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

2.1.2觀測(cè)量更新

卡爾曼增益計(jì)算：

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

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

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

估計(jì)的殘差向量：

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

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

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

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

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

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

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

式（29）中:

對(duì)式（31）兩邊求跡，得到如下表達(dá)式：

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

3試驗(yàn)結(jié)果分析

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

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

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

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

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

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

3.2 EKF、AFEKF勻速圓周運(yùn)動(dòng)場(chǎng)景比較

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

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

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

3.3 LS、EKF、AFEKF動(dòng)態(tài)突變場(chǎng)景比較

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

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

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

從圖5～圖7中可以看出，LS的定位誤差有較大的擾動(dòng)，在有突變的情況下，EKF和AFEKF處理效果，比LS效果要好；AFEKF在收斂性上，較EKF更能收斂到更加準(zhǔn)確的值。結(jié)合圖5～圖7，可以看出，起始時(shí)間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ù)突變的場(chǎng)景，傳統(tǒng)FEKF、未自適應(yīng)系統(tǒng)噪聲FEKF、自適應(yīng)系統(tǒng)噪聲AFEKF算法定位結(jié)果對(duì)比圖，圖8～圖10分別是X、Y、Z方向上的定位結(jié)果誤差圖。

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

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

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

4　結(jié)論

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

參考文獻(xiàn)：

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

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

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

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

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

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

［7］魏偉，秦永元，張曉冬，等.對(duì)Sage-Husa算法的改進(jìn)［J］.中國慣性技術(shù)學(xué)報(bào)，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全球定位接收機(jī)—原理與軟件實(shí)現(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

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

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

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

文章編號(hào)：1002-0640（2016）03-0177-06

中圖分類號(hào)：TN96

文獻(xiàn)標(biāo)識(shí)碼：A