KALMAN濾波在GPS導(dǎo)航定位中的應(yīng)用

1、 GPS導(dǎo)航定位中Kalman算法的應(yīng)用 摘要：GPS是美國從本世紀(jì)70年代開始研制，經(jīng)過20年,耗資200億美元，在1994年全面建成，具有在海陸空進(jìn)行全方位實(shí)時(shí)三維導(dǎo)航與定位能力的新一代衛(wèi)星導(dǎo)航與定位系統(tǒng)。GPS技術(shù)包括很多優(yōu)勢，其中最吸引人的地方就是它的全球性、全能性、全天侯性，不僅使得它能為用戶隨時(shí)隨地提供定位信息，而且使它可以廣泛應(yīng)用于全球各個(gè)軍事部門和民用等諸多領(lǐng)域，但是因?yàn)镚PS定位包含多種誤差源，這就使其定位精度受到了一定的影響。本文以介紹全球定位系統(tǒng)為前提，首先簡單的介紹GPS，其次在利用GPS定位測偽距和已知衛(wèi)星坐標(biāo)經(jīng)過絕對(duì)位置解算的前提下，為了改善和提高定位算法的精度，也

2、為了GPS定位技術(shù)的應(yīng)用更加廣泛，從而提出Kalman濾波算法。綜上所述，本文的研究工作對(duì)導(dǎo)航定位系統(tǒng)中的傳統(tǒng)濾波方法的改進(jìn)有一定的參考，并對(duì)Kalman濾波算法在改善GPS動(dòng)態(tài)導(dǎo)航的動(dòng)態(tài)性能和提高定位精度方面的應(yīng)用具有一定的實(shí)用價(jià)值。關(guān)鍵詞: GPS;偽距測量;卡爾曼濾波 Abstract GPS is developed since 70 s of this century in the United States, after 20 years, at a cost of $20 billion, completed in 1994, has in carring all-round r

3、eal-time three-dimensional navigation and positioning capability of a new generation of satellite navigation and positioning system.GPS technology including many advantages, one of the most attractive place is its global, totipotency, 24 hours a day, not only make it for the user to provide location

4、 information anytime and anywhere, and makes it can be widely used in many fields such as military and civilian, but because the GPS contains a variety of error sources, which makes its positioning accuracy is influenced by a certain. Based on global positioning system is introduced in this paper, f

5、irst of all, a simple introduction to GPS, second is using GPS pseudorange measurement and known satellite coordinates through the absolute position under the premise of solving, in order to improve and improve the accuracy of localization algorithm, but also to the application of GPS technology is

6、more extensive, and Kalman filter algorithm is put forward. To sum up, in this paper, the research work of navigation and positioning system of traditional filtering method improvement has a certain reference, and the Kalman filtering algorithm in improving the dynamic performance and improve the po

7、sitioning precision GPS navigation application has a certain practical value.Keywords：GPS; recursive least squares; kalman filter.目錄摘要IIAbstractII1緒論21.1 概述21.2 研究背景21.3 本文研究思路與工作安排22 GPS導(dǎo)航定位系統(tǒng)的基本原理32.1 GPS導(dǎo)航系統(tǒng)的組成及誤差組成22.2 GPS導(dǎo)航定位原理42.3 kalman濾波理論53 kalman濾波算法的matlab驗(yàn)證仿真23.1.一步預(yù)測63.1.1算法設(shè)計(jì)及驗(yàn)證63.1.2算

8、法仿真及結(jié)果分析83.2 多步預(yù)測114 kalman濾波算法的c語言算法程序134.1.一步預(yù)測134.1.1算法設(shè)計(jì)及驗(yàn)證134.1.2算法測試及驗(yàn)證144.1.3算法仿真及結(jié)果分析154.2 多步預(yù)測164.2.1.算法流程圖及設(shè)計(jì)164.2.2.算法仿真及結(jié)果分析17總結(jié)與展望19致 謝19參考文獻(xiàn)2 1緒論1.1概述GPS 計(jì)劃始于1973 年，已于1994 年進(jìn)入完全運(yùn)行狀態(tài)。利用該系統(tǒng)，用戶不僅可以在全球范圍內(nèi)實(shí)現(xiàn)全天候、連續(xù)、實(shí)時(shí)的三維導(dǎo)航定位和測速，還能夠進(jìn)行高精度的時(shí)間傳遞和高精度的精密定位。GPS 利用已知四顆衛(wèi)星的空間坐標(biāo)位置，以及接收到的用戶與衛(wèi)星之間的偽距進(jìn)行絕對(duì)位

