北航卡爾曼濾波實(shí)驗(yàn)報(bào)告-GPS靜動(dòng)態(tài)濾波實(shí)驗(yàn)

1、三爾曼濾波實(shí)驗(yàn)報(bào)告2014年4月GPS靜/動(dòng)態(tài)濾波實(shí)驗(yàn)一、實(shí)驗(yàn)要求1、 分別建立 GPS靜態(tài)及動(dòng)態(tài)卡爾曼濾波模型，編寫程序?qū)o態(tài)和動(dòng)態(tài)GPS數(shù)據(jù)進(jìn)行Kalman 濾波。2、對(duì)比濾波前后導(dǎo)航軌跡圖。3、畫出濾波過程中估計(jì)均方差 (P陣對(duì)角線元素開根號(hào))的變化趨勢(shì)。4、思考： 簡(jiǎn)述動(dòng)態(tài)模型與靜態(tài)模型的區(qū)別與聯(lián)系； R陣、Q陣，P0陣的選取對(duì)濾波精度及收斂速度有何影響，取值時(shí)應(yīng)注意什么；本濾波問題是否可以用最小二乘方法解決，如果可以，請(qǐng)闡述最小二乘方法與Kalman濾波方法的優(yōu)劣對(duì)比。二、實(shí)驗(yàn)原理2.1 GPS靜態(tài)濾波選取系統(tǒng)的狀態(tài)變量為X L hT ,其中L為緯度(deg), 為經(jīng)度(deg)，h

