sinsgpstcm2kf.m

GPS/惯导/电子罗盘组合导航

clear
R=6378137;
omega=7292115.1467e-11;
g=9.78;
T=14.4;
time=3750;
yinzi1=0.5;
yinzi2=0.5;

%initial value 
fai0=30*pi/180;
lamda0=30*pi/180;
vxe0=0.01;
vye0=0.01;

faie0=2.0/60*pi/180;
lamdae0=2.0/60*pi/180;
afae0=3.0/60*pi/180;
beitae0=3.0/60*pi/180;
gamae0=5.0/60*pi/180;

hxjz=pi/4;
vx=20*1852/3600*sin(hxjz);
vy=20*1852/3600*cos(hxjz);
%
weichagps=25;%GPS位置误差
suchagps=0.05;%GPS速度误差
gyroe0=(0.01/3600)*pi/180;
gyrotime=1/7200;%陀螺漂移反向相关时间
atime=1/1800;
gyronoise=(0.001/3600)/180*pi;%陀螺漂移白噪声
beta_d=1/6000.0;     %速度偏移误差反向相关时间
beta_drta=1/6000.0;  %偏流角误差反向相关时间

%matrix of system equation

fai=fai0;
lamada=lamda0;
     
zong=0*pi/180;
heng=0*pi/180;
hang=45*pi/180;


F(16,16)=0;
G(16,9)=0;

 

 
  %initial value 
  x1(16,1)=0;
  
%the error of sins

  xx=x1;
  
  xx(1)=faie0;  %ljn
  xx(2)=lamdae0;
  
  xx(5)=afae0;
  xx(6)=beitae0;
  xx(7)=gamae0;
  xx(8)=(0.01/3600)*pi/180;
  xx(9)=(0.01/3600)*pi/180;
  xx(10)=(0.01/3600)*pi/180;
  xx(11)=0.0005;
  xx(12)=0.0005;
  xx(13)=0.0005;
  
  
  %w=[gyronoise,gyronoise,gyronoise,gyronoise,gyronoise,gyronoise,g*1e-5,g*1e-5]';
  g1=randn(1,time);
  g2=randn(1,time);
  g3=randn(1,time);
  g4=randn(1,time);
  g5=randn(1,time);
  g6=randn(1,time);
  g7=randn(1,time);
  g8=randn(1,time);
  g9=randn(1,time);
  
  
% attitude change matrix
     
cbn(1,1)=cos(zong)*cos(hang)+sin(zong)*sin(heng)*sin(hang);
cbn(1,2)=-cos(zong)*sin(hang)+sin(zong)*sin(heng)*cos(hang);
cbn(1,3)=-sin(zong)*cos(heng);
cbn(2,1)= cos(heng)*sin(hang);
cbn(2,2)=cos(heng)*cos(hang);
cbn(2,3)=sin(heng);
cbn(3,1)= sin(zong)*cos(hang)-cos(zong)*sin(heng)*sin(hang);
cbn(3,2)=-sin(zong)*sin(hang)-cos(zong)*sin(heng)*cos(hang);
cbn(3,3)=cos(zong)*cos(heng);

F(1,4)=1/R;
F(2,3)=1/(R*cos(fai));
%F(3,1)=2*omega*vx*cos(fai)+vx*vy*sec(fai)^2/R;
F(3,1)=2*omega*vy*cos(fai)+vx*vy*sec(fai)^2/R;
%F(3,3)=vx*tan(fai)/R;
F(3,3)=vy*tan(fai)/R;
F(3,4)=vx*tan(fai)/R+2*omega*sin(fai);
F(3,6)=-g;
%F(4,1)=-(2*omega*vx*cos(fai)+vx^2*sec(fai)^2/R);
F(4,1)=-(2*omega*vx*sin(fai)+vx^2*sec(fai)^2/R);
F(4,3)=-2*(vx*tan(fai)/R+omega*sin(fai));
F(4,5)=g;
%F(4,7)=-g;
F(5,4)=-1/R;
F(5,6)=omega*sin(fai)+vx*tan(fai)/R;
F(5,7)=-(omega*cos(fai)+vx/R);
F(5,8)=1;
F(6,1)=-omega*sin(fai);
%F(6,3)=-1/R;
F(6,3)=1/R;
F(6,5)=-(omega*sin(fai)+vx*tan(fai)/R);
%F(6,7)=-vx/R;
F(6,7)=-vy/R;
F(6,9)=1;
F(7,1)=omega*cos(fai)+vx*sec(fai)^2/R;
F(7,3)=tan(fai)/R;
F(7,5)=omega*cos(fai)+vx/R;
%F(7,6)=vx/R;
F(7,6)=vy/R;
F(7,10)=1;
F(8,8)=-gyrotime;
F(9,9)=-gyrotime;
F(10,10)=-gyrotime;

	F(3,11)=cbn(1,1);
	F(3,12)=cbn(1,2);
	F(3,13)=cbn(1,3);

	F(4,11)=cbn(2,1);
	F(4,12)=cbn(2,2);
	F(4,13)=cbn(2,3);

	F(5,8)=cbn(1,1);
	F(5,9)=cbn(1,2);
	F(5,10)=cbn(1,3);

	F(6,8)=cbn(2,1);
	F(6,9)=cbn(2,2);
	F(6,10)=cbn(2,3);

	F(7,8)=cbn(3,1);
	F(7,9)=cbn(3,2);
   F(7,10)=cbn(3,3);
   