9、置解算。GPS整個(gè)系統(tǒng)由空間衛(wèi)星星座、地面監(jiān)控系統(tǒng)、用戶接收設(shè)備組成。其中的空間部分由21顆衛(wèi)星和3顆備用衛(wèi)星組成。這樣的空間分布使得在地球上任何地點(diǎn)、任何時(shí)刻接收機(jī)都能至少觀測到4顆衛(wèi)星1。隨著目前科學(xué)技術(shù)的發(fā)展， GPS的應(yīng)用已經(jīng)滲透在人們的日常生活中，可見怎樣更好的利用GPS已成為當(dāng)下我國科技工作的一個(gè)刻不容緩的任務(wù)。近些年隨著目前科學(xué)技術(shù)的發(fā)展，GPS導(dǎo)航定位的應(yīng)用在我國得到了廣泛的推廣并滲透在人們的日常生活中。但是由于GPS導(dǎo)航定位在精度上尚且存在提高的空間，因此，怎樣進(jìn)一步的改善GPS導(dǎo)航定位的動(dòng)態(tài)性能和精度，將成為當(dāng)下研究的一個(gè)重點(diǎn)。1.2研究背景 二十世紀(jì)以后，隨著人類科學(xué)技術(shù)

10、水平的提高和各種探險(xiǎn)活動(dòng)的頻繁，人們已不再滿足于GPS的粗略定位，而是在精確定位、精確導(dǎo)航、建立高精度的全國性的大地測量控制網(wǎng)、建立陸地海洋大地測量基準(zhǔn)、監(jiān)測地球板塊運(yùn)動(dòng)狀態(tài)和地殼形變、測定航空航天攝影瞬間的相機(jī)位置等多方面對(duì)GPS都提出了更高的要求。GPS定位系統(tǒng)是目前一套不僅穩(wěn)定性好而且精度高的衛(wèi)星定位系統(tǒng)，它除了能給用戶提供運(yùn)動(dòng)中的物體的速度信息還有提供空間位置坐標(biāo)。GPS采用的是測偽距定位原理，在接收機(jī)觀測到的空間衛(wèi)星數(shù)目至少達(dá)到4顆時(shí)，利用導(dǎo)航的定位算法實(shí)現(xiàn)絕對(duì)位置解算。方案一：最小二乘算法在日常生活中，由于慣性的存在，用戶接收機(jī)位置無法突變，相鄰時(shí)刻的位置必然存在一定的聯(lián)系。最小二

11、乘算法雖能夠?qū)崿F(xiàn)GPS定位，但其卻沒有將相鄰的不同時(shí)刻的用戶位置與之聯(lián)系起來，這就使得最小二乘法存在著無法避免的不足，定位精度有待提高。因此將最小二乘算法與卡爾曼濾波算法相比較，前者的定位結(jié)果較粗糙，定位精度存在一定的不足，故在工程實(shí)際應(yīng)用中提倡采用卡爾曼濾波算法。 方案二：卡爾曼濾波算法卡爾曼濾波技術(shù)最小方差估計(jì)就是讓系統(tǒng)最初的真值減去經(jīng)過濾波后的值，再對(duì)這個(gè)差值求方差，要讓這個(gè)方差達(dá)到最小。它只需利用前一時(shí)刻的狀態(tài)最優(yōu)估計(jì)值以及當(dāng)前時(shí)刻獲得的最新觀測量便能求出當(dāng)前時(shí)刻的狀態(tài)最優(yōu)估計(jì)值?？柭鼮V波實(shí)質(zhì)上就是系統(tǒng)協(xié)方差的遞歸，它的這種特點(diǎn)使它在沒法存儲(chǔ)大量數(shù)據(jù)的系統(tǒng)中得到了廣泛的應(yīng)用。綜上所述

