卡爾曼濾波第四次作業(yè)

1、 卡爾曼濾波與組合導(dǎo)航作業(yè)四一、作業(yè)內(nèi)容本實(shí)驗(yàn)的主要內(nèi)容是完成基于卡爾曼濾波的INS/GPS組合導(dǎo)航實(shí)驗(yàn)。INS的輸出頻率為100Hz,GPS的輸出頻率為20Hz，通過GPS給出的3個(gè)方向的位置和速度作為量測(cè)信息與慣導(dǎo)解算結(jié)果進(jìn)行組合，完成組合導(dǎo)航，比較分析組合導(dǎo)航的結(jié)果。二、系統(tǒng)分析與系統(tǒng)建模捷聯(lián)慣導(dǎo)的解算過程這里不再贅述,直接從組合導(dǎo)航所需的模型開始建模。1.狀態(tài)方程系統(tǒng)的狀態(tài)方程由捷聯(lián)慣導(dǎo)的誤差方程和慣性器件的誤差方程組成：X二FX+GW式中，狀態(tài)變量X=QQ8VSVSV8L8X8HsssVENUENUxyzxvv|t，其中e、和為數(shù)學(xué)平臺(tái)失準(zhǔn)角，8v、Sv和8v分別為東向、yzENUE

2、NU北向和天向速度誤差，8l、8九和8H分別為緯度誤差、經(jīng)度誤差和高度誤差；系統(tǒng)噪聲過程噪聲w=wwwwwwt，包括陀螺和加速度計(jì)的隨機(jī)sssvvvxyzxyz誤差（不包括隨機(jī)常值誤差）；系統(tǒng)噪聲方差陣Q根據(jù)SINS/GPS組合導(dǎo)航系統(tǒng)的慣性器件噪聲水平選取。狀態(tài)轉(zhuǎn)移矩陣F和系統(tǒng)噪聲矩陣G的具體形式為：F矩陣中，F(xiàn)S=Cnb03x303x303x3Cnb03x3一Cn0一FnFsb3x39x99x6，G=0Cn003x3b6x96x615x15009x39x3F二15x6，F(xiàn)N為對(duì)應(yīng)慣導(dǎo)9個(gè)誤差參數(shù)（3個(gè)姿態(tài)誤差，39x6個(gè)速度誤差，3個(gè)位置誤差）的系統(tǒng)動(dòng)態(tài)矩陣，它是（9*9）階方陣。其中非零

