卡尔曼滤波第四次作业

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

2、、 和H分别为纬度误差、经度误差和高度误差； 系统噪声过程噪声w wxwy wz w x w y w z T ,包括陀螺和加速度计的随机 误差不包括随机常值误差；系统噪声方差阵Q根据SINS/GPS组合导航系统的惯性器件噪声水平选取.状态转移矩阵 F和系统噪声矩阵G的具体形式为：F9N906 9F9%06 6 15 15Cb 03 303 3 Cb09 309 3 15 6Cn 03 3F矩阵中,FS 03 3 Cn,FN为对应惯导9个误差参数3个姿态误差,303 3 03 3个速度误差,3个位置误差的系统动态矩阵,它是9*9阶方阵.其中非零 元素可由惯导误差模型推导,具体为：f1,2VE .

3、 ie sin L tgLRN+hf1,3(ie cos LVeRnh)f1,5f 2,31Rm + hVnRm + hf2,1f2,4(ie sin LRN + hVeRnhtan L)f2,7ie sin L,1ie cos LVef3,2VnRMhf3,4Rnf3,7ie cos LVeRn一sec hf4,2fuf4,3 fnf 4,4Vn RM一 tan L hVurM_hf4,5ie sinVetgL RN + hf 4,6(2ie cos Lf4,7ie cos LVnVeVRnN 2 .sec L h2 ie sin L Vuf5,1fUf5,3fE5,42(ie sin LV

4、E tgL) rn + hf5,5VuRMhf5,6VnRMhf5,7(2 ie cos-sec2Rn hL)Vef6,1f6,2fEf 6,42( ie cos Lf 6,5f7,5f8,72VnRm h2Ve ie cos L1sec L1-f8 4Rmh,RN + hVsec L RN + htan Lf9,612 .量测方程SINS/GPS组合导航采用松组合的方式,GPS提供位置和速度信息,采用SINS 与GPS的位置和速度之差作为卡尔曼滤波的量测信息.量测方程：Z(t) H(t)X(t) V(t)HvHpX(t)M(t)Vp(t)式中,量测向量Z(t)VeVnVuL h T ,分别为

5、捷联解算与GPS的东向速度、北向速度、大向速度、纬度、经度和高度之差； V (t)VVe v Vn v Vu V L V vJ为量测噪声.量测噪声方差阵R根据GPS的位置、速度噪声水平选取；量测矩阵 H HV HP艮中.H v O3 3 diag 1 1 103 93 15H p 03 6 diag RmRn cosL103 63 15.3.卡尔曼滤波的流程为:状态一步预测与均方误差阵一步预测:Xk/k 1 k/kiXk滤波增益阵计算:Pk/k 1 k/k 1Pk 1Tk/k 1R) 1状态估计与均方误差阵估计:Xk Xk/k1 Kk(Zk HXk/k1)PkI KkHPk/k1I KkHT

6、KkRKT三、运行结果1 .滤波参数设置a陀螺常值漂移：度/小时；加速度计常值偏置：50ug；b GPS位置测量误差：水平 m,垂直；速度测量误差：/s；c初始姿态误差：航向30角分,俯仰/横滚60角秒；d初始速度误差：东向/北向/天向：；e初始位置误差：水平位置误差 2m,垂直位置误差2m.2 .初始条件IMU的初始姿态为：航向角为度北偏东逆时针为正,俯仰角为度,横滚角 为度；初始位置与初始速度为 GPS在第一个时刻的输出.3 .运行结果纬度比照35°/位 34.5单34GPS组合05001000150020002500300035004000/位 单时间/s经度比照时间/s高度比

7、照4000L(J' IGPS1hFr组合nr20005001000150020002500300035004000时间/s东向速度比照/(位单1000-100 0GPS组合5001000150020002500时间/s北向速度比照3000350040001000-10005001000150020002500时间/s天向速度比照100-100500GPS组合300035004000GPS组合1000150020002500300035004000时间/sOOO4航向角比照限O O惯合 纯组O O5 2O OO 2O OO O2400./位单./位单./位单-20匚 035./位单34.

8、2圻It小母纯贤-,GPS组合纬度比照34.834.634.4组合500100015002000250030003500时间/s500100015002000250030003500时间/s经度比照110./位单109.505001000150020002500300035004000时间/s东向速度比照纯惯GPS组合o O 0.0 105-52m/(位单2m八位单纯惯GPS组合I - o 00-510./位单o1X1 o东向失准角1000150020002500300035004000时间/s北向速度比照1000150020002500300035004000时间/so1X1 o./位单./

9、位单5 0 5 o o6 S-1000150020002500300035004000时间/s北向失准角1000150020002500300035004000时间/s天向失准角1000150020002500300035004000时间/s510Xx)s/m八位单2m八位单一 O 5 OO6-8O1X5纬度误差./位单O东向速度误差 FF-.*卜1麟小才喙w叫憧桃k 柚jrt1000150020002500300035004000时间/s北向速度误差1000150020002500300035004000时间/s天向速度误差1000150020002500300035004000时间/s3O

10、1X5./位单O5 0 5 o O6 6 -1000150020002500300035004000时间/s经度误差1000150020002500300035004000时间/s高度误差1000150020002500300035004000时间/s东向失准角均方误差0.02./位单O1 O O61000150020002500300035004000时间/s北向失准角均方误差./位单2 10 o O6 61000150020002500300035004000时间/s天向失准角均方误差./位单;一r r11000150020002500300035004000时间/s)STm八位单3-O1

11、X东向速度均方误差- L O 4 3 23-O1X1000150020002500300035004000时间/s北向速度均方误差4 3 23-O1X1000150020002500300035004000时间/s天向速度均方误差4 3 21000150020002500300035004000时间/s10X4./位单 ./位单-7纬度均万误差O1X51000150020002500300035004000时间/s经度均方误差8位单一 O 15 0 O 9O1000150020002500300035004000时间/s高度均方误差1000150020002500300035004000时间/

12、s东向陀螺漂移均方误差1000150020002500300035004000时间/s北向陀螺漂移均方误差- O 15 0 O 9O一 O 15 0O 口OO 15 0O 口O1000150020002500300035004000时间/s天向陀螺漂移均方误差1000150020002500300035004000时间/s东向加计零偏均方误差g位 单05001000150020002500300035004000时间/s-IP白出口 重力白为七；q关时间/Sg位 单时间/S四、实验分析由于纯惯高度通道发散,因此上述结果在纯惯结果与 GPS结果或者组合导航 结果比拟时,仅仅给出经纬度与东向、北向

13、速度的比对结果；而 GPS只能给出 速度位置信息,因此姿态信息只有纯惯结果与组合导航结果比对.分析以上结果,可知纯惯导航由于存在误差的积分效果,因此导航结果存在 一定的漂移,经过组合导航之后,速度位置信息得到了很好的校正, 姿态信息也 得到了一定程度的修正,实现组合导航的意义.通过失准角、速度误差、位置误差的结果可见,在系统静止时误差较小,当 系统处于动态下,导航误差会增大,其原因在于此时系统的非线性效果增强了, 因而误差会变大.通过均方误差的结果可见,在系统静止时,某些量如东、北向陀螺漂移, 东北向加计零偏等收敛很慢,而一旦系统处于动态下,这些量会立即收敛,说 明动态下系统的可观测度增大了,

14、滤波的效果提升了.五、源程序%卡尔曼滤波第四次作业clear allclose all clc % %数据文件的处理 format long para=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)*para; lon_GPS=GPS(:,4)*para; h_GPS=GPS(:,5); v_GPS=GPS(:,6:8)' % %变量的初始化-纯惯局