F(11,11)=-atime;
F(12,12)=-atime;
F(13,13)=-atime;
F(14,14)=-beta_d;
F(15,15)=-beta_drta;
F(16,16)=0;


G=[0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0; 
   0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0;
   1,0,0,0,0,0,0,0,0;
   0,1,0,0,0,0,0,0,0;
   0,0,1,0,0,0,0,0,0;
   0,0,0,1,0,0,0,0,0;
   0,0,0,0,1,0,0,0,0; 
   0,0,0,0,0,1,0,0,0;
   0,0,0,0,0,0,1,0,0;
   0,0,0,0,0,0,0,1,0; 
   0,0,0,0,0,0,0,0,1]; 

[A,B]=c2d(F,G,T);

  for i=1:time
     w(1,1)=gyronoise*g1(1,i);
     w(2,1)=gyronoise*g2(1,i);
     w(3,1)=gyronoise*g3(1,i);
     w(4,1)=(0.5*g*1e-5)*g4(1,i);
     w(5,1)=(0.5*g*1e-5)*g5(1,i);
     w(6,1)=(0.5*g*1e-5)*g6(1,i);
     w(7,1)=0.005*g7(1,i);
     w(8,1)=1/600*pi/180*g8(1,i);
     w(9,1)=0.0001*g9(1,i);
     xx=A*xx+B*w/T^2;
     
     
     sins1(1,i)=xx(1,1);
     sins1(2,i)=xx(2,1);
     sins1(3,i)=xx(3,1);
     sins1(4,i)=xx(4,1);
     sins1(5,i)=xx(5,1);
     sins1(6,i)=xx(6,1);
     sins1(7,i)=xx(7,1);

     s1(i)=xx(1,1)/pi*180*60;
     s2(i)=xx(2,1)/pi*180*60;
     s3(i)=xx(3,1)*3600/1852;
     s4(i)=xx(4,1)*3600/1852;
     s5(i)=xx(5,1)*180/pi*60;
     s6(i)=xx(6,1)*180/pi*60;
     s7(i)=xx(7,1)*180/pi*60;
  end 
  



fai0=30*pi/180;
lamda0=30*pi/180;
vxe0=0.01;
vye0=0.01;

faie0=2*pi/(180*60);
lamdae0=2*pi/(180*60);
afae0=3*pi/(180*60);
beitae0=3*pi/(180*60);
gamae0=5*pi/(180*60);

hxjz=pi/4;
vx=20*1842/3600*sin(hxjz);
vy=20*1842/3600*cos(hxjz);
%vx=0;
%vy=0;
fe=0;
fn=0;
fu=g;

% attitude change matrix
zong=0*pi/180;
heng=0*pi/180;
hang=45*pi/180;
cbn(1,1)=cos(zong)*cos(hang)+sin(zong)*sin(heng)*sin(hang);
cbn(1,2)=-cos(zong)*sin(hang)+sin(zong)*sin(heng)*cos(hang);
cbn(1,3)=-sin(zong)*cos(heng);
cbn(2,1)= cos(heng)*sin(hang);
cbn(2,2)=cos(heng)*cos(hang);
cbn(2,3)=sin(heng);
cbn(3,1)= sin(zong)*cos(hang)-cos(zong)*sin(heng)*sin(hang);
cbn(3,2)=-sin(zong)*sin(hang)-cos(zong)*sin(heng)*cos(hang);
cbn(3,3)=cos(zong)*cos(heng);
%
gpstime=1/600;
weichagps=25;%GPS位置误差
suchagps=0.05;%GPS速度误差
gyroe0=(0.01/3600)*pi/180;
gyrotime=1/7200;%陀螺漂移反向相关时间
atime=1/1800;
gyronoise=(0.01/3600)/180*pi;%陀螺漂移白噪声