3、元素可由慣導(dǎo)誤差模型推導(dǎo)，具體為：1,2iesinL+htgL1,3ie1,5一(rosinie-N2,32,4ie3,1rocosie3,23,4tan3,7rocos-Eiesec24,34,4tan4,52rosinie4,6一(2rocosie4,72rocosLV+ie5,15,35，4一2(ro.iesinRN+htgL)5,55,65,7一(2ro.cossec2ieL)VE6,16,26,42(rocosie2V6,56,7一2VrocosLie7,58,48,7secLtanL9,6secL&-E-EtanL)tgLVVensec2L+2rosinLVie2.量測(cè)方程SINS

4、/GPS組合導(dǎo)航采用松組合的方式,GPS提供位置和速度信息，采用SINS與GPS的位置和速度之差作為卡爾曼濾波的量測(cè)信息。量測(cè)方程：Z(t)=H(t)X(t)+V(t)=HvHpX(t)+(t)v(t)p式中，量測(cè)向量Z(t)-6V6V6V6L饑6hh,分別為捷聯(lián)解算與GPSENU的東向速度、北向速度、天向速度、緯度、經(jīng)度和高度之差；V(t)=v6v6v6v6vv片為量測(cè)噪聲。量測(cè)噪聲方差陣R根據(jù)GPS的位6VE6VN6VU6L6九6h置、速度噪聲水平選??；量測(cè)矩陣H=HH乍，其中：VPH二0diag11110v3x33x93x15H=0diag_RRcosL10。p3x6MN3x63x153

5、.卡爾曼濾波的流程為：狀態(tài)一步預(yù)測(cè)與均方誤差陣一步預(yù)測(cè)：X二Xk/k-1k/k-1kP=OPT+Qk/k-1k/k-1k-1k/k-1濾波增益陣計(jì)算：K=PHt(HPHt+R)-1kk/k-1k/k-1狀態(tài)估計(jì)與均方誤差陣估計(jì)：X=X+K(Z-HX)kk/k-1kkk/k-1P=(I-KH)P(I-KH)t+KRKtkkk/k-1kkk三、運(yùn)行結(jié)果濾波參數(shù)設(shè)置陀螺常值漂移：0.1度/小時(shí)；加速度計(jì)常值偏置：50ug；GPS位置測(cè)量誤差：水平0.1m,垂直0.15m；速度測(cè)量誤差：0.01m/s；初始姿態(tài)誤差：航向30角分，俯仰/橫滾60角秒；初始速度誤差：東向/北向/天向：0.05m/s；初始

6、位置誤差：水平位置誤差2m,垂直位置誤差2m。初始條件IMU的初始姿態(tài)為：航向角為305.34023度(北偏東逆時(shí)針為正)，俯仰角為0.25097度，橫滾角為1.78357度；初始位置與初始速度為GPS在第一個(gè)時(shí)刻的輸出。運(yùn)行結(jié)果緯度對(duì)比 /位單GPSOIo組合534時(shí)間/s高度對(duì)比5001000150020002500300035004000時(shí)間/s經(jīng)度對(duì)比40002000o0GPS組合5001000150020002500300035004000時(shí)間/s東向速度對(duì)比)100GPS組合010005001000150020002500300035004000時(shí)間/s北向速度對(duì)比時(shí)間/s天向速度

7、對(duì)比時(shí)間/s航向角對(duì)比0 /位單1-純慣組合_I_02/位單J.忙點(diǎn)a純慣1n組合.5001000150020002500300035004000時(shí)間/s俯仰角對(duì)比0時(shí)間/s5001000150020002500300035004000時(shí)間/s橫滾角對(duì)比緯度對(duì)比864433/位單慣S合純GP組34205001000150020002500300035004000時(shí)間/s經(jīng)度對(duì)比時(shí)間/s/位單 X純慣F、-.、GPS組合.11000150020002500300035004000時(shí)間/s北向速度對(duì)比東向速度對(duì)比05）s/m（/位單501000150020002500300035004000時(shí)間/

8、s05）s/m（/位單50東向失準(zhǔn)角3-01xIt.k.-.*!/；j山5000/位單ti*3Ji*iLk嚴(yán)bjfu5001000150020002500300035004000時(shí)間/s天向失準(zhǔn)角x10-31000150020002500300035004000時(shí)間/s北向失準(zhǔn)角o01/位單5001000150020002500300035004000時(shí)間/s5050000-/位單505-）s/m（/位單3-01x東向速度誤差4,.Ik*II/*.4丄于山丨Tib-*Fw0501000150020002500300035004000505-）s/m（/位單3-01x時(shí)間/s北向速度誤差一亠-一

9、亠|.1|YV1片畑ff-tA0501000150020002500300035004000時(shí)間/s天向速度誤差50）s/m（/位單0501000150020002500300035004000時(shí)間/s/位單/位單8-01X5o緯度誤差一祁;認(rèn)3*J.rPfrA韓打*Z./1JTl!Pnrfj508-01xo05-0.051000150020002500300035004000時(shí)間/s經(jīng)度誤差一一LAf.旳如心g1V-.-l-.i.怦1.-W斗F5001000150020002500300035004000時(shí)間/s高度誤差_,005001000150020002500300035004000時(shí)

10、間/s/位單/位單東向失準(zhǔn)角均方誤差00005001000150020002500300035004000時(shí)間/s北向失準(zhǔn)角均方誤差o1000501000150020002500300035004000時(shí)間/s天向失準(zhǔn)角均方誤差/位單o500501000150020002500300035004000時(shí)間/s）s/m（/位單3-01X4東向速度均方誤差o3250）s/m（/位單3-01X41000150020002500300035004000時(shí)間/s北向速度均方誤差o3250）s/m（/位單3-01X431000150020002500300035004000時(shí)間/s天向速度均方誤差5010

11、00150020002500300035004000時(shí)間/s01X42緯度均方誤差/位單5001X5/位單1000150020002500300035004000時(shí)間/s經(jīng)度均方誤差0501000150020002500300035004000時(shí)間/s高度均方誤差501000150020002500300035004000時(shí)間/s東向陀螺漂移均方誤差550）h/（/位單1000150020002500300035004000時(shí)間/s北向陀螺漂移均方誤差550）h/（/位單1000150020002500300035004000時(shí)間/s天向陀螺漂移均方誤差10001500200025003000

12、35004000時(shí)間/s550）h/（/位單x10-5東向加計(jì)零偏均方誤差x10-5北向加計(jì)零偏均方誤差時(shí)間/sx10-5天向加計(jì)零偏均方誤差四、實(shí)驗(yàn)分析由于純慣高度通道發(fā)散，因此上述結(jié)果在純慣結(jié)果與GPS結(jié)果或者組合導(dǎo)航結(jié)果比較時(shí)，僅僅給出經(jīng)緯度與東向、北向速度的比對(duì)結(jié)果；而GPS只能給出速度位置信息，因此姿態(tài)信息只有純慣結(jié)果與組合導(dǎo)航結(jié)果比對(duì)。分析以上結(jié)果，可知純慣導(dǎo)航由于存在誤差的積分效果，因此導(dǎo)航結(jié)果存在一定的漂移，經(jīng)過組合導(dǎo)航之后，速度位置信息得到了很好的校正，姿態(tài)信息也得到了一定程度的修正，實(shí)現(xiàn)組合導(dǎo)航的意義。通過失準(zhǔn)角、速度誤差、位置誤差的結(jié)果可見，在系統(tǒng)靜止時(shí)誤差較小，當(dāng)系統(tǒng)處

13、于動(dòng)態(tài)下，導(dǎo)航誤差會(huì)增大，其原因在于此時(shí)系統(tǒng)的非線性效果加強(qiáng)了，因而誤差會(huì)變大。通過均方誤差的結(jié)果可見，在系統(tǒng)靜止時(shí)，某些量（如東、北向陀螺漂移，東北向加計(jì)零偏等）收斂很慢，而一旦系統(tǒng)處于動(dòng)態(tài)下，這些量會(huì)立即收斂，說(shuō)明動(dòng)態(tài)下系統(tǒng)的可觀測(cè)度增大了，濾波的效果提升了。五、源程序%卡爾曼濾波第四次作業(yè)clearallcloseallclc%數(shù)據(jù)文件的處理formatlongpara=pi/180;g0=9.78049;load(IMU.dat);wibb=IMU(:,3:5)*para/3600;fibb=IMU(:,6:8)*g0;load(GPS.dat);lat_GPS=GPS(:,3)*pa

14、ra;lon_GPS=GPS(:,4)*para;h_GPS=GPS(:,5);v_GPS=GPS(:,6:8);%變量的初始化-純慣部分dt=0.01;%IMUperiodT=0.05;%KFperiodre=6378245;e=1/298.3;gk1=0.00193185138639;gk2=0.00669437999013;wie=7.292115147e-5;L=360000;v=zeros(3,L+1);lat=zeros(1,L+1);lon=zeros(1,L+1);h=zeros(1,L+1);psi=zeros(1,L+1);theta=zeros(1,L+1);gamma=

15、zeros(1,L+1);lambda=zeros(1,4);lat(1)=lat_GPS(1);lon(1)=lon_GPS(1);h(1)=h_GPS(1);v(:,1)=v_GPS(:,1);psi(1)=305.34023*para;theta(1)=0.25097*para;gamma(1)=1.78357*para;lambda(1)=cos(psi(1)/2)*cos(theta(1)/2)*cos(gamma(1)/2)-sin(psi(1)/2)*sin(theta(1)/2)*sin(gamma(1)/2);lambda(2)=cos(psi(1)/2)*sin(theta

16、(1)/2)*cos(gamma(1)/2)-sin(psi(1)/2)*cos(theta(1)/2)*sin(gamma(1)/2);lambda(3)=cos(psi(1)/2)*cos(theta(1)/2)*sin(gamma(1)/2)+sin(psi(1)/2)*sin(theta(1)/2)*cos(gamma(1)/2);lambda(4)=cos(psi(1)/2)*sin(theta(1)/2)*sin(gamma(1)/2)+sin(psi(1)/2)*cos(theta(1)/2)*cos(gamma(1)/2);cnb=q2cnb(lambda);cen=-sin(

17、lon(1)cos(lon(1)0;-sin(lat(1)*cos(lon(1)-sin(lat(1)*sin(lon(1)cos(lat(1);cos(lat(1)*cos(lon(1)cos(lat(1)*sin(lon(1)sin(lat(1);gyro=zeros(3,1);acc=zeros(3,1);%變量的初始化-Kalman濾波部分Phi=zeros(15,15);F=zeros(15,15);Fn=zeros(9,9);Fs=zeros(9,6);Q=diag(0.1*pi/180/3600)人2,(0.1*pi/180/3600)人2,(0.1*pi/180/3600)人2

18、,(5e-5*g0)人2,(5e-5*g0)人2,(5e-5*g0)人2,0,0,0,0,0,0,0,0,0);R=diag(0.01人2,0.01人2,0.01人2,0.1人2,0.1人2,0.15人2);H=zeros(6,15);H(1,4)=1;H(2,5)=1;H(3,6)=1;H(6,9)=1;Xk=zeros(15,1);Xkk1=zeros(15,1);Pk=diag(1/60*pi/180)A2,(1/60*pi/180)A2,(0.5*pi/180)A2,0.05A2,0.05A2,0.05A2,2A2,2A2,2A2,.(0.1*pi/180/3600)A2,(0.1*p

19、i/180/3600)A2,(0.1*pi/180/3600)A2,(50*(1e-6)*g0)A2,(50*(1e-6)*g0)A2,(50*(1e-6)*g0)A2);Pkk1=zeros(15,15);Kk=zeros(15,6);Zk=zeros(6,1);mse=zeros(15,72000);%mseisshortformeansquareerrorerr_angle=zeros(3,72000);err_position=zeros(3,72000);err_velocity=zeros(3,72000);%主循環(huán)fori=1:Lg=g0*(1+0.0052884*sin(lat

20、(i)A2-0.0000059*sin(2*lat(i)A2)-0.0003086*h(i);rx=(1+e*sin(lat(i)*sin(lat(i)*re;ry=(1-2*e+3*e*sin(lat(i)*sin(lat(i)*re;wien=cen*00wie;wenn=-v(2,i)/ryv(1,i)/rxv(1,i)/rx*cen(3,3)/cen(2,3);winn=wien+wenn;winb=cnb*winn;wnbb=wibb(:,i)-winb;wnbbdot=wnbb*dt;m1=norm(wnbbdot,2);lambdadot=0wnbbdot(1)wnbbdot(2

21、)wnbbdot(3)-wnbbdot(1)0-wnbbdot(3)wnbbdot(2)-wnbbdot(2)wnbbdot(3)0-wnbbdot(1)-wnbbdot(3);-wnbbdot(2);wnbbdot(1);0;temp=(l-(mlA2)/8+(mlA4)/384)*eye(4)+(0.5-(mlA2)/48)*lambdadot)*lambda;lambda=temp;lambda=lambda/(norm(lambda);cnb=q2cnb(lambda);cbn=cnb;angle=cnb2att(cnb);theta(i+l)=angle(l);%俯仰角更新gamma

22、(i+l)=angle(2);%橫滾角更新psi(i+l)=angle(3);%航向角更新fibn=cbn*(fibb(:,i);vxdot=fibn(l)+(2*wie*sin(lat(i)+v(l,i)*tan(lat(i)/rx)*v(2,i);%?vydot=fibn(2)-(2*wie*sin(lat(i)+v(l,i)*tan(lat(i)/rx)*v(l,i);vzdot=fibn(3)+(2*wie*cos(lat(i)+v(1,i)/rx)*v(1,i)+v(2,i)人2/ry-g;v(l,i+l)=v(l,i)+vxdot*dt;%東向速度更新v(2,i+l)=v(2,i)

23、+vydot*dt;%北向速度更新v(3,i+l)=v(3,i)+vzdot*dt;%天向速度更新wenn=-v(2,i+l)/ryv(l,i+l)/rxv(l,i+l)/rx*cen(3,3)/cen(2,3);wenndot=0-wenn(3)wenn(2);wenn(3)0-wenn(l);-wenn(2)wenn(l)0;dcen=-wenndot*cen;cen=cen+dcen*dt;lat(i+l)=asin(cen(3,3);%緯度更新lon(i+l)=acos(cen(l,2);%經(jīng)度更新h(i+l)=h(i)+v(3,i+l)*dt;%高度更新%開始卡爾曼濾波if(rem(

24、i,5)=0)k=fix(i/5)+l;Fn(1,2)=wie*sin(lat(i)+v(1,i)*tan(lat(i)/(rx+h(i);Fn(1,3)=-(wie*cos(lat(i)+v(1,i)/(rx+h(i);Fn(1,5)=-1/(ry+h(i);Fn(2,1)=-(wie*sin(lat(i)+v(1,i)*tan(lat(i)/(rx+h(i);Fn(2,3)=-v(2,i)/(ry+h(i);Fn(2,4)=1/(rx+h(i);Fn(2,7)=-wie*sin(lat(i);Fn(3,1)=wie*cos(lat(i)+v(1,i)/(rx+h(i);Fn(3,2)=v(

25、2,i)/(ry+h(i);Fn(3,4)=tan(lat(i)/(rx+h(i);Fn(3,7)=wie*cos(lat(i)+v(l,i)/(rx+h(i)*sec(lat(i)A2;Fn(4,2)=-fibn(3);Fn(4,3)=fibn(2);Fn(4,4)=v(2,i)/(ry+h(i)*tan(lat(i)-v(3,i)/(ry+h(i);Fn(4,5)=2*wie*sin(lat(i)+v(l,i)/(rx+h(i)*tan(lat(i);Fn(4,6)=-(2*wie*cos(lat(i)+v(l,i)/(rx+h(i);Fn(4,7)=2*wie*cos(lat(i)*v(

26、2,i)+v(l,i)*v(2,i)/(rx+h(i)*sec(lat(i)A2+2*wie*sin(lat(i)*v(3,i);Fn(5,l)=fibn(3);Fn(5,3)=-fibn(l);Fn(5,4)=-2*(wie*sin(lat(i)+v(l,i)/(rx+h(i)*tan(lat(i);Fn(5,5)=-v(3,i)/(ry+h(i);Fn(5,6)=-v(2,i)/(ry+h(i);Fn(5,7)=-(2*wie*cos(lat(i)+v(l,i)/(rx+h(i)*sec(lat(i)A2)*v(l,i);Fn(6,l)=-fibn(2);Fn(6,2)=fibn(l);F

27、n(6,4)=2*(wie*cos(lat(i)+v(l,i)/(rx+h(i);Fn(6,5)=2*v(2,i)/(ry+h(i);Fn(6,7)=-2*v(l,i)*wie*cos(lat(i);Fn(7,5)=l/(ry+h(i);Fn(8,4)=sec(lat(i)/(rx+h(i);Fn(8,7)=v(l,i)/(rx+h(i)*sec(lat(i)*tan(lat(i);Fn(9,6)=l;cbn=cnb;Fs=cbn,zeros(3,3);zeros(3,3),diag(cbn(l,l),cbn(2,2),cbn(3,3);zeros(3,3),zeros(3,3);F=Fn,F

28、s;zeros(6,9),zeros(6,6);Phi=eye(l5)+F*T+TA2*FA2/2;H(4,7)=ry+h(i);H(5,8)=rx*cos(lat(i)+h(i);Zk(1,1)=v(1,i+1)-v_GPS(1,k);Zk(2,1)=v(2,i+1)-v_GPS(2,k);Zk(3,1)=v(3,i+1)-v_GPS(3,k);Zk(4,1)=lat(i+1)-lat_GPS(k);Zk(5,1)=lon(i+1)-lon_GPS(k);Zk(6,1)=h(i+1)-h_GPS(k);Xk(1:9)=0;Xkk1=Phi*Xk;Pkk1=Phi*Pk*Phi+Q;Kk=Pk

29、k1*H/(H*Pkk1*H+R);Xk=Xkk1+Kk*(Zk-H*Xkk1);Pk=(eye(15)-Kk*H)*Pkk1*(eye(15)-Kk*H)+Kk*R*Kk;%存儲(chǔ)濾波信息err_angle(:,k)=Xk(1:3);err_velocity(:,k)=Xk(4:6);err_position(:,k)=Xk(7:9);mse(1,k)=sqrt(Pk(1,1);mse(4,k)=sqrt(Pk(4,4);mse(7,k)=sqrt(Pk(7,7);mse(2,k)=sqrt(Pk(2,2);mse(5,k)=sqrt(Pk(5,5);mse(8,k)=sqrt(Pk(8,8)

30、;mse(3,k)=sqrt(Pk(3,3);mse(6,k)=sqrt(Pk(6,6);mse(9,k)=sqrt(Pk(9,9);mse(10,k)=sqrt(Pk(10,10);mse(11,k)=sqrt(Pk(11,11);mse(12,k)=sqrt(Pk(12,12);mse(13,k)=sqrt(Pk(13,13);mse(14,k)=sqrt(Pk(14,14);mse(15,k)=sqrt(Pk(15,15);%速度修正v(1,i+1)=v(1,i+1)-Xk(4);v(2,i+1)=v(2,i+1)-Xk(5);v(3,i+1)=v(3,i+1)-Xk(6);%位置修正l

31、at(i+1)=lat(i+1)-Xk(7);lon(i+1)=lon(i+1)-Xk(8);h(i+1)=h(i+1)-Xk(9);%姿態(tài)修正cnc=eye(3)+0Xk(3)-Xk(2);-Xk(3)0Xk(1);Xk(2)-Xk(1)0;cnb=cnb*cnc;%四元數(shù)修正cbn=cnb;lambda=cnb2q(cnb);lambda=lambda;angle=cnb2att(cnb);%俯仰角更新%橫滾角更新%航向角更新%SINS_result為事先存好的純慣導(dǎo)航結(jié)果theta(i+1)=angle(1);gamma(i+1)=angle(2);psi(i+1)=angle(3);e

32、ndend%畫圖%純慣數(shù)據(jù)load(SINS_result.mat);lat_INS1=Slide_avt(lat_INS,100)/para;lon_INS1=Slide_avt(lon_INS,100)/para;vx_INS1=Slide_avt(v_INS(1,:),100);vy_INS1=Slide_avt(v_INS(2,:),100);ps_INS1=Slide_avt(psi_INS,100)/para;th_INS1=Slide_avt(theta_INS,100)/para;ga_INS1=Slide_avt(gamma_INS,100)/para;%組合數(shù)據(jù)lat1=S

33、lide_avt(lat,100)/para;lon1=Slide_avt(lon,100)/para;h1=Slide_avt(h,100);vx1=Slide_avt(v(1,:),100);vy1=Slide_avt(v(2,:),100);vz1=Slide_avt(v(3,:),100);ps1=Slide_avt(psi,100)/para;th1=Slide_avt(theta,100)/para;ga1=Slide_avt(gamma,100)/para;%GPS數(shù)據(jù)lat_GPS1=Slide_avt(lat_GPS,20)/para;lon_GPS1=Slide_avt(l

34、on_GPS,20)/para;h_GPS1=Slide_avt(h_GPS,20);vx_GPS1=Slide_avt(v_GPS(1,:),20);vy_GPS1=Slide_avt(v_GPS(2,:),20);vz_GPS1=Slide_avt(v_GPS(3,:),20);%導(dǎo)航結(jié)果對(duì)比f(wàn)iguresubplot(311)plot(lat_GPSl,g);gridon;xlabel(時(shí)間/s);ylabel(單位/);title(緯度對(duì)比);holdon;plot(lat1,r-);legend(GPS,組合)；subplot(3l2)plot(lon_GPS1,g);gridon;

35、xlabel(時(shí)間/s);ylabel(單位/);title(經(jīng)度對(duì)比);holdon;plot(lon1,r-);legend(GPS，組合)；subplor(313)plor(hGPSL-g-)-gridonEabel(-H4凹、s-)-ylabel(#、m-)Htle(m)-)ifiguresubploall)plor(vxGPSL-g-)-gridonEabel(-H4凹、s-)-ylabel(#(m/s)-)Htle(可X4民-)-holdonuo(VXL_T-)kgend(-GPSn)subploa12)plor(vyGPSL-g-)-gridonEabel(-H4凹、s-)-yl

36、abel(#(m/s)-)Htle(可sxa民-)-hold0nu0(vyL_rL)kgend(-GPS_注吟)subplor(313)plor(vzGPSL-g-)-gridonEabel(4ffl、s-)-ylabel(#(m/s)-)utle(H可X4民-)；holdonuor(VZL-T-)xegend(-GPSn-)ifiguresubploall)plor(psINSlo=-)-gridonEabel(-H4凹、s-)-ylabel(#、。-)H.tle(g民-)八holdonuo(PSL_T-)kgend(M?-注吟)subploa12)plor(thINSlo=-)；gridO

37、nnabel(-H4凹、s-)-ylabel(#、。-)Htle(a民-)八hold0nu0EhL_TMegend(M?-注吟)subplor(313)plor(gaINSlo=-)；gridOnnabel(-H4凹、s-)-ylabel(#、。-)utle(sM民-)八holdonuor(gaL-T-)xegend(Ms?-n-)ifiguresubplosll)ploaarINSlo=-)-gridonEabel(4ffl、s-)-ylabel(#、。-)Htle()-)；subplor(212)ploaonINSL-b-)；gridOnnabel(-H4凹、s-)-ylabel(#、。-

38、)Htle(-)-)；figuresubplosll)plor(vxINSlo=-)-gridonEabel(4ffl、s-)-ylabel(#(m/s)-)Htle(可X4民-)-holdonuor(vxGPSL-g-)uor(VXL_T-=egend(Ms?-GPSn)subplor(212)plor(vyINSlo=-)-gridonEabel(4ffl、s-)-ylabel(#(m/s)-)Htle(可sxa民-)-holdonuor(vyGPSL-g-)uor(vyL_TMegend(Ms?-GPSn)17 %誤差估計(jì)figuresubplot(311)plot(Slide_avt(

39、err_angle(l,:),20)/para);gridon;xlabel(時(shí)間/s);ylabel(單位/);title(東向失準(zhǔn)角);subplot(3l2)plot(Slide_avt(err_angle(2,:),20)/para);gridon;xlabel(時(shí)間/s);ylabel(單位/);title(北向失準(zhǔn)角);subplot(3l3)plot(Slide_avt(err_angle(3,:),20)/para);gridon;xlabel(時(shí)間/s);ylabel(單位/);title(天向失準(zhǔn)角);figuresubplot(3ll)plot(Slide_avt(err

40、_velocity(1,:),20);gridon;xlabel(時(shí)間/s);ylabel(單位/(m/s);title(東向速度誤差);subplot(312)plot(Slide_avt(err_velocity(2,:),20);gridon;xlabel(時(shí)間/s);ylabel(單位/(m/s);title(北向速度誤差);subplot(313)plot(Slide_avt(err_velocity(3,:),20);gridon;xlabel(時(shí)間/s);ylabel(單位/(m/s);title(天向速度誤差);figuresubplot(311)plot(Slide_avt(

41、err_position(1,:),20)/para);gridon;xlabel(時(shí)間/s);ylabel(單位/);title(緯度誤差);subplot(312)plot(Slide_avt(err_position(2,:),20)/para);gridon;xlabel(時(shí)間/s);ylabel(單位/);title(經(jīng)度誤差);subplot(313)plot(Slide_avt(err_position(3,:),20);gridon;xlabel(時(shí)間/s);ylabel(單位/m);title(高度誤差);%均方誤差估計(jì)figuresubplot(311)plot(Slide

42、_avt(mse(1,:),20)/para);gridon;xlabel(時(shí)間/s);ylabel(單位/);title(東向失準(zhǔn)角均方誤差);subplot(312)plot(Slide_avt(mse(2,:),20)/para);gridon;xlabel(時(shí)間/s);ylabel(單位/);title(北向失準(zhǔn)角均方誤差);subplot(313)plot(Slide_avt(mse(3,:),20)/para);gridon;xlabel(時(shí)間/s);ylabel(單位/);title(天向失準(zhǔn)角均方誤差);figuresubplot(311)plot(Slide_avt(mse(

43、4,:),20);gridon;xlabel(時(shí)間/s);ylabel(單位/(m/s);title(東向速度均方誤差);subplot(312)plot(Slide_avt(mse(5,:),20);gridon;xlabel(時(shí)間/s);ylabel(單位/(m/s);title(北向速度均方誤差);subplot(313)plot(Slide_avt(mse(6,:),20);gridon;xlabel(時(shí)間/s);ylabel(單位/(m/s);title(天向速度均方誤差);figuresubplot(311)plot(Slide_avt(mse(7,:),20)/para);gri

44、don;xlabel(時(shí)間/s);ylabel(單位/);title(緯度均方誤差);subplot(312)plot(Slide_avt(mse(8,:),20)/para);gridon;xlabel(時(shí)間/s);ylabel(單位/);title(經(jīng)度均方誤差);subplot(313)plot(Slide_avt(mse(9,:),20);gridon;xlabel(時(shí)間/s);ylabel(單位/m);title(高度均方誤差);figuresubplot(311)plot(Slide_avt(mse(10,:),20)/para*3600);gridon;xlabel(時(shí)間/s);

45、ylabel(單位/(/h);title(東向陀螺漂移均方誤差);subplot(312)plot(Slide_avt(mse(11,:),20)/para*3600);gridon;xlabel(時(shí)間/s);ylabel(單位/(/h);title(北向陀螺漂移均方誤差);subplot(313)plot(Slide_avt(mse(12,:),20)/para*3600);gridon;xlabel(時(shí)間/s);ylabel(單位/(/h);title(天向陀螺漂移均方誤差);figuresubplot(311)plot(Slide_avt(mse(13,:),20)/g0);gridon;xlabe