15、部 dt=0.01;% IMU periodT=0.05;% KF periodre=6378245; e=1/298.3;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=zeros(1,L+1); lambda=zeros(1,4);lat(1)=lat_GPS(1); 10n(1)=lon_GPS(1); h(1)=

16、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(gamm a(1)/2);lambda(2)=cos(psi(1)/2)*sin(theta(1)/2)*cos(gamma(1)/2) -sin(psi(1)/2)*cos(theta(1)/2)*sin(gamma(1)

17、/2);lambda(3)=cos(psi(1)/2)*cos(theta(1)/2)*sin(gamma(1)/2)+sin(psi(1)/2)*sin(theta(1)/2)*cos(gam ma(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(lon)cos(lon)0;-sin(lat(1)*cos(lon(1) -sin(lat(1)*sin(lon(1) cos(

18、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)A2,(0.1*pi/180/3600)A2,(0.1*pi/180/3600)A2,(5e-5*g0)A2,(5e -5*g0)A2,(5e-5*g0)A2,0,0,0,0,0,0,0,0,0);R=diag(

19、0.01A2,0.01A2,0.01A2,0.1A2,0.1A2,0.15A2);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*pi/180/3600)A2,(0.1*pi/180/3600)A2,(50*(1e-6)*g0)A2,(50*(1e -6)*

20、g02,(50*(1e-6)*g0)A2);Pkk1=zeros(15,15);Kk=zeros(15,6);Zk=zeros(6,1);mse=zeros(15,72000);% mse is short for mean square errorerr_angle=zeros(3,72000);err_position=zeros(3,72000);err_velocity=zeros(3,72000);%主循环for i=1:Lg = g0 * (1 + 0.0052884 * sin(lat(i)A2 - 0.0000059 * sin(2*lat(i)42)- 0.0003086 *

21、 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*0 0 wie'wenn=-v(2,i)/ry v(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)wnbbdot(3)-wnbbdot(1) 0-wnb

22、bdot(3) wnbbdot(2)-wnbbdot(2) wnbbdot(3) 0-wnbbdot(1)-wnbbdot(3);-wnbbdot(2);wnbbdot(1);0;temp=(1 -(m1A2)/8+(m1A4)/384)*eye(4)+(0.5 -(m1A2)/48)*lambdadot)*lambda'lambda=temp'lambda=lambda/(norm(lambda);cnb=q2cnb(lambda);cbn=cnb'angle=cnb2att(cnb);theta(i+1)=angle(1);% 俯仰角更新gamma(i+1)=ang

23、le(2);% 横滚角更新psi(i+1)=angle(3);% 航向角更新%?fibn=cbn*(fibb(:,i);vxdot=fibn(1)+(2*wie*sin(lat(i)+v(1,i)*tan(lat(i)/rx)*v(2,i);vydot=fibn(2) -(2*wie*sin(lat(i)+v(1,i)*tan(lat(i)/rx)*v(1,i);vzdot=fibn(3)+(2*wie*cos(lat(i)+v(1,i)/rx)*v(1,i)+v(2,i)A2/ry-g;v(1,i+1)=v(1,i)+vxdot*dt;% 东向速度更新v(2,i+1)=v(2,i)+vydo

24、t*dt;% 北向速度更新v(3,i+1)=v(3,i)+vzdot*dt;% 天向速度更新wenn=-v(2,i+1)/ryv(1,i+1)/rxv(1,i+1)/rx*cen(3,3)/cen(2,3)'wenndot= 0 -wenn(3) wenn(2);wenn(3) 0-wenn(1);-wenn(2) wenn(1) 0;dcen=-wenndot*cen;cen=cen+dcen*dt;lat(i+1)=asin(cen(3,3);%纬度更新lon(i+1)=acos(cen(1,2);%经度更新h(i+1)=h(i)+v(3,i+1)*dt;%高度更新%开始卡尔曼滤波

25、if (rem(i,5) = 0 )k=fix(i/5)+1;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*s

26、in(lat(i);Fn(3,1) = wie*cos(lat(i) + v(1,i) / (rx + h(i);Fn(3,2) = v(2,i) / (ry + h(i);Fn(3,4) = tan(lat(i) / (rx + h(i);Fn(3,7) = wie*cos(lat(i) + v(1,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 * w

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

28、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(1,i) / (rx + h(i) * sec(lat(i)A2) * v(1,i);Fn(6,1) = -fibn(2);Fn(6,2) = fibn(1);Fn(6,4) = 2 * (wie*cos(lat(i) + v(1,i) / (rx + h(i);Fn(6,5) = 2 * v(2,i) / (ry + h(i);Fn(6,7) = -2 * v(1,

29、i) * wie*cos(lat(i);Fn(7,5) = 1 / (ry + h(i);Fn(8,4) = sec(lat(i) / (rx + h(i);Fn(8,7) = v(1,i) / (rx + h(i) * sec(lat(i) * tan(lat(i);Fn(9,6) = 1;cbn=cnb'Fs=cbn,zeros(3,3);zeros(3,3),diag(cbn(1,1),cbn(2,2),cbn(3,3);zeros(3,3),zeros(3,3);F=Fn,Fs;zeros(6,9),zeros(6,6);Phi=eye(15)+F*T+TA2*FA2/2;H(

30、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=Pkk1*H'/(H*Pkk1*H'+R);Xk=

31、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);mse(3,k)=

32、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);%位置修正lat(i+1)

33、=lat(i+1) -Xk(7);lon(i+1)=lon(i+1) -Xk(8);h(i+1) = h(i+1) -Xk(9);%姿态修正cnc=eye(3)+0 Xk(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);%四元数修正end%俯仰角更新%横滚角更新%航向角更新theta(i+1)=angle(1);gamma(i+1)=angle(2);psi(i+1)=angle(3);end%画图%SINS r

34、esult为事先存好的纯惯导航结果%纯惯数据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,10

35、0)/para;%组合数据lat1=Slide_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;l

36、on_GPS1=Slide_avt(lon_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_GPS1,'g');grid on;xlabel('时间/s');ylabel('单位/° ');title('纬度比照');hold

37、on;plot(lat1,'r -');legend('GPS','组合');subplot(312)plot(lon_GPS1,'g');grid on;xlabel('时间/s');ylabel('单位/° ');title('经度比照');hold on;plot(lon1,'r -');legend('GPS','组合');subplot(313)plot(h_GPS1,'g');grid on;xla

38、bel('时间/s');ylabel('单位/m');title('高度比照');hold on;plot(h1,'r -');legend('GPS',纽合');figuresubplot(311)plot(vx_GPS1,'g');grid on;xlabel('时间/s');ylabel('单位/ (m/s) ');title('东向速度比照,);hold on;plot(vx1,'r -');legend('GPS

39、9;,'组合')；subplot(312)plot(vy_GPS1,'g');grid on;xlabel('时间/s');ylabel('单位/ (m/s) ');title('北向速度比照,);hold on;plot(vy1,'r -');legend('GPS','组合')；subplot(313)plot(vz_GPS1,'g');grid on;xlabel('时间/s');ylabel('单位/ (m/s) ');

40、title('天向速度比照,);hold on;plot(vz1,'r -');legend('GPS','组合');figuresubplot(311)plot(ps_INS1,'b');grid on;xlabel('时间/s');ylabel('单位/° ');title(,航向角比照,);hold on;plot(ps1,'r -');legend('纯惯','组合');subplot(312)plot(th_INS1,

41、9;b');grid on;xlabel('时间/s');ylabel('单位/° ');title('俯仰角比照,);hold on;plot(th1,'r -');legend('纯惯','组合');subplot(313)plot(ga_INS1,'b');grid on;xlabel('时间/s');ylabel('单位/° ');title('横滚角比照,);hold on;plot(ga1,'r -

42、9;);legend('纯惯','组合');figuresubplot(211)plot(lat_INS1,'b');grid on;xlabel('时间/s');ylabel('单位/° ');title('纬度比照');hold on;plot(lat_GPS1,'g');plot(lat1,'r -');legend('纯惯','GPS','组合')；subplot(212)plot(lon_INS1,&

43、#39;b');grid on;xlabel('时间/s');ylabel('单位/° ');title('经度比照');hold on;plot(lon_GPS1,'g');plot(lon1,'r -');legend('纯,惯','GPS',组合')；figuresubplot(211)plot(vx_INS1,'b');grid on;xlabel('时间/s');ylabel('单位/(m/s) ')

44、;title('东向速度比照');hold on;plot(vx_GPS1,'g');plot(vx1,'r -');legend('纯惯','GPS','组合');subplot(212)plot(vy_INS1,'b');grid on;xlabel('时间/s');ylabel('单位/(m/s) ');title('北向速度比照');hold on;plot(vy_GPS1,'g');plot(vy1,'

45、r -');legend('纯惯','GPS','组合');%误差估计figuresubplot(311)plot(Slide_avt(err_angle(1,:),20)/para);grid1)；subplot(312)plot(Slide_avt(err_angle(2,:),20)/para);grid 1);subplot(313)plot(Slide_avt(err_angle(3,:),20)/para);grid 1);on;xlabel('时间 /s');ylabel('单位 / ');t

46、itle('东向失准角on;xlabel('时间 /s');ylabel('单位 /');title('北向失准角on;xlabel('时间 /s');ylabel('单位 / ');title(,天向失准角figuresubplot(311)plot(Slide_avt(err_velocity(1,:),20);grid on;xlabel(' 时间 /s');ylabel('单位 / (m/s) ');title('东向速度 误差,);subplot(312)plot(

47、Slide_avt(err_velocity(2,:),20);grid on;xlabel(' 时间 /s');ylabel('单位 / (m/s) ');title('北向速度 误差,);subplot(313)plot(Slide_avt(err_velocity(3,:),20);grid on;xlabel(' 时间 /s');ylabel('单位 / (m/s) ');title(,天向速度 误差,);figuresubplot(311)plot(Slide_avt(err_position(1,:),20)/

48、para);grid on;xlabel('时间/s');ylabel('单位/ ° ');title('纬度误差1);subplot(312)plot(Slide_avt(err_position(2,:),20)/para);grid on;xlabel('时间/s');ylabel('单位/ ° ');title('经度误差1);subplot(313)plot(Slide_avt(err_position(3,:),20);grid on;xlabel(' 时间 /s')

49、;ylabel('单位 /m');title('高度误差');%均方误差估计figuresubplot(311)plot(Slide_avt(mse(1,:),20)/para);grid误差,);subplot(312)plot(Slide_avt(mse(2,:),20)/para);grid误差,);subplot(313)plot(Slide_avt(mse(3,:),20)/para);gridon;xlabel('时间 /s');ylabel('单位 / ');title('东向失准角均方on;xlabel(&

50、#39;时间 /s');ylabel('单位 /° ');title('北向失准角均方on;xlabel('时间 /s');ylabel('单位 / ');title(,天向失准角均方误差,);figuresubplot(311)plot(Slide_avt(mse(4,:),20);grid on;xlabel('时间/s');ylabel('单位/ ( m/s) ');title('东向速度均方误 差)；subplot(312)plot(Slide_avt(mse(5,:),2

51、0);grid on;xlabel('时间/s');ylabel('单位/ ( m/s) ');title('北向速度均方误 差)；subplot(313)plot(Slide_avt(mse(6,:),20);grid on;xlabel('时间/s');ylabel('单位/ ( m/s) ');title('天向速度均方误 差)；figuresubplot(311)plot(Slide_avt(mse(7,:),20)/para);grid on;xlabel('时间/s');ylabel(&

52、#39;单位/° ');title('纬度均方误差,);subplot(312)plot(Slide_avt(mse(8,:),20)/para);grid on;xlabel('时间/s');ylabel('单位/° ');title('经度均方误差,);subplot(313)plot(Slide_avt(mse(9,:),20);grid on;xlabel('时间/s');ylabel('单位/m');title('高度均方误差,);figuresubplot(311)p

53、lot(Slide_avt(mse(10,:),20)/para*3600);grid on;xlabel('时间 /s');ylabel('单位/ (° /h) ');title('东向 陀螺漂移均方误差,);subplot(312)plot(Slide_avt(mse(11,:),20)/para*3600);grid on;xlabel('时间/s');ylabel('单位/ (° /h) ');title('北向 陀螺漂移均方误差,);subplot(313)plot(Slide_avt(mse(12,:),20)/para*3600);grid on;xlabel('时间 /s');ylabel('单位/ (° /h) ');title(,天向 陀螺漂移均方误差,);figuresubplot(311)plot(Slide_avt(mse(13,:),20)/g0);grid on;xlabel('时间 /s');ylabel('单位 /g');title('东向加计零偏均方 误差,);