halorithm3.m

组合导航的程序

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

  clear all;
  wie = 7.2921158e-5;
  Re = 6378393.0; %Re = 6378137.0
  Rad_Deg = 0.01745329;
  e = 3.367e-3; 
  g0 = 9.78049;
  Lati = 45.7796*Rad_Deg;
  G_Drift = 0.05*Rad_Deg/3600.0;%陀螺漂移和加速度零偏  
  A_bias = 1e-4; 
  
  TimeS1 = 30;%对准时间（可由仿真决定）
  TimeS2 = 90;
  TimeS3 = 300;

   mmm = 0; %测试变量
   nnn = 0;
   Kgx = 0;%0.001;
   Kgy = 0;%0.001;
   Kgz = 0;%0.001;
   
   Kax = 0;%0.001;
	Kay = 0;%0.001;
	Kaz = 0;%0.001; 
   
   dvx = 0.0;
   dvy = 0.0;
   tempx = 0.0;
   tempy = 0.0;
   Kb = 0.0;
   Kk = 0.0;
   boAlign = 1;
   
  pitch0 = 0;
  yaw0 = 0;
  roll0 = 0;  %初始位置 东北天
 
  T_p = 8;          %晃动周期导致扰动加速度对对准精度的影响？ 
  T_r = 10;
  T_y = 6;
  yawK = 0;
   
  phi = 45.7796*Rad_Deg;     %运算中全为弧度
  lamda = 126.6705*Rad_Deg;
    
 roll_m = 15*Rad_Deg;  %15*Rad_Deg;  %晃动幅值大会导致解析粗对准的失败？（化弧）
 pitch_m = 12*Rad_Deg; %12*Rad_Deg;
 yaw_m = 10*Rad_Deg;   %10*Rad_Deg;
 
 pitch0 = 2;
 roll0 = 2;
 yaw0 = 30;
 
 
 g = g0 + 0.051799*sin(phi)*sin(phi);  
 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]; 
   
hn = 0.1;    %采样周期为hn/2，四元数的更新周期
Tn = (TimeS1+TimeS2+TimeS3+50)/hn;
tmp = 1;   %取点数(次数少时，tmp=1)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%开始仿真
%%%%%

for k = 1:Tn  %时钟基准0时刻开始/k 表示四元数的第k次更新  仿真时间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的函数
 disturb(i) = 0.01*sin( 2*pi*(k-(3-i)/2)*hn/T_p);  %加速度计的干扰
  
	                                                 
 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


%rollT = [3*Rad_Deg; 3*Rad_Deg; 3*Rad_Deg];  
%pitchT = [3*Rad_Deg; 3*Rad_Deg; 3*Rad_Deg];  
%yawT =   [30*Rad_Deg; 30*Rad_Deg; 30*Rad_Deg];  