12、，研究GPS中的卡爾曼濾波定位算法有其不容忽視的實(shí)用價(jià)值。1.3本文研究思路與工作安排本論文的所做工作先是簡單的介紹了GPS的研究背景、組成部分、基本定位原理。然后與最小二乘法相比提出了更高精度的定位算法卡爾曼濾波算法，并對(duì)其進(jìn)行了簡單的仿真。 本論文的工作安排如下：第一章：首先介紹了GPS的目前狀況、研究背景以及意義。第二章：介紹了GPS導(dǎo)航定位系統(tǒng)的基礎(chǔ)知識(shí)和卡爾曼濾波理論。第三章：講述了卡爾曼濾波基于matlab的實(shí)驗(yàn)仿真。第四章：介紹了卡爾曼濾波基于visual C的實(shí)驗(yàn)仿真。第五章：總結(jié)與展望，總結(jié)了本文所做的主要工作，并且具體分析了所做工作的不足之處。2 GPS導(dǎo)航定位系統(tǒng)的基本原

13、理2.1 GPS導(dǎo)航系統(tǒng)的組成及誤差組成GPS導(dǎo)航系統(tǒng)有三個(gè)組成部分：空間衛(wèi)星星座、地面監(jiān)控系統(tǒng)、用戶接收設(shè)備。其中空間衛(wèi)星星座是由位于6個(gè)軌道平面且軌道傾角為55度的24顆衛(wèi)星組成。當(dāng)GPS進(jìn)行定位時(shí)，接收機(jī)至少需要觀測到4顆GPS衛(wèi)星，聯(lián)立方程求解出待測點(diǎn)的空間三維坐標(biāo)2。這部分不僅可以提供衛(wèi)星星歷、時(shí)間信息和其他有用的輔助信息還可以發(fā)射測量偽距的信號(hào)與載表信號(hào)。一個(gè)主控站、三個(gè)注入站和五個(gè)監(jiān)測站構(gòu)成了GPS的地面監(jiān)測系統(tǒng)。它是中心控制系統(tǒng)可以實(shí)現(xiàn)時(shí)間同步跟蹤衛(wèi)星進(jìn)行定軌。用戶接收機(jī)主要負(fù)責(zé)捕獲待測衛(wèi)星的信號(hào)，接收和處理衛(wèi)星信號(hào)并對(duì)處理后的數(shù)據(jù)進(jìn)行記錄，另外的重要作用是為用戶提供了精確的定

14、位信息。接收機(jī)配有石英鐘，比原子鐘的精度低很多，當(dāng)接收到GPS信號(hào)時(shí)，接收機(jī)要復(fù)制C/A碼和P碼，利用補(bǔ)償技術(shù)提取信息。接收機(jī)構(gòu)造類型決定了解碼的可能性，因此就決定了定位測量的精度。GPS導(dǎo)航定位系統(tǒng)的組成分為三部分，由于星座的鐘差、傳播過程的損失、接收機(jī)的鐘差等一些原因致使每一部分都存在著一定的無法避免的誤差，誤主要包括：星歷誤差、傳播誤差和接受誤差3。圖2-1 導(dǎo)航定位誤差組成 2.2 GPS導(dǎo)航定位原理GPS系統(tǒng)的基本定位原理是：通過記錄衛(wèi)星發(fā)射的測距碼信號(hào)傳送到到用戶接收機(jī)的時(shí)間乘以光速來算出偽距，再利用衛(wèi)星星歷查出已知衛(wèi)星在確定的某時(shí)刻的具體位置坐標(biāo)，最后根據(jù)以上所獲得得數(shù)據(jù)來計(jì)算出