tcm2time=1/300;
tcm2noise=0.04*pi/(60*180);
afatcm2=6*pi/(180*60);
betatcm2=6*pi/(180*60);
gamatcm2=6*pi/(180*60);

%matrix of system equation

fai=fai0;
lamada=lamda0;
F(22,22)=0;
F(1,4)=1/R;

F(2,1)=vx*tan(fai)*sec(fai)/R;
F(2,3)=sec(fai)/R;

F(3,1)=2*omega*vx*cos(fai)+vx*vy*sec(fai)^2/R;
F(3,3)=vx*tan(fai)/R;
F(3,4)=vx*tan(fai)/R+2*omega*sin(fai);
F(3,6)=-fu;
F(3,7)=fn;

F(4,1)=-(2*omega*vx*cos(fai)+vx^2*sec(fai)^2/R);
F(4,3)=-2*(vx*tan(fai)/R+omega*sin(fai));
F(4,5)=fu;
F(4,7)=-fe;

F(5,4)=-1/R;
F(5,6)=omega*sin(fai)+vx*tan(fai)/R;
F(5,7)=-(omega*cos(fai)+vx/R);
%F(5,8)=1;
F(6,1)=-omega*sin(fai);
F(6,3)=1/R;
F(6,5)=-(omega*sin(fai)+vx*tan(fai)/R);
F(6,7)=-vx/R;
%F(6,9)=1;
F(7,1)=omega*cos(fai)+vx*sec(fai)^2/R;
F(7,3)=tan(fai)/R;
F(7,5)=omega*cos(fai)+vx/R;
F(7,6)=vx/R;
%F(7,10)=1;
F(5,8)=cbn(1,1);
F(5,9)=cbn(1,2);
F(5,10)=cbn(1,3);
F(5,11)=cbn(1,1);
F(5,12)=cbn(1,2);
F(5,13)=cbn(1,3);
F(6,8)=cbn(2,1);
F(6,9)=cbn(2,2);
F(6,10)=cbn(2,3);
F(6,11)=cbn(2,1);
F(6,12)=cbn(2,2);
F(6,13)=cbn(2,3);
F(7,8)=cbn(3,1);
F(7,9)=cbn(3,2);
F(7,10)=cbn(3,3);
F(7,11)=cbn(3,1);
F(7,12)=cbn(3,2);
F(7,13)=cbn(3,3);

F(3,14)=cbn(1,1);
F(3,15)=cbn(1,2);
F(4,14)=cbn(2,1);
F(4,15)=cbn(2,2);

F(8,8)=-gyrotime;
F(9,9)=-gyrotime;
F(10,10)=-gyrotime;
F(14,14)=-atime;
F(15,15)=-atime;

F(16,16)=-gpstime;
F(17,17)=-gpstime;
F(18,18)=-gpstime;
F(19,19)=-gpstime;

F(20,20)=-tcm2time;
F(21,21)=-tcm2time;
F(22,22)=-tcm2time;



G(22,15)=0;
G=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,0; 
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
   cbn(1,1),cbn(1,2),cbn(1,3),0,0,0,0,0,0,0,0,0,0,0,0;
   cbn(2,1),cbn(2,2),cbn(2,3),0,0,0,0,0,0,0,0,0,0,0,0;
   cbn(3,1),cbn(3,2),cbn(3,3),0,0,0,0,0,0,0,0,0,0,0,0;
   0,0,0,1,0,0,0,0,0,0,0,0,0,0,0;
   0,0,0,0,1,0,0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,1,0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,0; 
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,1,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,1,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,1,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0,1,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0,0,1,0,0,0,0;
   0,0,0,0,0,0,0,0,0,0,0,1,0,0,0;
   0,0,0,0,0,0,0,0,0,0,0,0,1,0,0;
   0,0,0,0,0,0,0,0,0,0,0,0,0,1,0;
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,1]; 


 
 
   H1=[1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0;
      0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0;
      0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0;
      0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0,0,0];
   H2=[0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0,0;
      0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0;
      0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1];

  
  Q=[2*gyronoise^2/7200,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
     0,2*gyronoise^2/7200,0,0,0,0,0,0,0,0,0,0,0,0,0;
     0,0,2*gyronoise^2/7200,0,0,0,0,0,0,0,0,0,0,0,0;
     0,0,0,gyronoise^2,0,0,0,0,0,0,0,0,0,0,0;
     0,0,0,0,gyronoise^2,0,0,0,0,0,0,0,0,0,0;
     0,0,0,0,0,gyronoise^2,0,0,0,0,0,0,0,0,0;
     0,0,0,0,0,0,2*5*g*1e-5/1800,0,0,0,0,0,0,0,0;
     0,0,0,0,0,0,0,2*5*g*1e-5/1800,0,0,0,0,0,0,0;
     0,0,0,0,0,0,0,0,2*(25/R)^2/600,0,0,0,0,0,0;
     0,0,0,0,0,0,0,0,0,2*(25/R)^2/600,0,0,0,0,0;
     0,0,0,0,0,0,0,0,0,0,2*0.05^2/600,0,0,0,0;
     0,0,0,0,0,0,0,0,0,0,0,2*0.05^2/600,0,0,0;
     0,0,0,0,0,0,0,0,0,0,0,0,2*tcm2noise^2/300,0,0;
     0,0,0,0,0,0,0,0,0,0,0,0,0,2*tcm2noise^2/300,0;
     0,0,0,0,0,0,0,0,0,0,0,0,0,0,2*tcm2noise^2/300];
  Q1=1/yinzi1*Q;
  Q2=1/yinzi2*Q;
  
  r=[(weichagps/R)^2,0,0,0,0,0,0;
     0,(weichagps/R)^2,0,0,0,0,0;
     0 , 0,suchagps^2,0,0,0,0;
     0, 0, 0, suchagps^2,0,0,0;
     0,0,0,0,tcm2noise^2,0,0;
     0,0,0,0,0,tcm2noise^2,0;
     0,0,0,0,0,0,tcm2noise^2];