%wpT = [0; 0; 0]; 
%wrT = [0; 0; 0];
%wyT = [0; 0; 0];
 
 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;
	wien(2) = wie*cos(Lati);   %Lati != phi
	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 + disturb(1);
	fb1(3) = (1 + Kaz)*fb1T(3) + A_bias;
   
   fb2(1) = (1 + Kax)*fb2T(1) + A_bias;
	fb2(2) = (1 + Kay)*fb2T(2) + A_bias + disturb(2);
	fb2(3) = (1 + Kaz)*fb2T(3) + A_bias;
   
   fb3(1) = (1 + Kax)*fb3T(1) + A_bias;
	fb3(2) = (1 + Kay)*fb3T(2) + A_bias + disturb(3);
   fb3(3) = (1 + Kaz)*fb3T(3) + A_bias;
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%以上为惯性器件仿真器
   
   if boAlign == 1
     wcp =[ -Kb*dvy;
             Kb*dvx;
              0.0];
   else
     wcp = [0.0;
            0.0;
            0.0];
   end
        
   wpbb1 = wib1'-inv(T)*(wepp+wiep+wcp);
   wpbb2 = wib2'-inv(T)*(wepp+wiep+wcp);
   wpbb3 = wib3'-inv(T)*(wepp+wiep+wcp);
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Kb，Kk 对准参数设置
   if k <= (TimeS1+TimeS2)/hn       %强阻尼
     Kb = (1.414/5)*(1.414/5)/g; %10;
     Kk =  2*0.707*(1.414/5);    %14;
    else
     Kb = (1.414/80)*(1.414/80)/g;  %3
     Kk =  2*0.707*(1.414/80);      %6
    end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%(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;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 方位粗修正
 if  k == (TimeS1+TimeS2)/hn
	  tempx = tempx/(TimeS2/hn);
	  tempy = tempy/(TimeS2/hn);
     phiz = atan(tempx/(tempy+wiep(2)));
     
      qc(1) = q(1)*cos(phiz/2.0) - q(4)*sin(phiz/2.0) ;
	   qc(2) = q(2)*cos(phiz/2.0) - q(3)*sin(phiz/2.0) ;
	   qc(3) = q(3)*cos(phiz/2.0) + q(2)*sin(phiz/2.0) ;
      qc(4) = q(4)*cos(phiz/2.0) + q(1)*sin(phiz/2.0) ;
		q = qc';
      
       tempx = 0.0;
       tempy = 0.0;
   end    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 方位精修正
   if k> (TimeS1+TimeS2)/hn & k <= (TimeS1+TimeS2+TimeS3)/hn
   	
      tempx = tempx + wcp(1); 
      nnn = nnn+1;
 
	if k == (TimeS1+TimeS2+TimeS3)/hn
      tempx = tempx/(TimeS3/hn);
      dphiz = tempx/wiep(2);
	    
	   qc(1) = q(1)*cos(dphiz/2.0) - q(4)*sin(dphiz/2.0) ;
	   qc(2) = q(2)*cos(dphiz/2.0) - q(3)*sin(dphiz/2.0) ;
	   qc(3) = q(3)*cos(dphiz/2.0) + q(2)*sin(dphiz/2.0) ;
      qc(4) = q(4)*cos(dphiz/2.0) + q(1)*sin(dphiz/2.0) ;
      q = qc';
       boAlign = 0;
    end
   end 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
  q = q/sqrt(q(1)*q(1)+q(2)*q(2)+q(3)*q(3)+q(4)*q(4)); % Normal
  
  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';                                      %比力转换
   
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 水平控制

  if k<(TimeS1+TimeS2+TimeS3)/hn              
   f_dvx = fp(1) - Kk*dvx;
   f_dvy = fp(2) - Kk*dvy;
   
   dvx = dvx + f_dvx*hn;
   dvy = dvy + f_dvy*hn;
  end
   
   if k>= TimeS1/hn & k< (TimeS1+TimeS2)/hn   %方位粗估计
        tempx = tempx + wcp(1);
        tempy = tempy + wcp(2);
        mmm = mmm+1; 
   end
   
   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);
   
   Kv1 = fv';
   tmp_v(1) = v(1) + fv(1)*hn;
   tmp_v(2) = v(2) + fv(2)*hn;
   
   fv(1) = fp(1)+(2*wiep(3) + wepp(3))*tmp_v(2);     
   fv(2) = fp(2)-(2*wiep(3) + wepp(3))*tmp_v(1);
    
   Kv2 = fv';
   
   Kv = (Kv1+Kv2)/2.0;
  
   
    v = v + Kv*hn;
        
    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));  
       
   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
           
    if mod(k,tmp)==0
    
    pitch(k/tmp) = attitude(1);
    roll(k/tmp) = attitude(2);
    yaw(k/tmp) =  attitude(3);
    
    attpT(k/tmp) = pitchT(3)/Rad_Deg;  %Ture
    attrT(k/tmp) = rollT(3)/Rad_Deg;
    attyT(k/tmp) = yawT(3)/Rad_Deg;
     
    dp(k/tmp) = pitch(k/tmp) - attpT(k/tmp);
    dr(k/tmp) = roll(k/tmp) - attrT(k/tmp);
    dy(k/tmp) = yaw(k/tmp) -  attyT(k/tmp);  
        
    ve(k/tmp) = v(1);
    vn(k/tmp) = v(2);
    
    latitude(k/tmp) = phi/Rad_Deg - 45.7796;
    longtitude(k/tmp) = lamda/Rad_Deg - 126.6705 ;
    
    end
   
 end
 
 i = 1:Tn/tmp;
  subplot(4,2,1)
  plot(i*hn*tmp,dp(i),'r')
  subplot(4,2,2)
  plot(i*hn*tmp,dr(i),'r')
  subplot(4,2,3)
  plot(i*hn*tmp,dy(i),'r')
  subplot(4,2,4)
  plot(i*hn*tmp,ve(i),'r')
  subplot(4,2,5)
  plot(i*hn*tmp,vn(i),'r')
  subplot(4,2,6)
  plot(i*hn*tmp,latitude(i),'r')
  subplot(4,2,7)
  plot(i*hn*tmp,longtitude(i),'r')