strapdown.m

组合导航的程序

%%The simulation of strapdown system
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%navigation frame ( ENZ )
Re = 6378393.0; %Re = 6378137.0
e = 3.367e-3;  
DeltaQuat = 1e-12;  %四元数误差
pi= 3.1415926;
Rad_Deg = 0.01745329; 
g0 = 9.78049; 
wie = 7.2921158e-5;   
w_Drift = 0.01*0.01745329/3600; 
a_bias = 1e-4; 
   
phi = 45.7796;% inital longtitude and latitude
lamda = 126.6705; 
v = [0.0  
     0.0];
  
phi = phi*Rad_Deg; 
lamda = lamda*Rad_Deg;

% c(1,1) = -sin(lamda);
% c(1,2) =  cos(lamda);
% c(1,3) = 0.0;

% c(2,1) = -sin(phi)*cos(lamda);
% c(2,2) = -sin(phi)*sin(lamda);
% c(2,3) =  cos(phi);

% c(3,1) = cos(phi)*cos(lamda);
% c(3,2) = cos(phi)*sin(lamda);
% c(3,3) = sin(phi);

q = [1
     0
     0
     0];% inital quarternion (static base)
  
T = eye(3);
wiep = [ 0
   wie*cos(phi)
   wie*sin(phi)];		
wepp =[0.0;0.0;0.0];   	
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
wib = [0 + w_Drift     %gyro error model
       wie*cos(phi) + w_Drift  
       wie*sin(phi) + w_Drift];
                                        
fb = [0  + 1e-5                       %accelerator model 
	   0  + 1e-5                          
     9.8 + 1e-5];       
h = 60;               %sample interval 
                      %仿真周期长有发散的趋势!  
for i = 1:86400*2/h %simulation time  
   
    wpbb = wib-inv(T)*(wepp+wiep);
    
    temp = [ 0    -wpbb(1)  -wpbb(2) -wpbb(3) 
           wpbb(1)   0      wpbb(3) -wpbb(2)
           wpbb(2) -wpbb(3)  0       wpbb(1) 
           wpbb(3)  wpbb(2) -wpbb(1)    0   ];
        
  f_q = temp*q/2.0;  %四元数的更新（一阶）
  q = q + f_q*h;       
          
  T(1,1) = q(1)*q(1)+q(2)*q(2)-q(3)*q(3)-q(4)*q(4); %求解姿态矩阵
  T(1,2) = 2*(q(2)*q(3)-q(1)*q(4));
  T(1,3) = 2*(q(2)*q(4)+q(1)*q(3));

  T(2,1) =  2*(q(2)*q(3)+q(1)*q(4));
  T(2,2) =  q(1)*q(1)-q(2)*q(2)+q(3)*q(3)-q(4)*q(4);
  T(2,3) =  2*(q(3)*q(4)-q(1)*q(2));

  T(3,1) = 2*(q(2)*q(4)-q(1)*q(3)); 
  T(3,2) = 2*(q(3)*q(4)+q(1)*q(2));
  T(3,3) = q(1)*q(1)-q(2)*q(2)-q(3)*q(3)+q(4)*q(4);
   
    fp = T*fb;                                      %比力转换
   
     
   Rxp = Re*(1 + e*sin(phi)*sin(phi));         %RN 卯酉圈半径 
   Ryp = Re*(1 - 2*e + 3*e*sin(phi)*sin(phi)); %RM	子午圈半径
      
        
   fv(1) = fp(1)+(2*wiep(3) + wepp(3))*v(2);     % 速度更新
   fv(2) = fp(2)-(2*wiep(3) + wepp(3))*v(1);
       
   v(1) = fv(1)*h + v(1);
   v(2) = fv(2)*h + v(2);
   
   ve(i) = v(1);
   vn(i) = v(2);
   
    wepp(1) = -v(2)/Ryp;                         %位置速率的计算
    wepp(2) =  v(1)/Rxp;
    wepp(3) =  v(1)*tan(phi)/Rxp;

   
   %sys_w = [  0        wepp(3)  -wepp(2)
   %         -wepp(3)      0      wepp(1)
   %         wepp(2)    -wepp(1)       0 ];
   %f_c = sys_w*c;
         
   
   %c = c + f_c*h;
   
  %wiep(1) = wie*c(1,3);
  %wiep(2) = wie*c(2,3);
  %wiep(3) = wie*c(3,3);
  
     
  %  position(1) = asin(c(3,3))/Rad_Deg;   %纬度
  %  position(2) = atan(c(3,2)/c(3,1))/Rad_Deg;  %经度
  %  position(3) = atan(c(1,3)/c(2,3))/Rad_Deg; %游动方位角
   
   phi = phi + h*v(2)/Ryp;                         %位置计算
   lamda = lamda +h*v(1)/(Rxp*cos(phi));  
  
   latitude(i) = (phi/Rad_Deg - 45.7796)*60;
   longtitude(i) = (lamda/Rad_Deg - 126.6705 )*60;
   
   wiep(1) = 0;                                    %地球速率的计算
	wiep(2) = wie*cos(phi);
	wiep(3) = wie*sin(phi);
   
   attitude(1) = asin(T(3,2))/Rad_Deg;             %姿态提取   
    attitude(2) = atan(-T(3,1)/T(3,3))/Rad_Deg;
    attitude(3) = atan(T(1,2)/T(2,2))/Rad_Deg;
    
    pitch(i) = attitude(1)*60;
    roll(i) = attitude(2)*60;
    yaw(i) =  attitude(3)*60;
    
   
   g = g0+0.051799*sin(phi)*sin(phi);  %gravity model
   %q = q/sqrt(q(1)*q(1)+q(2)*q(2)+q(3)*q(3)+q(4)*q(4)); % Normal
      
    delta=1.0-(q(1)*q(1)+q(2)*q(2)+q(3)*q(3)+q(4)*q(4));
    if abs(delta)>DeltaQuat
        q=q*(1+delta/2.0);
    end    
   
   
   
 end
 
 k = 1:1:86400*2/h;
  subplot(4,2,1)
  plot(k,roll(k),'r')
  subplot(4,2,2)
  plot(k,pitch(k),'r')
  subplot(4,2,3)
  plot(k,yaw(k),'r')
  subplot(4,2,4)
  plot(k,ve(k),'r')
  subplot(4,2,5)
  plot(k,vn(k),'r')
  subplot(4,2,6)
  plot(k,longtitude(k),'r')
  subplot(4,2,7)
  plot(k,latitude(k),'r')