摘 要：介紹了一種有效提高GPS定位精度的改進(jìn)卡爾曼濾波算法。該算法針對(duì)最小二乘法和標(biāo)準(zhǔn)卡爾曼濾波的特點(diǎn)，通過(guò)偽距估計(jì)出接收機(jī)的位置和鐘差，有效避免了由于濾波初值、系統(tǒng)噪聲方差以及量測(cè)噪聲方差帶來(lái)的濾波發(fā)散問(wèn)題。同時(shí)該算法不直接使用卡爾曼濾波來(lái)估計(jì)接收機(jī)的狀態(tài)，而是估計(jì)接收機(jī)狀態(tài)的誤差，減小了運(yùn)算量，有效提高了定位精度。在進(jìn)行狀態(tài)誤差估計(jì)時(shí)，不需要存儲(chǔ)大量測(cè)量數(shù)據(jù)，能方便地進(jìn)行動(dòng)態(tài)測(cè)量數(shù)據(jù)的實(shí)時(shí)處理。仿真結(jié)果證明此算法具有較快的收斂速度和較高的定位精度。

關(guān)鍵詞：卡爾曼濾波;定位解算;最小二乘法;定位精度

中圖分類號(hào)：TP22841 文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：1004373X(2008)0300403

Study of Improved Arithmetic of Kalman Filter on How to Improve

the Precision with Global Position System

TENG Yunlong1，2，CHEN Xiaoping1，TANG Yinghui2

(1.Research Institute of Electronic Science and Technology，University of Electronic Science and Technology of China，Chengdu，610054，China;

2.Applied Mathematics College，University of Electronic Science and Technology of China，Chengdu，610054，China)

Abstract：In this paper，we introduce a new algorithm of improving GPS positioning precision.This algorithm is based on the least—square method and normal Kalman filter.This algorithm can effectively restrain the filter′s divergence caused by the initial value of filter and system—noise—variance and measurement—noise—variance.The receiver position and clock offset can be estimated via measuring the pseudo ranges.Kalman filter is used to confirm the errors of paremeters instead of the parameters themselves，which reduces the operation errors and improves the positioning accuracy efficiently.During the estimation，the mass measurement data do not have to be saved.The data measured dynamically can be easily processed in real time.This simulation results indicate that it has positioning precision and high convergent rate.

Keywords：Kalman filter;positioning caculate;least—square method;positioning precision

在接收機(jī)定位解算中，通常采用最小二乘法或者卡爾曼濾波。他們各有優(yōu)缺點(diǎn)：最小二乘法只能利用當(dāng)前的觀測(cè)量，不能對(duì)觀測(cè)量進(jìn)行誤差分析，因此定位結(jié)果受觀測(cè)量的誤差影響比較大[1]，精度不高，但最小二乘法在初始迭代時(shí)收斂速度很快，受接收機(jī)初始概略坐標(biāo)的影響較小?？柭鼮V波不需要存儲(chǔ)大量數(shù)據(jù)，能方便地進(jìn)行動(dòng)態(tài)數(shù)據(jù)的實(shí)時(shí)處理[1—6]，但利用偽距作為觀測(cè)量進(jìn)行卡爾曼濾波時(shí)，需要接收機(jī)初始位置的概略坐標(biāo)。如果概略坐標(biāo)偏差很大，導(dǎo)致量測(cè)方程不準(zhǔn)確，容易造成濾波發(fā)散[2—6]，而且實(shí)時(shí)性較差。

本文綜合考慮了最小二乘法和常規(guī)卡爾曼濾波的優(yōu)缺點(diǎn)，提出了一種改進(jìn)的卡爾曼濾波算法。該算法利用最小二乘法提供接收機(jī)的初始坐標(biāo)，然后利用卡爾曼在已經(jīng)得到的初始坐標(biāo)的基礎(chǔ)上進(jìn)行濾波。同時(shí)，本文對(duì)卡爾曼濾波做了適當(dāng)?shù)男薷?，即不直接利用卡爾曼濾波估計(jì)接收機(jī)的狀態(tài)，而是估計(jì)狀態(tài)的誤差，使舍入和非線性誤差達(dá)到最小，同時(shí)在矩陣運(yùn)算中由于將許多元素設(shè)為零，減少了運(yùn)算量，提高了系統(tǒng)的實(shí)時(shí)性和運(yùn)算精度。

1 標(biāo)準(zhǔn)卡爾曼濾波模型

卡爾曼在1960年提出的卡爾曼濾波算法是一種線性最小方差估計(jì)方法。他通過(guò)建立式(1)中的系統(tǒng)方程和量測(cè)方程來(lái)描述系統(tǒng)的動(dòng)態(tài)變化過(guò)程，依據(jù)濾波增益矩陣的變化，從測(cè)量數(shù)據(jù)中提取有用信息，修正狀態(tài)參量，無(wú)需存儲(chǔ)不同時(shí)刻的觀測(cè)數(shù)據(jù)，便于實(shí)時(shí)數(shù)據(jù)處理。

