北航卡爾曼濾波實(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)要求GPS數(shù)據(jù)進(jìn)行1、分別建立 GPS靜態(tài)及動(dòng)態(tài)卡爾曼濾波模型，編寫程序?qū)o態(tài)和動(dòng)態(tài)Kalman 濾波。2、對(duì)比濾波前后導(dǎo)航軌跡圖。3、畫出濾波過程中估計(jì)均方差(P陣對(duì)角線元素開根號(hào))的變化趨勢。4、思考：簡述動(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 h hT ,其中L為緯度(deg),九為經(jīng)度(deg),

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

3、陣為：(5 180)2(Re cos(L)二0；5M180)2(R X H )(4)R =05202.2 GPS動(dòng)態(tài)濾波動(dòng)態(tài)濾波基于當(dāng)前統(tǒng)計(jì)模型，在地球坐標(biāo)系下解算。選取系統(tǒng)的狀態(tài)變量為qTX=|_x a yy q 8y zy 爐 L,其中x,Vx,ax,%依次為地球坐標(biāo)系下 x 軸上的位置、速度、加速度和位置誤差分量，y, Z軸同理。系統(tǒng)的狀態(tài)模型可以表示為：X (t)= AX (t) U (t) W (t)式中，位置誤差視為有色噪聲，為一階馬爾科夫過程，可表示為:1名 x =一名x + Wxx14.。4酒輸入量U可表不為:U (t) =00 曳 000 互 000 2%xZy.(8)式中，

4、通。=x, y, z)為機(jī)動(dòng)加速度的當(dāng)前均值，其自適應(yīng)確定方法為: TOC o 1-5 h z 同理可得ayk、azk。 y ,z,系統(tǒng)噪聲為： HYPERLINK l bookmark38 o Current Document W (t) = 0 0WaWx0 0WaWy0 0WaWzI(9)量測量為緯度動(dòng)態(tài)量測值、經(jīng)度動(dòng)態(tài)量測值、高度和三向速度量測值。由于濾波在地球坐標(biāo)系下進(jìn)行，為了簡便首先將緯度、經(jīng)度和高度轉(zhuǎn)化為三軸位置坐標(biāo)值，轉(zhuǎn)化方式如下：x = (Re h) cos(L)cos( 1) HYPERLINK l bookmark40 o Current Document y = (Re

5、 h) cos(L)sin( 1)(10)z = (Re h)sin( L)所以，濾波的量測量為三軸位置坐標(biāo)值和三軸速度測量值，即 Z = x y z vx vy vzT o量測方程為:(11)Z = HX +V1 001 00000000000010010000式中，H = 0 0 0 0 0 0 0 0 0 0 0 0V為零均值高斯白噪聲。0 0 0 0 0 1 0 0 0 0 0 00 000000001 0 0綜上，離散化的 Kalman濾波方程為：. AX k +/k =蟲；書* X kA AA(12)Xk+ =Xk+/k + K(Zk41 -H k + Xk書/k) TT1K k

6、1 = P k 1/k H k 1 (H k 1Pk 1/kH k 1 R k 1)pk +/k =卜書爾Pk 0 k1/k Q Q kPk1 =(I -Kk1H k 1 )Pk 1/k式中,0 111 T T2/20 i0 1 T00，k書/k =(i =x, y,z)一.Z0 010k書/k -I0 00 e_一乙/k0&/k = |0 klk00一1T)20一 自/k001工-工y- k +/k =0 k+/k0，czz z00k書/k _ k 1/k0：0(T/ ai -1 e *(1 - e/ai) aieq ai e0e”(i=x,y,z)離散化的系統(tǒng)噪聲協(xié)方差陣為:機(jī)動(dòng)加速度自適

7、應(yīng)確定方法為：q =diag 0 0 每：尺 0 0 每ay 尺 0 0 每az 每；,卜：=4 amax-Xk 也k2= j amax-Xk2“當(dāng)前”加速度 0(13) TOC o 1-5 h z JTt九4tx+?當(dāng)前”加速度0axliaxi jL九離散化量測噪聲協(xié)方差陣為：R =diagm2 Ry2 R； Rv2x R： RZ】。三、實(shí)驗(yàn)結(jié)果3.1 GPS靜態(tài)濾波緯度時(shí)間/s116.3468116.3468116.3468116.3468e 116.3468度經(jīng) 116.3468116.3468116.3468116.3468經(jīng)度時(shí)間/s116.3468/度高-5X 10時(shí)間/sgea/

8、差誤計(jì)估度緯差誤計(jì)估度經(jīng)時(shí)間/s次差誤計(jì)估度高圖1 GPS靜態(tài)濾波前后導(dǎo)航軌跡圖和估計(jì)誤差3.2 GPS動(dòng)態(tài)濾波緯度時(shí)間/s116.06116.05116.04g 116.03 度經(jīng) 116.02116.01116115.99 0經(jīng)度100200300400500時(shí)間/s60010201015101010051000995990985高度時(shí)間/s東向速度估計(jì)速度測量速度北向速度時(shí)間/s天向速度0.10.080.060.040.020-0.02-0.04-0.06-0.08-0.10100200300400500600時(shí)間/s-7X 1087 6 5 X 1012相對(duì)誤差一 . L .均方誤差4

9、 3差誤計(jì)估度緯210-1100200300400500600時(shí)間/s差誤計(jì)估度經(jīng)-201 一一1.-一相對(duì)誤差,.I均方誤差-7108642100200400500300時(shí)間/s6005 4 o O o O差 誤 對(duì) 目O65 Q3 Q差差 誤誤 對(duì)方 相均差誤計(jì)估度速向東050403-0.02 1一-0.03 0100200300400500O 6 6 差誤計(jì)估度速向天時(shí)間/s差誤計(jì)估度高ot)6 Q 4 Q o0 5 02 11-0.02-0.03 0100200300400時(shí)間/s500600圖2 GPS動(dòng)態(tài)濾波前后導(dǎo)航軌跡圖和估計(jì)誤差四、實(shí)驗(yàn)討論1 .簡述動(dòng)態(tài)模型與靜態(tài)模型的區(qū)別與聯(lián)

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

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