halorithm2.m

组合导航的程序

% 我要实现我伟大的想法！
% 巧妙的设计仿真环境！
% 此程序用来设计和验证姿态算法。%该算法采用 (仿真器) 的输出（导航解算参与其中）

  clear all;
  %常值区
  wie = 7.2921158e-5;
  Re = 6378393.0; %Re = 6378137.0
  e = 3.367e-3; 
  Rad_Deg = pi/180.0;
  Lati = 45.7796*Rad_Deg; 
  G_Drift = 0.0; %0.1*Rad_Deg/3600.0;%陀螺漂移和加速度零偏  
  A_bias = 0.0;  %1e-4*g; 
  
   Kgx = 0;%0.001;
   Kgy = 0;%0.001;
   Kgz = 0;%0.001;
   
   Kax = 0;%0.001;
	Kay = 0;%0.001;
	Kaz = 0;%0.001; 
  
  %赋初值
  phi = 45.7796*Rad_Deg;
  lamda = 126.6705*Rad_Deg;
  g0 = 9.78049; 
   
  pitch0 = 0;
  yaw0 = 0;
  roll0 = 0;  %初始位置 东北天
  g = g0 + 0.051799*sin(phi)*sin(phi);  
    
  T_p = 4;%8;          %晃动周期导致扰动加速度对对准精度的影响？ 
  T_r = 5;%10;
  T_y = 3;%6;
  yawK = 0;
    
 roll_m =  15*Rad_Deg;  %晃动幅值大会导致解析粗对准的失败？（化弧）
 pitch_m = 12*Rad_Deg;
 yaw_m =  10*Rad_Deg;
 
 wepp = [0.0 ;  0.0;  0.0];
 wiep = [0.0 ;  wie*cos(phi);  wie*sin(phi)];
 v = [0.0 ; 0.0];
 
 q = [1  
      0 
      0 
      0];
 T = [1  0   0
      0  1   0
      0  0   1]; 
   
%    pitchT = 0;pitch_m;
%    rollT = roll_m;
%    yawT = yaw_m;
%   
%    T(1,1) = cos(rollT)*cos(yawT)+sin(rollT)*sin(pitchT)*sin(yawT); 
% 	T(2,1) = -cos(rollT)*sin(yawT)+sin(rollT)*sin(pitchT)*cos(yawT);
% 	T(3,1) = -sin(rollT)*cos(pitchT);
% 	
% 	T(1,2) = cos(pitchT)*sin(yawT);
% 	T(2,2) = cos(pitchT)*cos(yawT);
% 	T(3,2) = sin(pitchT);
% 	
% 	T(1,3) = sin(rollT)*cos(yawT)-cos(rollT)*sin(pitchT)*sin(yawT);
% 	T(2,3) = -sin(rollT)*sin(yawT)-cos(rollT)*sin(pitchT)*cos(yawT);
% 	T(3,3) = cos(rollT)*cos(pitchT);  
%    
%    q(1) =  sqrt(1 + T(1,1) + T(2,2) + T(3,3))/2.0; 
%     q(2) =  (T(3,2)-T(2,3)) / (4*q(1));
%     q(3) =  (T(1,3)-T(3,1)) / (4*q(1));
%     q(4) =  (T(2,1)-T(1,2)) / (4*q(1));
	        
    
   
hn = 0.01;   %采样周期为hn/2，四元数的更新周期
Tn = 30/hn;

