北航卡爾曼濾波實驗報告-GPS靜動態(tài)濾波實驗

1、卡爾曼濾波實驗報告2014年4月GPS靜/動態(tài)濾波實驗一、實驗要求GPS數(shù)據(jù)進行1、分別建立 GPS靜態(tài)及動態(tài)卡爾曼濾波模型，編寫程序?qū)o態(tài)和動態(tài)Kalman 濾波。2、對比濾波前后導(dǎo)航軌跡圖。3、畫出濾波過程中估計均方差(P陣對角線元素開根號)的變化趨勢。4、思考：簡述動態(tài)模型與靜態(tài)模型的區(qū)別與聯(lián)系； R陣、Q陣，P0陣的選取對濾 波精度及收斂速度有何影響，取值時應(yīng)注意什么；本濾波問題是否可以用最小二乘方法解決，如果可以，請闡述最小二乘方法與Kalman濾波方法的優(yōu)劣對比。二、實驗原理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)模型是十分準確的，所以系統(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動態(tài)濾波動態(tài)濾波基于當前統(tǒng)計模型，在地球坐標系下解算。選取系統(tǒng)的狀態(tài)變量為qTX=|_x a yy q 8y zy 爐 L,其中x,Vx,ax,%依次為地球坐標系下 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)為機動加速度的當前均值，其自適應(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)量測量為緯度動態(tài)量測值、經(jīng)度動態(tài)量測值、高度和三向速度量測值。由于濾波在地球坐標系下進行，為了簡便首先將緯度、經(jīng)度和高度轉(zhuǎn)化為三軸位置坐標值，轉(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)所以，濾波的量測量為三軸位置坐標值和三軸速度測量值，即 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é)方差陣為:機動加速度自適

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

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

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

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

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