erroreqn.m

组合导航的程序

%The simulation of strapdown navigation general error equations
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%This model is applicable to a navigation system intalled in a vehicle which is moving over the surfaceo of 
%a spherical earth.
Re = 6378393.0; %the radius of the earth    
e = 3.367e-3;   %
g0 = 9.78049; %g = g0+0.051799*sin(phi)*sin(phi);
wie = 7.2921158e-5;    
PI = 3.1415926;
Rad_D = 0.01745329; % Rad_Degree 

phi = 45.7796;
lamda = 126.6705; 

dr_e = 0.01*0.01745329/3600;% gyro drift
dr_n = 0.01*0.01745329/3600; 
dr_z = 0.01*0.01745329/3600; 

bias_e = 5e-4;   %accelermeter bias
bias_n = 5e-4; 
bias_z = 5e-4; 

 phi = phi*Rad_D;
 lamda = lamda*Rad_D;
 step = 100;
 
 ve = 0.0;% stationary 
 vn = 0.0;
 fe = 0.0;
 fn = 0.0;
 fz = 9.8;
 alfa0 = 3*Rad_D/60; %1 delta:there is a 68%probability of each lying with the limits indicated.
 beta0 = 3*Rad_D/60; %The distribution of errors is assuned tobe Gaussian and the above figures represent 1 delta value.
 gama0 = 5*Rad_D/60;
 dve0 = 0.1; %unit  m/s
 dvn0 = 0.1; %unit  m/s
 dphi0 = 2*Rad_D/60;  % unit: radian
 dlamda0 =2*Rad_D/60; %unit: radian


 
 %%f_dx = Fdx + Gu; % A matrix error eqution
 
 dx = [alfa0; beta0; gama0; dve0; dvn0; dphi0; dlamda0];
 u =  [dr_e; dr_n; dr_z; bias_e; bias_n];
  
 G = [ -1   0   0  0  0  
       0   -1   0  0  0  
       0   0   -1  0  0   
       0   0   0  1  0  
       0	  0   0  0  1  
       0   0   0  0  0  
       0   0   0  0  0 ];
    
  for j =1:129600/step
    
    Rn = Re*(1+e*sin(phi)*sin(phi));
    Rm = Re*(1-2*e+3*e*sin(phi)*sin(phi));
    
   F = [                  0         wie*sin(phi)+ve*tan(phi)/Rn  -(wie*cos(phi)+ve/Rn)   0   -1/Rm   0              0;
       -(wie*sin(phi)+ve*tan(phi)/Rn)        0                   -vn/Rm              1/Rn   0     -wie*sin(phi)  0;
       wie*cos(phi)+ve/Rn             vn/Rm           0     tan(phi)/Rn          0        wie*cos(phi)+ve*sec(phi)*sec(phi)/Rn  0; 
       0             -fz        fn      vn*tan(phi)/Rn     2*wie*sin(phi)+ve*tan(phi)/Rn    2*wie*cos(phi)*vn+ve*vn*sec(phi)*sec(phi)/Rn  0;
       fz           0          -fe     -2*(wie*sin(phi)+ve*tan(phi)/Rn)  0     -(2*wie*cos(phi)*ve+ve*ve*sec(phi)*sec(phi)/Rn)             0;
       0           0             0           0               1/Rm          0             0;
       0           0             0          sec(phi)/Rn       0           ve*sec(phi)*tan(phi)/Rn   0   ];
  
  
    f_dx = F*dx+G*u;
  
	   for i = 1:7
	     k(1,i) = f_dx(i);
        y(i) = dx(i);  
      end
	 
      for i = 1:7  
        dx(i) = y(i)+step*k(1,i);
      end
      f_dx = F*dx+G*u;
     
     for i =1:7  
       k(2,i) = f_dx(i);
       dx(i) = y(i)+step*(k(1,i)+k(2,i))/2.0;
     end
        
      alfa(j+1) = dx(1)*60/Rad_D;
      beta(j+1) = dx(2)*60/Rad_D;
      gama(j+1) = dx(3)*60/Rad_D;
       dve(j+1) = dx(4);
       dvn(j+1) = dx(5);
      dphi(j+1) = dx(6)*60/Rad_D;
      dlamda(j+1) = dx(7)*60/Rad_D;
 

end     

  alfa(1) = alfa0*60/Rad_D;
  beta(1) = beta0*60/Rad_D;
  gama(1) = gama0*60/Rad_D;
   dve(1) = dve0;
   dvn(1) = dvn0;
  dphi(1) = dphi0*60/Rad_D;
dlamda(1) = dlamda0*60/Rad_D;

i=1:1:129600/step+1;
  subplot(4,2,1)
  plot(i,alfa(i),'r')
  subplot(4,2,2)
  plot(i,beta(i),'r')
  subplot(4,2,3)
  plot(i,gama(i),'r')
  subplot(4,2,4)
  plot(i,dve(i),'r')
  subplot(4,2,5)
  plot(i,dvn(i),'r')
  subplot(4,2,6)
  plot(i,dphi(i),'r')
  subplot(4,2,7)
  plot(i,dlamda(i),'r')