X(k)=Φ(k/k-1)X(k-1)+Γ(k-1)W(k-1)

Z(k)=H(k)X(k)+V(k)[JY](1)

根據(jù)上述建立的卡爾曼濾波方程，給定濾波初值和相關(guān)噪聲統(tǒng)計(jì)特性，濾波過(guò)程如下[3]：

X(k/k-1)=Φ(k/k-1)X(k-1)

P(k/k-1)[WB]=Φ(k/k-1)P(k-1)Φ(k/k-1)′+

[DW] Γ(k-1)Q(k-1)Γ(k-1)′

K(k)[WB]=P(k/k-1)H(k)′(H(k)P(k/k-1)

[DW] H(k)′+R(k))-1

P(k)=(I-K(k)H(k))P(k/k-1)

X(k)[WB]=X(k/k-1)+K(k)(Z(k)-

[DW] H(k)X(k/k-1))[JY](2)

2 運(yùn)動(dòng)載體的GPS動(dòng)態(tài)定位系統(tǒng)數(shù)學(xué)模型

在建立卡爾曼濾波方程時(shí)，需要知道運(yùn)動(dòng)載體的GPS動(dòng)態(tài)定位系統(tǒng)數(shù)學(xué)模型。

根據(jù)接收機(jī)在k時(shí)刻與k-1時(shí)刻的關(guān)系，應(yīng)用Taylor級(jí)數(shù)展開(kāi)并略去高次項(xiàng)(具體的展開(kāi)階數(shù)根據(jù)精度來(lái)定，一般情況下展開(kāi)到三階導(dǎo)數(shù))，得到如下的數(shù)學(xué)模型：

δx(k)[WB]=δx(k-1)+T*δvx(k-1)+T^22*

[DW] δax(k-1)+T^36*δjx(k-1)

δvx(k)=δvx(k-1)+T*δax(k-1)

δy(k)[WB]=δy(k-1)+T*δvy(k-1)+T^22*

[DW] δay(k-1)+T^36*δjy(k-1)

δvy(k)=δvy(k-1)+T*δay(k-1)

δz(k)[WB]=δz(k-1)+T*δvz(k-1)+T^22*

[DW] δaz(k-1)+T^36*δjz(k-1)

δvz(k)=δvz(k-1)+T*δaz(k-1)

b(k)=b(k-1)+T*δb(k-1)

δb(k)=δb(k-1)[JY](3)

在模型(3)中，各變量的含義如下：δx，δy，δz表示3個(gè)方向的位置坐標(biāo)誤差；δvx，δvy，δvz表示速度誤差；δax，δay，δaz表示加速度誤差；δjx，δjy，δjz表示加加速度誤差；b表示接收機(jī)時(shí)鐘偏差；δb表示接收機(jī)時(shí)鐘漂移誤差，T表示采樣周期。

3 提高GPS定位精度的改進(jìn)卡爾曼濾波算法的實(shí)現(xiàn)

在進(jìn)行卡爾曼濾波時(shí)，關(guān)鍵是確定狀態(tài)向量、建立系統(tǒng)方程和量測(cè)方程、確定噪聲統(tǒng)計(jì)特性等。我們將在下面的部分中，分別加以討論。

3.1 系統(tǒng)的狀態(tài)向量

根據(jù)上面推導(dǎo)的系統(tǒng)數(shù)學(xué)模型式(3)，選取如下的系統(tǒng)狀態(tài)向量和系統(tǒng)噪聲向量：系統(tǒng)狀態(tài)向量選取為X=[δx δvx δy δvy δz δvz b δb]，將加速度、加加速度轉(zhuǎn)化為隨機(jī)噪聲，用白噪聲來(lái)描述；系統(tǒng)噪聲向量W=[δax δjx δay δjy δaz δjz]。

3.2 系統(tǒng)方程的推導(dǎo)

根據(jù)系統(tǒng)的誤差狀態(tài)向量、系統(tǒng)噪聲向量和數(shù)學(xué)模型(3)，得到的系統(tǒng)方程如下：

X(k)=Φ(k/k-1)X(k-1)+Γ(k-1)W(k-1)

其中：

3.3 量測(cè)方程的推導(dǎo)

量測(cè)方程矩陣的元素把觀測(cè)量和狀態(tài)向量聯(lián)系起來(lái)，本方法采用偽距做為觀測(cè)量。為完成觀測(cè)量和狀態(tài)向量的關(guān)聯(lián)，需要把每次接收到的測(cè)量值轉(zhuǎn)化為位置誤差、速度誤差以及接收機(jī)時(shí)鐘差，然后把這些誤差分解到其狀態(tài)矢量的分量中。

應(yīng)用偽距作為量測(cè)值進(jìn)行解算時(shí)，偽距方程組[1，2，7]:

在應(yīng)用卡爾曼濾波進(jìn)行數(shù)據(jù)處理時(shí)，我們對(duì)上面的wj進(jìn)行修改，近似認(rèn)為wj=ρj，將式(6)中wj的其他項(xiàng)都轉(zhuǎn)化到偽距的測(cè)量誤差中，得到的結(jié)果如下：