r1=[(weichagps/R)^2,0,0,0;
     0,(weichagps/R)^2,0,0;
     0 , 0,suchagps^2,0;
     0, 0, 0, suchagps^2];
 r2=[tcm2noise^2,0,0;
     0,tcm2noise^2,0;
     0,0,tcm2noise^2];


  %discrete manage
  [A,B]=c2d(F,G,T);
  r1=r1/T;
  r2=r2/T;
  Q1=Q1/T;
  Q2=Q2/T;

 

  %initial value 
  p=[faie0^2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
     0,lamdae0^2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
     0,0,vxe0^2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
     0,0,0,vye0^2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
     0,0,0,0,afae0^2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
     0,0,0,0,0,beitae0^2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
     0,0,0,0,0,0,gamae0^2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
     0,0,0,0,0,0,0,gyroe0^2,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
     0,0,0,0,0,0,0,0,gyroe0^2,0,0,0,0,0,0,0,0,0,0,0,0,0;
     0,0,0,0,0,0,0,0,0,gyroe0^2,0,0,0,0,0,0,0,0,0,0,0,0;
     0,0,0,0,0,0,0,0,0,0,gyroe0^2,0,0,0,0,0,0,0,0,0,0,0;
     0,0,0,0,0,0,0,0,0,0,0,gyroe0^2,0,0,0,0,0,0,0,0,0,0;
     0,0,0,0,0,0,0,0,0,0,0,0,gyroe0^2,0,0,0,0,0,0,0,0,0;
     0,0,0,0,0,0,0,0,0,0,0,0,0,(g*5*1e-5)^2,0,0,0,0,0,0,0,0;
     0,0,0,0,0,0,0,0,0,0,0,0,0,0,(g*5*1e-5)^2,0,0,0,0,0,0,0;
     0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,(25/R)^2,0,0,0,0,0,0;
     0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,(25/R)^2,0,0,0,0,0;
     0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.05^2,0,0,0,0;
     0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.05^2,0,0,0;
     0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,afatcm2^2,0,0;
     0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,betatcm2^2,0;
     0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,gamatcm2^2];
  
  
  p11=1/yinzi1*p;
  p22=1/yinzi2*p;


  x(22,1)=0;
  x1=x;
  x2=x;

  randn('seed',9);
  %the error of gps and tcm2
  gp1=randn(1,time);
  gp2=randn(1,time);
  gp3=randn(1,time);
  gp4=randn(1,time);

  tc1=randn(1,time);
  tc2=randn(1,time);
  tc3=randn(1,time);


  
  for i=1:time
  gps1(i,1)=gp1(1,i)*25/R;
  gps2(i,2)=gp2(1,i)*25/R;
  gps3(i,3)=gp3(1,i)*0.05;
  gps4(i,4)=gp4(1,i)*0.05;

  tcm21(i,1)=tc1(1,i)*tcm2noise;
  tcm22(i,2)=tc2(1,i)*tcm2noise;
  tcm23(i,3)=tc3(1,i)*tcm2noise;
  end
  %kalman filter
  
    %filter no-reset
    
    for i=1:time
       
       
       x1=A*x1;
       p1=A*p11*A'+B*Q1*B';
       
       x2=A*x2;
       p2=A*p22*A'+B*Q2*B';
       
       z1=[sins1(1,i)-gps1(i,1)
       sins1(2,i)-gps2(i,2)
       sins1(3,i)-gps3(i,3)
       sins1(4,i)-gps4(i,4)];

       z2=[sins1(5,i)-tcm21(i,1)
          sins1(6,i)-tcm22(i,2)
          sins1(7,i)-tcm23(i,3)];
       
       gama1=z1-H1*x1;
       u1=H1*p1*H1'+r1;
       %lamda1(i)=gama1'*inv(u1)*gama1
       lamda1(i)=gama1(1,1)^2/u1(1,1);

       p11=inv(inv(p1)+H1'*inv(r1)*H1);
       x1=p11*(inv(p1)*x1+H1'*inv(r1)*z1);
       
       gama2=z2-H2*x2;
       u2=H2*p2*H2'+r2;
       lamda2(i)=gama2'*inv(u2)*gama2;

       p22=inv(inv(p2)+H2'*inv(r2)*H2);
       x2=p22*(inv(p2)*x2+H2'*inv(r2)*z2);
       
       p=inv(inv(p11)+inv(p22));
       x=p*(inv(p11)*x1+inv(p22)*x2);
       
       %reset
       
       x1=x;
       x2=x;
       p11=1/yinzi1*p;
       p22=1/yinzi2*p;
       
       %****************
       
    sg1(i)=x(1,1)*180*60/pi;
    sg2(i)=x(2,1)*180*60/pi;
    sg3(i)=x(3,1)*3600/1842;
    sg4(i)=x(4,1)*3600/1842;
    sg5(i)=x(5,1)*180*60/pi;
    sg6(i)=x(6,1)*180*60/pi;
    sg7(i)=x(7,1)*180*60/pi;
   

    
 end
 for i=1:time
    t(i)=0;
   end
  for i=1:time
     t(i)=(t(i)+T*i)/3600;
     ss1(i)=s1(i)-sg1(i);
     ss2(i)=s2(i)-sg2(i);
     ss3(i)=s3(i)-sg3(i);
     ss4(i)=s4(i)-sg4(i);
     ss5(i)=s5(i)-sg5(i);
     ss6(i)=s6(i)-sg6(i);
     ss7(i)=s7(i)-sg7(i);
 end
 
 figure(1);
subplot(3,2,1)
plot(t,sg1,'b:') 
grid
xlabel('time(h)')
ylabel('纬度误差估计（角分）')
subplot(3,2,2)
plot(t,ss1,'b:') 
grid
xlabel('time(h)')
ylabel('误差的残差曲线（角分）')
subplot(3,2,3)
plot(t,sg2 ,'b:')
grid
xlabel('time(h)')
ylabel('经度误差估计（角分）')
subplot(3,2,4)
plot(t,ss2 ,'b:')
grid
xlabel('time(h)')
ylabel('误差的残差曲线（角分）')
set(gcf,'color',[1 1 1])


figure(2);
subplot(3,2,1)
plot(t,sg3,'b:') 
grid
xlabel('time(h)')
ylabel('东向速度误差估计（kn）')
subplot(3,2,2)
plot(t,ss3,'b:') 
grid
xlabel('time(h)')
ylabel('误差的残差曲线（kn）')

subplot(3,2,3)
plot(t,sg4 ,'b:')
grid
xlabel('time(h)')
ylabel('北向速度误差估计(kn)')
subplot(3,2,4)
plot(t,ss4 ,'b:')
grid
xlabel('time(h)')
ylabel('误差的残差曲线(kn)')
set(gcf,'color',[1 1 1])

figure(3);
subplot(3,2,1)
plot(t,sg5,'b:') 
grid
xlabel('time(h)')
ylabel('纵摇角误差估计（角分）')
subplot(3,2,2)
plot(t,ss5,'b:') 
grid
xlabel('time(h)')
ylabel('误差的残差曲线（角分）')

subplot(3,2,3)
plot(t,sg6 ,'b:')
grid
xlabel('time(h)')
ylabel('横摇角误差估计（角分）')
subplot(3,2,4)
plot(t,ss6 ,'b:')
grid
xlabel('time(h)')
ylabel('误差的残差曲线（角分）')

subplot(3,2,5)
plot(t,sg7 ,'b:')
grid
xlabel('time(h)')
ylabel('首向角误差估计（角分）')
subplot(3,2,6)
plot(t,ss7 ,'b:')
grid
xlabel('time(h)')
ylabel('误差的残差曲线（角分）')
set(gcf,'color',[1 1 1])