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

1、卡爾曼濾波實驗報告2014年4月GPS靜/動向濾波實驗一、實驗要求1、分別成立GPS靜態(tài)及動向卡爾曼濾波模型，編寫程序對靜態(tài)和動向GPS數據進行Kalman濾波。2、對照濾波前后導航軌跡圖。3、畫出濾波過程中預計均方差(P陣對角線元素開根號)的變化趨向。4、思慮：簡述動向模型與靜態(tài)模型的差別與聯系；R陣、Q陣，P0陣的選用對濾波精度及收斂速度有何影響，取值時應注意什么；本濾波問題能否能夠用最小二乘方法解決，假如能夠，請論述最小二乘方法與Kalman濾波方法的好壞對照。二、實驗原理2.1GPS靜態(tài)濾波選用系統的狀態(tài)變量為XLhT,此中L為緯度(deg),為經度(deg)，h為高度。設w(t)為零

2、均值高斯白噪聲，則系統的狀態(tài)方程為：&031w(t)(1)X所以失散化的狀態(tài)模型為：Xkk,k1Xk1Wk1(2)式中，k,k1為33單位陣，Wk為系統噪聲序列。丈量數據包含：緯度靜態(tài)量測值、經度靜態(tài)量測值和高度組成31矩陣Z，量測方程能夠表示為：ZkHXkVk(3)式中，H為33單位陣，Vk為量測噪聲序列。系統的狀態(tài)模型是十分正確的，所以系統模型噪聲方差陣能夠獲得十分小，取Q陣零矩陣。系統丈量噪聲方差陣R由丈量確立，因為地點量測精度為5m，采納克拉索夫斯基地球橢球模型，長半徑Re為6378245m，短半徑Rp為6356863m。所以R陣為：(5180)200RpR0(5180)20(4)Re

3、cos(L)00522.2GPS動向濾波動向濾波鑒于目前統計模型，在地球坐標系下解算。選用系統的狀態(tài)變量為XxvxaxxyvyayyzvzazTx挨次為地球坐標系下xz,此中x,vx,ax,軸上的地點、速度、加快度和地點偏差重量，y,z軸同理。系統的狀態(tài)模型能夠表示為：X&(t)AX(t)U(t)W(t)(5)式中，地點偏差視為有色噪聲，為一階馬爾科夫過程，可表示為：&1wxxxx&y1wy(6)yy&1wzzzz此中，i(ix,y,z)為對應馬爾科夫過程的有關時間常數，wi(ix,y,z)為零均值高斯白噪聲。系統矩陣A可表示為：Ax044044A044Ay044(7)044044Az0100

4、此中，Ai0010001/ai(i=x,y,z)00001/i輸入量U可表示為：Taxay000az0(8)U(t)00000axayazaiixyzax,k?式中，(,)為靈活加快度的目前均值，其自適應確立方法為：&同理可得ay,k、az,k。系統噪聲為：W(t)00waxwx00waywy00wazwzT(9)量丈量為緯度動向量測值、經度動向量測值、高度和三向速胸懷測值。因為濾波在地球坐標系下進行，為了簡易第一將緯度、經度和高度轉變?yōu)槿S地點坐標值，轉變方式以下：x(Reh)cos(L)cos()y(Reh)cos(L)sin()(10)z(Reh)sin(L)所以，濾波的量丈量為三軸地點

5、坐標值和三軸速度丈量值，即ZxyzvxvyvzT。量測方程為：Z=HX+V(11)100100000000000010010000式中，H000000001001，V為零均值高斯白噪聲。010000000000000001000000000000000100綜上，失散化的Kalman濾波方程為：Xk1/kk1/kXkXk1Xk1/kKk1(Zk1Hk1Xk1/k)Kk1Pk1/kHkT1(Hk1Pk1/kHTk1Rk1)1(12)Pk1/kk1/kPkTk1/kQkPk1(IKk1Hk1)Pk1/kx001TT2/20k1/k01T0式中，k1/k0ky1/k0，ki1/k(ix,y,z)，0

6、0kz1/k0010000eT/i1T(T/a1eT/ai)a2kx00ii1/k01(1eT/ai)a0y0ik1/kk1/k，k1/ki00(ix,y,z)00kz1/k00eT/a0i000eT/i離散化的系統噪聲協方差陣為：Qdiag002200220022，靈活加快度自適應確立方法為：axxayyazz24?24?2“目前”加快度axamax&xk1/kamax&xk04?(13)22“目前”加快度axamax&0 xk?失散化量測噪聲協方差陣為：RdiagRx2Ry2Rz2Rv2xRv2yRv2z。三、實驗結果3.1GPS靜態(tài)濾波緯度116.3468經度39.9774預計緯度預計經