for k = 1:Tn  %时钟基准0时刻开始/k 表示四元数的第k次更新  仿真时间t = Tn*hn;
   
 q0 = q; 
 for i = 1:3 %获取龙格库塔法的采样值wpb 和 理论值pitchT，rollT，yawT。 
 pitchT(i) = pitch_m*sin( 2*pi*(k-(3-i)/2)*hn/T_p + pitch0*Rad_Deg); %真实姿态角 TRUE
 rollT(i) =  roll_m*sin( 2*pi*(k-(3-i)/2)*hn/T_r + roll0*Rad_Deg);
 yawT(i) =   yaw_m*sin( 2*pi*(k-(3-i)/2)*hn/T_y + yaw0*Rad_Deg) + yawK; %时间t的函数
	                                                     
 wpT(i) = 2*pitch_m* cos( 2*pi*(k-(3-i)/2)*hn/T_p+pitch0*Rad_Deg )*pi/T_p; %真实姿态角速度  
 wrT(i) = 2*roll_m* cos( 2*pi*(k-(3-i)/2)*hn/T_r+roll0*Rad_Deg )*pi/T_r;   
 wyT(i) = 2*yaw_m* cos( 2*pi*(k-(3-i)/2)*hn/T_y+yaw0*Rad_Deg )*pi/T_y;    %时间t的函数
 end
 
 for i = 1:3
    
 wnbb(i,1) =  sin(rollT(i))*cos(pitchT(i))*wyT(i) + cos(rollT(i))*wpT(i);
 wnbb(i,2) = -sin(pitchT(i))*wyT(i) + wrT(i);
 wnbb(i,3) = -cos(rollT(i))*cos(pitchT(i))*wyT(i) + sin(rollT(i))*wpT(i);
 end

 for i = 1:3  
    wnbb1(i) = wnbb(1,i);      %算法中所需的三点采样值
    wnbb2(i) = wnbb(2,i);
    wnbb3(i) = wnbb(3,i);
 end
  
	wien(1) = 0;              %此处Lati用phi替代 ＝ 相当于引入逐日漂移
	wien(2) = wie*cos(Lati);
	wien(3) = wie*sin(Lati);
   
   wenn(1) = 0;
	wenn(2) = 0;
	wenn(3) = 0;
   winn = wien + wenn;
  
   %获取方向余弦矩阵Cnb1,Cnb2,Cnb3
   for i = 1:3  
   Cnb11(i) = cos(rollT(i))*cos(yawT(i))+sin(rollT(i))*sin(pitchT(i))*sin(yawT(i)); 
	Cnb12(i) = -cos(rollT(i))*sin(yawT(i))+sin(rollT(i))*sin(pitchT(i))*cos(yawT(i));
	Cnb13(i) = -sin(rollT(i))*cos(pitchT(i));
	
	Cnb21(i) = cos(pitchT(i))*sin(yawT(i));
	Cnb22(i) = cos(pitchT(i))*cos(yawT(i));
	Cnb23(i) = sin(pitchT(i));
	
	Cnb31(i) = sin(rollT(i))*cos(yawT(i))-cos(rollT(i))*sin(pitchT(i))*sin(yawT(i));
	Cnb32(i) = -sin(rollT(i))*sin(yawT(i))-cos(rollT(i))*sin(pitchT(i))*cos(yawT(i));
   Cnb33(i) = cos(rollT(i))*cos(pitchT(i));
   end   
   
   Tnb1 = [Cnb11(1)  Cnb12(1)  Cnb13(1)   %动坐标系与定坐标系间的关系
           Cnb21(1)  Cnb22(1)  Cnb23(1) 
           Cnb31(1)  Cnb32(1)  Cnb33(1)];
        
   Tnb2 = [Cnb11(2)  Cnb12(2)  Cnb13(2) 
           Cnb21(2)  Cnb22(2)  Cnb23(2) 
           Cnb31(2)  Cnb32(2)  Cnb33(2)];
        
   Tnb3 = [Cnb11(3)  Cnb12(3)  Cnb13(3) 
           Cnb21(3)  Cnb22(3)  Cnb23(3) 
           Cnb31(3)  Cnb32(3)  Cnb33(3)];
   
   winb1 = Tnb1*winn';
   winb2 = Tnb2*winn';
   winb3 = Tnb3*winn';
      
   wib1T = winb1 + wnbb1'; %陀螺的无误差输出(t, t+ hn/2 , t+hn)
   wib2T = winb2 + wnbb2';
   wib3T = winb3 + wnbb3';
  
  
	wib1(1) = (1 + Kgx)*wib1T(1) + G_Drift; %仿真陀螺仪输出 （t ， t+hn/2,  t+hn）
	wib1(2) = (1 + Kgy)*wib1T(2) + G_Drift;
	wib1(3) = (1 + Kgz)*wib1T(3) + G_Drift;
   
   wib2(1) = (1 + Kgx)*wib2T(1) + G_Drift;
	wib2(2) = (1 + Kgy)*wib2T(2) + G_Drift;
	wib2(3) = (1 + Kgz)*wib2T(3) + G_Drift;
   
   wib3(1) = (1 + Kgx)*wib3T(1) + G_Drift;
	wib3(2) = (1 + Kgy)*wib3T(2) + G_Drift;
	wib3(3) = (1 + Kgz)*wib3T(3) + G_Drift;

	fn = [0  0 g]';
   
   fb1T = Tnb1*fn;
   fb2T = Tnb2*fn;
   fb3T = Tnb3*fn;

   fb1(1) = (1 + Kax)*fb1T(1) + A_bias;%仿真加速度计的输出（t ， t+hn/2,  t+hn）
	fb1(2) = (1 + Kay)*fb1T(2) + A_bias;
	fb1(3) = (1 + Kaz)*fb1T(3) + A_bias;
   
   fb2(1) = (1 + Kax)*fb2T(1) + A_bias;
	fb2(2) = (1 + Kay)*fb2T(2) + A_bias;
	fb2(3) = (1 + Kaz)*fb2T(3) + A_bias;
   
   fb3(1) = (1 + Kax)*fb3T(1) + A_bias;
	fb3(2) = (1 + Kay)*fb3T(2) + A_bias;
   fb3(3) = (1 + Kaz)*fb3T(3) + A_bias;
   
   
   wpbb1 = wib1'-inv(T)*(wepp+wiep);
   wpbb2 = wib2'-inv(T)*(wepp+wiep);
   wpbb3 = wib3'-inv(T)*(wepp+wiep);

   
    
 %(1) 四阶龙格库塔法 (针对输出为角速率的算法)
 sysW1 =[  0           -wpbb1(1)  -wpbb1(2)  -wpbb1(3)
           wpbb1(1)     0         wpbb1(3)  -wpbb1(2)
           wpbb1(2)  -wpbb1(3)     0         wpbb1(1)
           wpbb1(3)   wpbb1(2)  -wpbb1(1)     0      ];
        
 sysW2 =[  0           -wpbb2(1)  -wpbb2(2)  -wpbb2(3)
           wpbb2(1)     0         wpbb2(3)  -wpbb2(2)
           wpbb2(2)  -wpbb2(3)     0         wpbb2(1)
           wpbb2(3)   wpbb2(2)  -wpbb2(1)     0      ];