15、用戶接收機(jī)的具體位置坐標(biāo)。因?yàn)榭臻g衛(wèi)星星載的時(shí)鐘和用戶接受機(jī)使用的時(shí)鐘做到永遠(yuǎn)同步幾乎是不可能的，一定存在某個(gè)時(shí)刻兩者的時(shí)鐘不同步，故除了用戶接收機(jī)的三維空間坐標(biāo)的x、y、z外，還需要增加一個(gè)未知數(shù)，即就是已知的空間衛(wèi)星和用戶接收機(jī)之間的鐘差，用t表示。從以上所述可知，為了能夠求出上面所提到的四個(gè)未知數(shù)那么就需要建立四個(gè)方程，所以用戶接收機(jī)觀測到的衛(wèi)星數(shù)目至少為四顆，接著接收來自這四顆衛(wèi)星發(fā)送的信號(hào)，才可確定出用戶接收機(jī)的具體空間位置。如下圖所示，假設(shè)t時(shí)刻用戶接收機(jī)測定出GPS信號(hào)到達(dá)接收機(jī)的時(shí)間t，接著利用接收機(jī)所接收到的來自衛(wèi)星的信號(hào)查出衛(wèi)星星歷等其它數(shù)據(jù)就可以可以確定以下四個(gè)方程式4。

16、 S1 (x1,y1,z1) S2(x2,y2,z2) S3(x3,y3,z3) S4(x4,y4,z4) 圖2-2 GPS定位原理圖 （2-1）綜上所述的方程組有四個(gè)未知量，分別為，其中為為待測點(diǎn)的空間三圍坐標(biāo)，為已知的四顆衛(wèi)星在t時(shí)刻的空間三維坐標(biāo)，為星座衛(wèi)星到用戶接收機(jī)之間的距離，為各已知衛(wèi)星信號(hào)到達(dá)用戶接收機(jī)所經(jīng)歷的時(shí)間，c為光速，為接收機(jī)的鐘差，是未知的。為衛(wèi)星的鐘差，是已知的5。將上述參數(shù)代入四個(gè)方程中，便可求解出其中的，即就是待測點(diǎn)的具體空間位置和接收機(jī)鐘差。2.3 kalman濾波理論Kalman濾波就是將上一時(shí)刻的最優(yōu)估計(jì)與最新的量測序列合并后，計(jì)算出當(dāng)前時(shí)刻的最優(yōu)估計(jì)。適宜

17、應(yīng)用在處理多變、時(shí)變和非平穩(wěn)系統(tǒng)的隨機(jī)過程。假設(shè)線性系統(tǒng)的離散模型為： （2-2） （2-3）Kalman濾波的時(shí)間更新方程如下： （2-4） （2-5） 量測更新方程如下： （2-6） （2-7） （2-8）在時(shí)刻系統(tǒng)的初值分別為系統(tǒng)狀態(tài)和系統(tǒng)狀態(tài)的協(xié)方差。Kalman濾波過程主要體現(xiàn)在式（2-4）（2-8）。主要分為兩個(gè)過程：時(shí)間更新階段和兩側(cè)更新階段6。時(shí)間更新階段利用初值計(jì)算系統(tǒng)狀態(tài)及其協(xié)方差的先驗(yàn)估計(jì)值，修正階段就是利用前面求得的先驗(yàn)估計(jì)值結(jié)合最新獲得的量測值來計(jì)算后驗(yàn)估計(jì)和濾波增益。式（2-8）另一種常見的形式為： （2-9）可將計(jì)算的公式展開為如下形式： （2-10）3 kalm

18、an濾波算法基于matlab的驗(yàn)證仿真3.1一步預(yù)測3.1.1算法設(shè)計(jì)及驗(yàn)證（1）核心算法：圖3-1 matlab程序 （2）系統(tǒng)真實(shí)值給定連續(xù)的固定量，且沒有噪聲時(shí)，根據(jù)測量值來使用卡爾曼濾波算法來對(duì)系統(tǒng)的真實(shí)值做最優(yōu)估計(jì)。由于沒有噪聲則經(jīng)過濾波器的最優(yōu)估計(jì)和真實(shí)值一樣。圖3-2 matlab程序輸出結(jié)果圖3-3 matlab程序真值和濾波后的輸出結(jié)果（3）系統(tǒng)真實(shí)值給定連續(xù)的固定量，且有噪聲時(shí)，根據(jù)測量值來使用卡爾曼濾波算法來對(duì)系統(tǒng)的真實(shí)值做最優(yōu)估計(jì)。輸出的最優(yōu)估計(jì)如下圖所示：圖3-5 matlab程序輸出結(jié)果估計(jì)誤差的方差為0.7288，量測誤差的方差為3.1225（4）系統(tǒng)真實(shí)值給定連

