卡尔曼滤波第四次作业

1、 卡尔曼滤波与组合导航作业四一、作业内容本实验的主要内容是完成基于卡尔曼滤波的INS/GPS组合导航实验。INS的输出频率为100Hz,GPS的输出频率为20Hz，通过GPS给出的3个方向的位置和速度作为量测信息与惯导解算结果进行组合，完成组合导航，比较分析组合导航的结果。二、系统分析与系统建模捷联惯导的解算过程这里不再赘述,直接从组合导航所需的模型开始建模。1.状态方程系统的状态方程由捷联惯导的误差方程和惯性器件的误差方程组成：X二FX+GW式中，状态变量X=QQ8VSVSV8L8X8HsssVENUENUxyzxvv|t，其中e、和为数学平台失准角，8v、Sv和8v分别为东向、yzENUE

2、NU北向和天向速度误差，8l、8九和8H分别为纬度误差、经度误差和高度误差；系统噪声过程噪声w=wwwwwwt，包括陀螺和加速度计的随机sssvvvxyzxyz误差（不包括随机常值误差）；系统噪声方差阵Q根据SINS/GPS组合导航系统的惯性器件噪声水平选取。状态转移矩阵F和系统噪声矩阵G的具体形式为：F矩阵中，FS=Cnb03x303x303x3Cnb03x3一Cn0一FnFsb3x39x99x6，G=0Cn003x3b6x96x615x15009x39x3F二15x6，FN为对应惯导9个误差参数（3个姿态误差，39x6个速度误差，3个位置误差）的系统动态矩阵，它是（9*9）阶方阵。其中非零

3、元素可由惯导误差模型推导，具体为：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.量测方程SINS

4、/GPS组合导航采用松组合的方式,GPS提供位置和速度信息，采用SINS与GPS的位置和速度之差作为卡尔曼滤波的量测信息。量测方程：Z(t)=H(t)X(t)+V(t)=HvHpX(t)+(t)v(t)p式中，量测向量Z(t)-6V6V6V6L饥6hh,分别为捷联解算与GPSENU的东向速度、北向速度、天向速度、纬度、经度和高度之差；V(t)=v6v6v6v6vv片为量测噪声。量测噪声方差阵R根据GPS的位6VE6VN6VU6L6九6h置、速度噪声水平选取；量测矩阵H=HH乍，其中：VPH二0diag11110v3x33x93x15H=0diag_RRcosL10。p3x6MN3x63x153

5、.卡尔曼滤波的流程为：状态一步预测与均方误差阵一步预测：X二Xk/k-1k/k-1kP=OPT+Qk/k-1k/k-1k-1k/k-1滤波增益阵计算：K=PHt(HPHt+R)-1kk/k-1k/k-1状态估计与均方误差阵估计：X=X+K(Z-HX)kk/k-1kkk/k-1P=(I-KH)P(I-KH)t+KRKtkkk/k-1kkk三、运行结果滤波参数设置陀螺常值漂移：0.1度/小时；加速度计常值偏置：50ug；GPS位置测量误差：水平0.1m,垂直0.15m；速度测量误差：0.01m/s；初始姿态误差：航向30角分，俯仰/横滚60角秒；初始速度误差：东向/北向/天向：0.05m/s；初始

6、位置误差：水平位置误差2m,垂直位置误差2m。初始条件IMU的初始姿态为：航向角为305.34023度(北偏东逆时针为正)，俯仰角为0.25097度，横滚角为1.78357度；初始位置与初始速度为GPS在第一个时刻的输出。运行结果纬度对比 /位单GPSOIo组合534时间/s高度对比5001000150020002500300035004000时间/s经度对比40002000o0GPS组合5001000150020002500300035004000时间/s东向速度对比)100GPS组合010005001000150020002500300035004000时间/s北向速度对比时间/s天向速度