7、度39.9774丈量緯度116.3468丈量經度39.9774116.346839.9774116.3468g39.9774g116.3468eedd度39.9774度116.3468緯經39.9774116.346839.9774116.346839.9774116.346839.9774116.3468050010001500200025003000050010001500200025003000時間/s時間/s高度x10-51405均方偏差相對偏差120預計高度4丈量高度100g3ed/m差/80誤2度計高估度60緯14002050010001500200025003000-150010

8、00150020002500300000時間/s時間/sx10-5205均方偏差均方偏差相對偏差相對偏差40g3m-20e/差差誤誤2-40計計估估度度高經1-600-80-150010001500200025003000-1005001000150020002500300000時間/s時間/s圖1GPS靜態(tài)濾波前后導航軌跡圖和預計偏差3.2GPS動向濾波緯度經度40.12116.06預計緯度預計經度丈量緯度116.05丈量經度40.1116.0440.08gg116.03eedd40.06度度緯經116.0240.04116.0140.0211640115.99010020030040050

9、06000100200300400500600時間/s時間/s高度東向速度102016預計高度預計速度1015丈量高度14丈量速度12101010)m1005s8/m度/度高10006速499529900985100200300400500600-210020030040050060000時間/s時間/s北向速度天向速度210.1預計速度預計速度20丈量速度0.08丈量速度0.06190.04)0.02s18smm(0/(度17度速速-0.0216-0.04-0.0615-0.0814100200300400500600-0.110020030040050060000時間/s時間/sx10-7

10、x10-7812相對偏差相對偏差7均方偏差10均方偏差685差差6誤4誤計計估3估4度度緯經22100-1100200300400500600-210020030040050060000時間/s時間/s200.05相對偏差相對偏差15均方偏差0.04均方偏差100.035差0.02差誤誤計0.01計0估估度0度速高-5向東-0.01-10-0.02-15-0.03-20-0.0401002003004005006000100200300400500600時間/s時間/s0.050.05相對偏差相對偏差0.04均方偏差0.04均方偏差0.030.03差0.02差0.02誤誤計計估0.01估0.0

11、1度度速速向0向0北天-0.01-0.01-0.02-0.02-0.03100200300400500600-0.0310020030040050060000時間/s時間/s圖2GPS動向濾波前后導航軌跡圖和預計偏差四、實驗議論1簡述動向模型與靜態(tài)模型的差別與聯系。靜態(tài)模型的速度和加快度均為0，系統靜止，狀態(tài)模型比較正確，模型偏差較小，量測信息只有地點信息。動向模型系統的速度和加快度均發(fā)生變化，采納目前統計模型建模，對比之下，系統模型的偏差較大，量測信息由地點和速度信息。靜態(tài)模型是動向模型在速度和加速度均為0時的特別狀況。2R陣、Q陣，P0陣的選用對濾波精度及收斂速度有何影響，取值時應注意什么

12、。R陣的取值對濾波精度的影響很大，當R獲得太大，系統就不可以有效的利用量測信息對狀態(tài)進行修正，所以濾波精度較低；相反，R獲得太小，系統過分依靠量測信息，沒法利用狀態(tài)模型有效的去除有害的量測噪聲，相同降低濾波的精度。Q陣的取值對濾波精度的影響也很大：Q獲得太大，系統就不可以有效的利用狀態(tài)模型對丈量噪聲進行修正，所以濾波精度就較低；反之，Q獲得太小，系統就會過分的依靠狀態(tài)模型的精度，致使量測信息沒法對狀態(tài)進行有效的修正，也會降低濾波精度；只有當R和Q的取值恰巧與使用的狀態(tài)模型的精度相符合時，才能使狀態(tài)模型和量測信息都能有效的發(fā)揮作用，相互增補，獲得最高的濾P0不大，但影響收斂速度。3）本濾波問題能否能夠用最小二乘方法解決，假如能夠，請論述最小二乘方法與Kalman濾波方法的好壞對照。本濾波問題能夠用最小二乘方法解決。最小二乘方法的最大長處是算法簡單，特別是對一般的最小二乘預計，根本不用知道量測偏差的統計信息。但又存在使用上的限制性，該方法只好預計確立性