19、續(xù)的隨機(jī)值，且有噪聲時(shí)，根據(jù)測量值來使用卡爾曼濾波算法來對(duì)系統(tǒng)的真實(shí)值做最優(yōu)估計(jì)。輸出的最優(yōu)估計(jì)如下圖所示:圖3-6 matlab程序輸出結(jié)果估計(jì)誤差的方差輸出結(jié)果為0.0102，量測誤差的方差為0.03633.1.2算法仿真及結(jié)果分析為了測試kalman filter algorithm，我設(shè)計(jì)了一個(gè)簡單的實(shí)驗(yàn)，來驗(yàn)證kalman filter的正確性。程序中給定系統(tǒng)真實(shí)值的初值，然后給定一組測量值，驗(yàn)證kalman filter估值的準(zhǔn)確性。根據(jù)kalman filter algorithm，我們需要給定系統(tǒng)初始值，系統(tǒng)噪聲Q(程

20、序中用Rww表示)和測量噪聲R（程序中用Rvv表示）（Q和R分別是系統(tǒng)過程噪聲和量測噪聲的方差）以及初始值所對(duì)應(yīng)的協(xié)方差P2（1）。為了驗(yàn)證優(yōu)劣性，還需要用給定的真實(shí)值和量測序列來計(jì)算kalman filter誤差，以及測量誤差。（1）實(shí)驗(yàn)中給定連續(xù)的真實(shí)值，噪聲一定且x_bef(1)=0,P_now(1)=0.02可以通過適當(dāng)改變Rvv和Rww來獲得更好的估計(jì)結(jié)果。1）Rvv=0.056,Rww=0.1圖3-7為Rvv=0.056,Rww=0.1的輸出結(jié)果估計(jì)誤差的方差輸出結(jié)果為0.0153，量測誤差的方差為0.22742）Rvv=0.072,Rww=0.1圖3-8 為Rvv=0.

21、072，Rww=0.1的輸出結(jié)果估計(jì)誤差的方差輸出結(jié)果為0.0175，量測誤差的方差為0.22743）Rvv=0.072,Rww=0.8圖3-9為Rvv=0.072 Rww=0.8的輸出結(jié)果估計(jì)誤差的方差輸出結(jié)果為0.0053，量測誤差的方差為0.2274綜上所述，當(dāng)有x_bef（1）=0，P_now（1）=0.02，改變Rvv和Rww估計(jì)誤差方差和量測誤差方差有不同的結(jié)果：Rvv=0.056Rww=0.1Rvv=0.072Rww=0.1Rvv=0.072Rww=0.8估計(jì)誤差方差0.01530.0175 0.0053量測誤差方差 0.2274 0.2268 0.2271表一 改變Rvv和Rw

22、w（2）實(shí)驗(yàn)中確定系統(tǒng)真實(shí)值不變，Rww和Rvv不變，改變P_now（1）的值，由于kalman filter是對(duì)協(xié)方差的遞歸算法來估計(jì)信號(hào)數(shù)據(jù)的，所以P2(1)對(duì)算法結(jié)果的影響較大。 1）P2(1)=10圖3-10改變P2(1)=10的輸出結(jié)果估計(jì)誤差的方差輸出結(jié)果為0.0154，量測誤差的方差為 0.2275綜上所述，當(dāng)有x_bef（1）=0，Rvv=0.056,Rww=0.1,改變P_now（1）的值，估計(jì)誤差方差和量測誤差方差有不同的結(jié)果：P_now（1）=0.02P_now（1）=10估計(jì)誤差方差0.01530.0154量測誤差方差 0.2274 0.2275表二 改變P_