7、对比时间/s航向角对比0 /位单1-纯惯组合_I_02/位单J.忙点a纯惯1n组合.5001000150020002500300035004000时间/s俯仰角对比0时间/s5001000150020002500300035004000时间/s横滚角对比纬度对比864433/位单惯S合纯GP组34205001000150020002500300035004000时间/s经度对比时间/s/位单 X纯惯F、-.、GPS组合.11000150020002500300035004000时间/s北向速度对比东向速度对比05）s/m（/位单501000150020002500300035004000时间/

8、s05）s/m（/位单50东向失准角3-01xIt.k.-.*!/；j山5000/位单ti*3Ji*iLk严bjfu5001000150020002500300035004000时间/s天向失准角x10-31000150020002500300035004000时间/s北向失准角o01/位单5001000150020002500300035004000时间/s5050000-/位单505-）s/m（/位单3-01x东向速度误差4,.Ik*II/*.4丄于山丨Tib-*Fw0501000150020002500300035004000505-）s/m（/位单3-01x时间/s北向速度误差一亠-一

9、亠|.1|YV1片畑ff-tA0501000150020002500300035004000时间/s天向速度误差50）s/m（/位单0501000150020002500300035004000时间/s/位单/位单8-01X5o纬度误差一祁;认3*J.rPfrA韓打*Z./1JTl!Pnrfj508-01xo05-0.051000150020002500300035004000时间/s经度误差一一LAf.旳如心g1V-.-l-.i.怦1.-W斗F5001000150020002500300035004000时间/s高度误差_,005001000150020002500300035004000时

10、间/s/位单/位单东向失准角均方误差00005001000150020002500300035004000时间/s北向失准角均方误差o1000501000150020002500300035004000时间/s天向失准角均方误差/位单o500501000150020002500300035004000时间/s）s/m（/位单3-01X4东向速度均方误差o3250）s/m（/位单3-01X41000150020002500300035004000时间/s北向速度均方误差o3250）s/m（/位单3-01X431000150020002500300035004000时间/s天向速度均方误差5010

11、00150020002500300035004000时间/s01X42纬度均方误差/位单5001X5/位单1000150020002500300035004000时间/s经度均方误差0501000150020002500300035004000时间/s高度均方误差501000150020002500300035004000时间/s东向陀螺漂移均方误差550）h/（/位单1000150020002500300035004000时间/s北向陀螺漂移均方误差550）h/（/位单1000150020002500300035004000时间/s天向陀螺漂移均方误差10001500200025003000

12、35004000时间/s550）h/（/位单x10-5东向加计零偏均方误差x10-5北向加计零偏均方误差时间/sx10-5天向加计零偏均方误差四、实验分析由于纯惯高度通道发散，因此上述结果在纯惯结果与GPS结果或者组合导航结果比较时，仅仅给出经纬度与东向、北向速度的比对结果；而GPS只能给出速度位置信息，因此姿态信息只有纯惯结果与组合导航结果比对。分析以上结果，可知纯惯导航由于存在误差的积分效果，因此导航结果存在一定的漂移，经过组合导航之后，速度位置信息得到了很好的校正，姿态信息也得到了一定程度的修正，实现组合导航的意义。通过失准角、速度误差、位置误差的结果可见，在系统静止时误差较小，当系统处

13、于动态下，导航误差会增大，其原因在于此时系统的非线性效果加强了，因而误差会变大。通过均方误差的结果可见，在系统静止时，某些量（如东、北向陀螺漂移，东北向加计零偏等）收敛很慢，而一旦系统处于动态下，这些量会立即收敛，说明动态下系统的可观测度增大了，滤波的效果提升了。五、源程序%卡尔曼滤波第四次作业clearallcloseallclc%数据文件的处理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);%主循环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);%经度更新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;%存储滤波信息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);%姿态修正cnc=eye(3)+0Xk(3)-Xk(2);-Xk(3)0Xk(1);Xk(2)-Xk(1)0;cnb=cnb*cnc;%四元数修正cbn=cnb;lambda=cnb2q(cnb);lambda=lambda;angle=cnb2att(cnb);%俯仰角更新%横滚角更新%航向角更新%SINS_result为事先存好的纯惯导航结果theta(i+1)=angle(1);gamma(i+1)=angle(2);psi(i+1)=angle(3);e

32、ndend%画图%纯惯数据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;%组合数据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数据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);%导航结果对比figuresubplot(311)plot(lat_GPSl,g);gridon;xlabel(时间/s);ylabel(单位/);title(纬度对比);holdon;plot(lat1,r-);legend(GPS,组合)；subplot(3l2)plot(lon_GPS1,g);gridon;

35、xlabel(时间/s);ylabel(单位/);title(经度对比);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 %误差估计figuresubplot(311)plot(Slide_avt(

39、err_angle(l,:),20)/para);gridon;xlabel(时间/s);ylabel(单位/);title(东向失准角);subplot(3l2)plot(Slide_avt(err_angle(2,:),20)/para);gridon;xlabel(时间/s);ylabel(单位/);title(北向失准角);subplot(3l3)plot(Slide_avt(err_angle(3,:),20)/para);gridon;xlabel(时间/s);ylabel(单位/);title(天向失准角);figuresubplot(3ll)plot(Slide_avt(err

40、_velocity(1,:),20);gridon;xlabel(时间/s);ylabel(单位/(m/s);title(东向速度误差);subplot(312)plot(Slide_avt(err_velocity(2,:),20);gridon;xlabel(时间/s);ylabel(单位/(m/s);title(北向速度误差);subplot(313)plot(Slide_avt(err_velocity(3,:),20);gridon;xlabel(时间/s);ylabel(单位/(m/s);title(天向速度误差);figuresubplot(311)plot(Slide_avt(

41、err_position(1,:),20)/para);gridon;xlabel(时间/s);ylabel(单位/);title(纬度误差);subplot(312)plot(Slide_avt(err_position(2,:),20)/para);gridon;xlabel(时间/s);ylabel(单位/);title(经度误差);subplot(313)plot(Slide_avt(err_position(3,:),20);gridon;xlabel(时间/s);ylabel(单位/m);title(高度误差);%均方误差估计figuresubplot(311)plot(Slide

42、_avt(mse(1,:),20)/para);gridon;xlabel(时间/s);ylabel(单位/);title(东向失准角均方误差);subplot(312)plot(Slide_avt(mse(2,:),20)/para);gridon;xlabel(时间/s);ylabel(单位/);title(北向失准角均方误差);subplot(313)plot(Slide_avt(mse(3,:),20)/para);gridon;xlabel(时间/s);ylabel(单位/);title(天向失准角均方误差);figuresubplot(311)plot(Slide_avt(mse(

43、4,:),20);gridon;xlabel(时间/s);ylabel(单位/(m/s);title(东向速度均方误差);subplot(312)plot(Slide_avt(mse(5,:),20);gridon;xlabel(时间/s);ylabel(单位/(m/s);title(北向速度均方误差);subplot(313)plot(Slide_avt(mse(6,:),20);gridon;xlabel(时间/s);ylabel(单位/(m/s);title(天向速度均方误差);figuresubplot(311)plot(Slide_avt(mse(7,:),20)/para);gri

44、don;xlabel(时间/s);ylabel(单位/);title(纬度均方误差);subplot(312)plot(Slide_avt(mse(8,:),20)/para);gridon;xlabel(时间/s);ylabel(单位/);title(经度均方误差);subplot(313)plot(Slide_avt(mse(9,:),20);gridon;xlabel(时间/s);ylabel(单位/m);title(高度均方误差);figuresubplot(311)plot(Slide_avt(mse(10,:),20)/para*3600);gridon;xlabel(时间/s);

45、ylabel(单位/(/h);title(东向陀螺漂移均方误差);subplot(312)plot(Slide_avt(mse(11,:),20)/para*3600);gridon;xlabel(时间/s);ylabel(单位/(/h);title(北向陀螺漂移均方误差);subplot(313)plot(Slide_avt(mse(12,:),20)/para*3600);gridon;xlabel(时间/s);ylabel(单位/(/h);title(天向陀螺漂移均方误差);figuresubplot(311)plot(Slide_avt(mse(13,:),20)/g0);gridon;xlabe