sysW3 =[   0           -wpbb3(1)  -wpbb3(2)  -wpbb3(3)
           wpbb3(1)     0         wpbb3(3)  -wpbb3(2)
           wpbb3(2)  -wpbb3(3)     0         wpbb3(1)
           wpbb3(3)   wpbb3(2)  -wpbb3(1)     0      ];
     
 K1 = sysW1*q0/2.0; %四维
 q0 = q + hn*K1/2.0;
 
 K2 = sysW2*q0/2.0;  
 q0 = q + hn*K2/2.0; 
 
 K3 = sysW2*q0/2.0;  
 
 q0 = q + hn*K3; 
 
 K4 = sysW3*q0/2.0;  
 
% K = (K1 + 2*K2 + 2*K3 + K4)/6.0;
% q = q + K*hn;
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
%     delx = (wnbb1(1)+4*wnbb2(1)+wnbb3(1))*hn/6; %(2.1)辛普森积分法 （又有专门针对角增量的算法）
%     dely = (wnbb1(2)+4*wnbb2(2)+wnbb3(2))*hn/6;
%     delz = (wnbb1(3)+4*wnbb2(3)+wnbb3(3))*hn/6;
    
    del1 = (wpbb1 + wpbb2)*hn/4;   %又有专门针对角增量的算法
    del2 = (wpbb2 + wpbb3)*hn/4; 
    Sumdel = (wpbb1 + 4*wpbb2 + wpbb3)*hn/6.0;
    
     A = [   0        -del1(3)     del1(2)
            del1(3)     0         -del1(1)
           -del1(2)   del1(1)     0       ] ;
         
     B = [del2(1) del2(2)  del2(3)]';
     
   delb = A*B;   
     
   % Newdel = del1 + del2 + 2*delb/3.0;
     Newdel = Sumdel + 2*delb/3.0;   %good
    
    delx = Newdel(1);
    dely = Newdel(2);
    delz = Newdel(3);


    del = sqrt(delx*delx+dely*dely+delz*delz); %平方项
    Ac = cos(del/2); %1- del/8 + del*del/384;  
    As = sin(del/2)/del; %0.5- del/48 + del*del/3840;    % delta = 0   如何处理？
  
    p(1) = Ac;        %四元数的变化量
    p(2) = As*delx; 
    p(3) = As*dely;
    p(4) = As*delz;
  
     y(1) = q(1)*p(1)-q(2)*p(2)-q(3)*p(3)-q(4)*p(4);
     y(2) = q(1)*p(2)+q(2)*p(1)+q(3)*p(4)-q(4)*p(3);
     y(3) = q(1)*p(3)+q(3)*p(1)+q(4)*p(2)-q(2)*p(4);
     y(4) = q(1)*p(4)+q(4)*p(1)+q(2)*p(3)-q(3)*p(2);
   
    for j =1:4
      q(j) = y(j);
    end

 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
  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*fb3';                                      %比力转换
   
     
   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)*hn + v(1);
   v(2) = fv(2)*hn + v(2);
   
   ve(k) = v(1);
   vn(k) = v(2);
   
    wepp(1) = -v(2)/Ryp;                         %位置速率的计算
    wepp(2) =  v(1)/Rxp;
    wepp(3) =  v(1)*tan(phi)/Rxp;

     
   phi = phi + hn*v(2)/Ryp;                         %位置计算
   lamda = lamda + hn*v(1)/(Rxp*cos(phi));  
  
   latitude(k) = (phi/Rad_Deg - 45.7796)*60;
   longtitude(k) = (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;     %右手反螺旋
    
    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
        
    pitch(k) = attitude(1);
    roll(k) = attitude(2);
    yaw(k) =  attitude(3);
    
    attpT(k) = pitchT(3)/Rad_Deg;  %Ture
    attrT(k) = rollT(3)/Rad_Deg;
    attyT(k) = yawT(3)/Rad_Deg;
     
    dp(k) = pitch(k) - attpT(k);
    dr(k) = roll(k)- attrT(k);
    dy(k) = yaw(k)- attyT(k);    
     
   
 end
 
 i = 1:Tn;
  subplot(4,2,1)
  plot(i*hn,dp(i),'r')
  subplot(4,2,2)
  plot(i*hn,dr(i),'r')
  subplot(4,2,3)
  plot(i*hn,dy(i),'r')
  subplot(4,2,4)
  plot(i*hn,ve(i),'r')
  subplot(4,2,5)
  plot(i*hn,vn(i),'r')
  subplot(4,2,6)
  plot(i*hn,latitude(i),'r')
  subplot(4,2,7)
  plot(i*hn,longtitude(i),'r')