23、now（1）3.2多步預(yù)測算法設(shè)計(jì)（為了編程簡單，選取F=1，H=1）（1）系統(tǒng)真實(shí)值x_real=0.82，且有噪聲時(shí)(Rww=0.018,Rvv=0.542)，根據(jù)測量值來使用kalman filter來對(duì)系統(tǒng)的真實(shí)值做最優(yōu)估計(jì)。輸出的最優(yōu)估計(jì)如下圖所示：量測輸出：濾波輸出：累積卡爾曼誤差為0.0776 ，累積測量誤差為0.1284（2）系統(tǒng)真實(shí)值x_real=0.82，且有噪聲時(shí)(Rww=0.021,Rvv=0.63)，根據(jù)測量值來使用kalman filter來對(duì)系統(tǒng)的真實(shí)值做最優(yōu)估計(jì)。輸出的最優(yōu)估計(jì)如下圖所示：量測輸出：濾波輸出： 累積卡爾曼誤差為0.0769 ，

24、累積測量誤差為0.1272綜上所述x_real=0.82，且有噪聲時(shí)改變Rww和Rvv的值，累積誤差的變化：x_real=0.82Rvv=0.542Rww=0.018Rvv=0.63Rww=0.021累積卡爾曼誤差0.07760.0769累積量測誤差 0.1284 0.1272表三 改變Rvv和Rww4 kalman濾波算法基于visual c的驗(yàn)證仿真4.1一步預(yù)測4.1.1算法流程圖及設(shè)計(jì)算法流程圖： 系統(tǒng)狀態(tài)和狀態(tài)協(xié)方差的初始化X_now0, P_now0,H,F時(shí)間更新階段 量測更新階段 核心算法設(shè)計(jì)：4.1.2算法測試和驗(yàn)證（1）系統(tǒng)真實(shí)值給定連續(xù)的固定量且沒有噪聲時(shí)，根據(jù)測量值來使

25、用kalman filter來對(duì)系統(tǒng)的真實(shí)值做最優(yōu)估計(jì)。由于沒有噪聲則經(jīng)過濾波器的最優(yōu)估計(jì)和真實(shí)值一樣。（2）系統(tǒng)真實(shí)值給定連續(xù)的固定量，且有噪聲一定時(shí)，根據(jù)測量值來使用kalman filter來對(duì)系統(tǒng)的真實(shí)值做最優(yōu)估計(jì)。輸出的最優(yōu)估計(jì)如下圖所示：（3）系統(tǒng)真實(shí)值給定連續(xù)的隨機(jī)值，且有噪聲時(shí)，根據(jù)測量值來使用kalman filter來對(duì)系統(tǒng)的真實(shí)值做最優(yōu)估計(jì)。輸出的最優(yōu)估計(jì)如下圖所示: 4.1.3算法仿真和結(jié)果分析系統(tǒng)真實(shí)值給定連續(xù)的固定量，且有噪聲一定時(shí)，根據(jù)測量值來使用kalman filter來對(duì)系統(tǒng)的真實(shí)值做最優(yōu)估計(jì)。（1）Rvv=0.32,

26、Rww=0.892（2）Rvv=0.036,Rww=0.74綜上所述：輸入一定，噪聲一定時(shí)，改變Rww和Rvv的值，累積誤差的變化：Rvv=0.32Rww=0.892Rvv=0.036Rww=0.74累積卡爾曼誤差0.34470.8526累積量測誤差 1.2632 1.8000表四 改變Rvv和Rww4.2多步預(yù)測算法設(shè)計(jì)4.2.1算法流程圖及設(shè)計(jì)算法流程圖： 系統(tǒng)狀態(tài)和狀態(tài)協(xié)方差的初始化X_now0, P_now0,H,F時(shí)間更新階段 更新系統(tǒng)狀態(tài)及其協(xié)方差 量測更新階段 核心算法設(shè)計(jì)：4.2.2算法仿真及結(jié)果分析（1）Rvv=0.892,Rww=0.036（2）Rvv=0.45,Rww=0.762（3）Rvv=0.012,Rww=0.892綜上訴述：輸入相同條件下，改變Rvv和Rww值，兩種累積誤差和卡爾曼誤差所占比例：Rvv=0.892Rww=0.036Rvv=0.45Rww=0.762Rvv=0.012Rww=0.892卡爾曼累積誤差0.1983930.252586 0.309337測量累積誤差0.312517 0.3125170.312517卡爾曼誤差所占比例37%20%2%表五 改變Rvv和Rww總結(jié)與展望經(jīng)過第四章的仿真結(jié)