設(shè)視界內(nèi)的衛(wèi)星數(shù)為N，將上述方程組寫(xiě)為矩陣形式，同時(shí)考慮到相應(yīng)的偽距測(cè)量誤差，得到量測(cè)方程：

Z(k)=[WTHX]H[WTBX](k)X(k)+V(k)

其中，Z(k)=[L1 L2 = LN]T，V(k)為量測(cè)噪聲序列，[WTHX]H[WTBX](k)為量測(cè)矩陣；

3.4 噪聲統(tǒng)計(jì)特性的確定

系統(tǒng)噪聲和量測(cè)噪聲都是零均值的高斯白噪聲序列[1，3，4，5]。系統(tǒng)噪聲方差陣Q的主對(duì)角元素構(gòu)成的向量為[δ2Dδ2dδ2Dδ2dδ2Dδ2d]，量測(cè)噪聲方差陣R的主對(duì)角元素構(gòu)成的向量為[δ2Dδ2D … δ2D]，方差陣Q與R的其他元素為零。其中：δ2D表示偽距的噪聲方差，δ2d表示偽距率的噪聲方差。

3.5 濾波初值的選取

卡爾曼濾波是遞推算法，需給定狀態(tài)向量初始值X(0)與初始估計(jì)均方誤差矩陣P(0)。在不了解系統(tǒng)特性的情況下，常令X(0)=0，同時(shí)令初始估計(jì)均方誤差矩陣P(0)=αI(其中α是很大的正數(shù)，I為單位矩陣)，這樣可以保證濾波的結(jié)果是無(wú)偏的[3]。

4 仿真分析

仿真條件為：采樣周期T=1 s，常數(shù)α=1 000 000.0，偽距噪聲方差δD2= 100.0 m2，偽距率噪聲方差δd2=25.0 m2/s，初始的狀態(tài)向量X(0)=0。用于定位解算的衛(wèi)星的位置坐標(biāo)和偽距來(lái)源于文獻(xiàn)[7]。圖1給出了沿X軸方向的卡爾曼濾波定位解算的位置誤差曲線，圖2給出了沿x軸方向的卡爾曼濾波定位解算的速度誤差曲線。

從兩圖可以看出，采用估計(jì)接收機(jī)載體狀態(tài)誤差的方法大約經(jīng)過(guò)10次迭代，位置和速度就能夠穩(wěn)定而且精度很高。隨著卡爾曼濾波的不斷進(jìn)行，位置和速度誤差逐漸降低，最后趨近于零。仿真結(jié)果證明了此方法具有較高的定位精度和較快的收斂速度。

5 結(jié) 語(yǔ)

針對(duì)最小二乘法以及常規(guī)卡爾曼濾波算法的不足，本文研究了的改進(jìn)卡爾曼濾波算法。該算法結(jié)合了最小二乘法與卡爾曼濾波算法的優(yōu)點(diǎn)，采用偽距作為觀測(cè)量，不直接估計(jì)載體的狀態(tài)，而是估計(jì)載體的狀態(tài)誤差從而對(duì)載體的狀態(tài)進(jìn)行估計(jì)，這樣使估計(jì)誤差達(dá)到最小。以上的理論分析和仿真結(jié)果表明：研究的改進(jìn)卡爾曼濾波定位算法是合理的，對(duì)濾波模型的建立、狀態(tài)向量和噪聲向量的選取、系統(tǒng)噪聲方差陣和量測(cè)噪聲方差陣等問(wèn)題的處理方法是可行的。此算法濾波過(guò)程穩(wěn)定，具有較快的收斂速度和較高的定位精度。

參考文獻(xiàn)

［1］張守信.GPS技術(shù)與應(yīng)用[M].北京:國(guó)防工業(yè)出版社，2004.

［2］劉立龍.動(dòng)態(tài)對(duì)動(dòng)態(tài)GPS高精度定位理論及其應(yīng)用研究[D].武漢:武漢大學(xué)，2005.

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

［4］帥平，陳定昌，江涌.關(guān)于GPS導(dǎo)航計(jì)算的卡爾曼濾波問(wèn)題[M].遙測(cè)遙控，2001，22(5):14—20.

［5］董緒榮，陶大欣.一個(gè)快速Kalman濾波方法及其在GPS動(dòng)態(tài)數(shù)據(jù)處理中的應(yīng)用[J].測(cè)繪學(xué)報(bào)，1997，26(3):221—227.

［6］Euler H，Goad C C.On Optional Filtering of GPS Dual-Frequency Observations without Using Orbit Information.Bulletin Giodisique，1991，65(2):130—143.

［7］John B.Lundberg.Alternative Algorithms for the GPS Statict positioning Solution.Applied Mathematics and Computation，2001，119:21—34.

作者簡(jiǎn)介

滕云龍 男，1982年出生，碩士研究生。主要從事GPS及衛(wèi)星導(dǎo)航領(lǐng)域的研究工作。

注：本文中所涉及到的圖表、注解、公式等內(nèi)容請(qǐng)以PDF格式閱讀原文。