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

2、為了GPS定位技術(shù)的應(yīng)用更加廣泛，從而提出Kalman濾波算法。綜上所述，本文的研究工作對導(dǎo)航定位系統(tǒng)中的傳統(tǒng)濾波方法的改進(jìn)有一定的參考，并對Kalman濾波算法在改善GPS動態(tài)導(dǎo)航的動態(tài)性能和提高定位精度方面的應(yīng)用具有一定的實用價值。關(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驗證仿真23.1.一步預(yù)測63.1.1算法設(shè)計及驗證63.1.2算

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

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

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

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

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

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

14、位信息。接收機(jī)配有石英鐘，比原子鐘的精度低很多，當(dāng)接收到GPS信號時，接收機(jī)要復(fù)制C/A碼和P碼，利用補償技術(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ā)射的測距碼信號傳送到到用戶接收機(jī)的時間乘以光速來算出偽距，再利用衛(wèi)星星歷查出已知衛(wèi)星在確定的某時刻的具體位置坐標(biāo)，最后根據(jù)以上所獲得得數(shù)據(jù)來計算出

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

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

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

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

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

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

21、為0.22743）Rvv=0.072,Rww=0.8圖3-9為Rvv=0.072 Rww=0.8的輸出結(jié)果估計誤差的方差輸出結(jié)果為0.0053，量測誤差的方差為0.2274綜上所述，當(dāng)有x_bef（1）=0，P_now（1）=0.02，改變Rvv和Rww估計誤差方差和量測誤差方差有不同的結(jié)果：Rvv=0.056Rww=0.1Rvv=0.072Rww=0.1Rvv=0.072Rww=0.8估計誤差方差0.01530.0175 0.0053量測誤差方差 0.2274 0.2268 0.2271表一 改變Rvv和Rww（2）實驗中確定系統(tǒng)真實值不變，Rww和Rvv不變，改變P_now（1）的值，由于

22、kalmanfilter是對協(xié)方差的遞歸算法來估計信號數(shù)據(jù)的，所以P2(1)對算法結(jié)果的影響較大。 1）P2(1)=10圖3-10改變P2(1)=10的輸出結(jié)果估計誤差的方差輸出結(jié)果為0.0154，量測誤差的方差為 0.2275綜上所述，當(dāng)有x_bef（1）=0，Rvv=0.056,Rww=0.1,改變P_now（1）的值，估計誤差方差和量測誤差方差有不同的結(jié)果：P_now（1）=0.02P_now（1）=10估計誤差方差0.01530.0154量測誤差方差 0.2274 0.2275表二 改變P_now（1）3.2多步預(yù)測算法設(shè)計（為了編程簡單，選取F=1，H=1）（1）系統(tǒng)真實值x_rea

23、l=0.82，且有噪聲時(Rww=0.018,Rvv=0.542)，根據(jù)測量值來使用kalmanfilter來對系統(tǒng)的真實值做最優(yōu)估計。輸出的最優(yōu)估計如下圖所示：量測輸出：濾波輸出：累積卡爾曼誤差為0.0776 ，累積測量誤差為0.1284（2）系統(tǒng)真實值x_real=0.82，且有噪聲時(Rww=0.021,Rvv=0.63)，根據(jù)測量值來使用kalmanfilter來對系統(tǒng)的真實值做最優(yōu)估計。輸出的最優(yōu)估計如下圖所示：量測輸出：濾波輸出： 累積卡爾曼誤差為0.0769 ，累積測量誤差為0.1272綜上所述x_real=0.82，且有噪聲時改變Rww和Rvv的值，累積誤差的變化：x_real

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

25、定連續(xù)的固定量，且有噪聲一定時，根據(jù)測量值來使用kalmanfilter來對系統(tǒng)的真實值做最優(yōu)估計。輸出的最優(yōu)估計如下圖所示：（3）系統(tǒng)真實值給定連續(xù)的隨機(jī)值，且有噪聲時，根據(jù)測量值來使用kalmanfilter來對系統(tǒng)的真實值做最優(yōu)估計。輸出的最優(yōu)估計如下圖所示: 4.1.3算法仿真和結(jié)果分析系統(tǒng)真實值給定連續(xù)的固定量，且有噪聲一定時，根據(jù)測量值來使用kalmanfilter來對系統(tǒng)的真實值做最優(yōu)估計。（1）Rvv=0.32,Rww=0.892（2）Rvv=0.036,Rww=0.74綜上所述：輸入一定，噪聲一定時，改變Rww和Rvv的值，累積誤差的變化：Rvv=0.32Rww=0.892R

26、vv=0.036Rww=0.74累積卡爾曼誤差0.34470.8526累積量測誤差 1.2632 1.8000表四 改變Rvv和Rww4.2多步預(yù)測算法設(shè)計4.2.1算法流程圖及設(shè)計算法流程圖： 系統(tǒng)狀態(tài)和狀態(tài)協(xié)方差的初始化X_now0, P_now0,H,F時間更新階段 更新系統(tǒng)狀態(tài)及其協(xié)方差 量測更新階段 核心算法設(shè)計：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é)果與數(shù)據(jù)分析可以看出，