2、為高度(m)。設(shè)w(t)為零均值高斯白噪聲，則系統(tǒng)的狀態(tài)方程為:031w(t)所以離散化的狀態(tài)模型為:Xkk,k 1Xk 1Wk 1式中，k,k 1為3 3單位陣，Wk為系統(tǒng)噪聲序列。測(cè)量數(shù)據(jù)包括：緯度靜態(tài)量測(cè)值、經(jīng)度靜態(tài)量測(cè)值和高度構(gòu)成 可以表示為：3 1矩陣Z，量測(cè)方程Zk HXk Vk式中，H為3 3單位陣，V為量測(cè)噪聲序列。系統(tǒng)的狀態(tài)模型是十分準(zhǔn)確的，所以系統(tǒng)模型噪聲方差陣可以取得十分小，取Q陣零矩陣。系統(tǒng)測(cè)量噪聲方差陣 R由測(cè)量確定，由于位置量測(cè)精度為 5m,采用克拉索夫斯基地球橢球模型，長(zhǎng)半徑 R.為6378245m，短半徑Rp為6356863m。所以R陣為:5 18O、2(R(5

3、 180)2(Re COS(L)0522.2 GPS動(dòng)態(tài)濾波動(dòng)態(tài)濾波基于當(dāng)前統(tǒng)計(jì)模型，X x Vx ax x y Vy ay y z Vz az在地球坐標(biāo)系下解算。選取系統(tǒng)的狀態(tài)變量為T其中Z5 丿、Ix, vx, ax, x依次為地球坐標(biāo)系下x軸上的位置、速度、加速度和位置誤差分量, y,z軸同理。系統(tǒng)的狀態(tài)模型可以表示為:戲(t) AX (t) U(t) W(t)式中，位置誤差視為有色噪聲，為一階馬爾科夫過程，可表示為:Wx&yWy&zWz其中，i(i x, y, z)為對(duì)應(yīng)馬爾科夫過程的相關(guān)時(shí)間常數(shù),Wi(ix,y,z)為零均值高斯白噪聲。系統(tǒng)矩陣A可表示為:Ax04 4

4、04 404 4Ay04 404 404 4Az0其中，Ai 01/(i=x, y,z)ai1/輸入量U可表示為:(10)Z = HX +V(11)U(t)0 0邑axay0 00 0aya 0az(8)式中，ai(i x,y,z)為機(jī)動(dòng)加速度的當(dāng)前均值，其自適應(yīng)確定方法為:ax,k?/k 1 ，同理可得Oyfaz,k。系統(tǒng)噪聲為:W (t)0 0 Wax Wx 0 0 Wa Wy 0 0 Waz Wz量測(cè)量為緯度動(dòng)態(tài)量測(cè)值、經(jīng)度動(dòng)態(tài)量測(cè)值、高度和三向速度量測(cè)值。(9)由于濾波在地球坐標(biāo)系下進(jìn)行，為了簡(jiǎn)便首先將緯度、經(jīng)度和高度轉(zhuǎn)化為三軸位置坐標(biāo)值，轉(zhuǎn)化方式如下：x (Re h)cos(L)co

5、s( y (Re h)cos( L)sin( z (Re h)sin( L)所以，濾波的量測(cè)量為三軸位置坐標(biāo)值和三軸速度測(cè)量值,Z xyzVxVyVzT。量測(cè)方程為:0(13)式中，H0 ，V為零均值高斯白噪聲。綜上，離散化的Kalma n濾波方程為:X k 1/k k 1/k X kXk 1X k 1/k Kk 1(Zk 1 Hk1X k 1/k )Kk 1Pk 1/k H k 1 (H k 1Pk 1/kHRk1)1(12)Pk 1/k k 1/k Pk k 1/k Q kPk 1(I Kk1H k 1 )Pk 1/k式中，k1/kX k 1/k0T2/20z1/k， k 1/k0 (iT

6、/ ix, y, z), k 1/k木X k 1/k001/k0aiT/ a2ai0 k 1/k1/k離散化的系統(tǒng)噪聲協(xié)方差陣為:機(jī)動(dòng)加速度自適應(yīng)確定方法為:2ax2ax4n aLmaxn4 na maxn纂 1/kXk2離散化量測(cè)噪聲協(xié)方差陣為:diag(1三、實(shí)驗(yàn)結(jié)果3.1 GPS靜態(tài)濾波e")T/ aai(ix,y,z)2ay0eT/i2az4n aL Umaxn?當(dāng)前”加速度x&2“當(dāng)前”加速度0diag R2 Ry R； R： R： r2緯度經(jīng)度時(shí)間/S度經(jīng) 116.3468140116.3468116.3468116.3468116.3468116.3468116

7、.3468116.3468116.3468116.3468時(shí)間/S200估計(jì)咼度測(cè)量高度1高度12010080604050010002000250030001500時(shí)間/Sx 105-1均方誤差1一，相對(duì)誤差宀-5432105001000200025001500時(shí)間/S03000時(shí)間/S-5x 1020-100均方誤差1相對(duì)誤差嚴(yán)4r0-20-40-60-8005001000200025001500時(shí)間/S30003.2 GPS動(dòng)態(tài)濾波圖1 GPS靜態(tài)濾波前后導(dǎo)航軌跡圖和估計(jì)誤差40.12度緯40.140.0840.0640.0440.02400緯度經(jīng)度F度高W高度1ftni111O5估計(jì)高度

8、' I測(cè)量高度W599O589uro七 iSliiiinrrTW11 ir100200400500600300時(shí)間/S天向速度*$度速北向速度時(shí)間/sO70.10.080.060.040.020-0.02-0.04-0.06-0.08-0.1II;估計(jì)速度測(cè)量速度-V EI V- : I；W ML' l f!'ir"ii;II bLti liJn/HI. Im H門 皿山 lUlilMim欄 MMT*0100200300時(shí)間/S400500600差誤計(jì)估度緯O O相對(duì)誤差1-.I 拆舌 扌早圭 1.100200400500-7108 7 6 53 212300

9、時(shí)間/S600-20七-h_-_.l1-«相對(duì)誤差1 均方誤差-7x 101086420100200400500300時(shí)間/S600-0.01相對(duì)誤差10 04 HiiHI均方誤差0 03r0 020.01.d II,L1J 1,±L_JilLLiJi.J,0-0.02-0.01-0.02-0.03 I11110100200300400500600時(shí)間/S叩叩ITIF 附EINNWpqE 血d価血擁M山川h也仙皿-0.03 L0100200300400500600時(shí)間/S圖2 GPS動(dòng)態(tài)濾波前后導(dǎo)航軌跡圖和估計(jì)誤差四、實(shí)驗(yàn)討論模型誤差較小，量測(cè)信采用當(dāng)前統(tǒng)計(jì)模型建模， 相比

10、 靜態(tài)模型是動(dòng)態(tài)模型在速度和加1 .簡(jiǎn)述動(dòng)態(tài)模型與靜態(tài)模型的區(qū)別與聯(lián)系。靜態(tài)模型的速度和加速度均為0,系統(tǒng)靜止，狀態(tài)模型比較準(zhǔn)確,息只有位置信息。動(dòng)態(tài)模型系統(tǒng)的速度和加速度均發(fā)生變化， 之下，系統(tǒng)模型的誤差較大， 量測(cè)信息由位置和速度信息。速度均為0時(shí)的特殊情況。2. R陣、Q陣，P0陣的選取對(duì)濾波精度及收斂速度有何影響，取值時(shí)應(yīng)注意什么。R陣的取值對(duì)濾波精度的影響很大，當(dāng)R取得太大，系統(tǒng)就不能有效的利用量測(cè)信息對(duì)狀態(tài)進(jìn)行修正，因此濾波精度較低；相反，R取得太小，系統(tǒng)過分依賴量測(cè)信息，無法利用狀態(tài)模型有效的去除有害的量測(cè)噪聲，同樣降低濾波的精度。Q陣的取值對(duì)濾波精度的影響也很大：Q取得太大，系

11、統(tǒng)就不能有效的利用狀態(tài)模型對(duì)測(cè)量噪聲進(jìn)行修正，因此濾波精度就較低；反之，Q取得太小，系統(tǒng)就會(huì)過分的依賴狀態(tài)模型的精度，以致量測(cè)信息無法對(duì) 狀態(tài)進(jìn)行有效的修正，也會(huì)降低濾波精度；只有當(dāng)R和Q的取值恰好與使用的狀態(tài)模型的精度相吻合時(shí)，才能使?fàn)顟B(tài)模型和量測(cè)信息都能有效的發(fā)揮作用，互相補(bǔ)充，得到最高的濾波精度。P0陣的取值對(duì)于可觀測(cè)性良好的系統(tǒng)，只影響開始的濾波精度，對(duì)收斂精度影響 不大，但影響收斂速度。3）本濾波問題是否可以用最小二乘方法解決，如果可以，請(qǐng)闡述最小二乘方法與 Kalman濾波方法的優(yōu)劣對(duì)比。本濾波問題可以用最小二乘方法解決。最小二乘方法的最大優(yōu)點(diǎn)是算法簡(jiǎn)單，特別是對(duì)一般的最小二乘估計(jì)， 根本不必知道量測(cè)誤差的統(tǒng)計(jì)信息。但又存在使用上的局限性，該方法只能估計(jì)確定性的常值向量，而無法估計(jì)隨機(jī)向量的時(shí)間過程