halorithm1.m

组合导航的程序

% 我要实现我伟大的想法！！！
% 巧妙的设计仿真环境！！！
% 此程序用来设计和验证姿态算法。算法比较

  clear all;
  Rad_Deg = pi/180.0;
  latiT = 45.7796*Rad_Deg;
  wie = 7.2921158e-5;
  g = 9.807;
  
  pitch0 = 0;
  yaw0 = 0;
  roll0 = 0;  %初始位置 东北天
  
  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;
 q = [1  
      0 
      0 
      0];
   
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
   
 %(1) 四阶龙格库塔法 (针对输出为角速率的算法)
 sysW1 =[ 0           -wnbb(1,1)  -wnbb(1,2)  -wnbb(1,3)
           wnbb(1,1)     0         wnbb(1,3)  -wnbb(1,2)
           wnbb(1,2)  -wnbb(1,3)     0         wnbb(1,1)
           wnbb(1,3)   wnbb(1,2)  -wnbb(1,1)     0      ];
        
 sysW2 =[  0          -wnbb(2,1)  -wnbb(2,2)  -wnbb(2,3)
           wnbb(2,1)     0         wnbb(2,3)  -wnbb(2,2)
           wnbb(2,2)  -wnbb(2,3)     0         wnbb(2,1)
           wnbb(2,3)   wnbb(2,2)  -wnbb(2,1)     0      ];
        
 sysW3 =[ 0           -wnbb(3,1)  -wnbb(3,2)  -wnbb(3,3)
           wnbb(3,1)     0         wnbb(3,3)  -wnbb(3,2)
           wnbb(3,2)  -wnbb(3,3)     0         wnbb(3,1)
           wnbb(3,3)   wnbb(3,2)  -wnbb(3,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;
 
 %二阶算法
%  K1 = sysW1*q0/2.0; 
%  q0 = q + hn*K1;
%  K2 = sysW3*q0/2.0;
%  K = (K1+K2)/2.0;
%  q = q + K*hn;
 
   %(2)没有补偿的增量法，精度太差，进行圆锥补偿后，精度如何？ 
 
    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;
   
    %delx = (wnbb1(1) + wnbb3(1))*hn/2;   %（2.2）梯形积分法
    %dely = (wnbb1(2) + wnbb3(2))*hn/2;
    %delz = (wnbb1(3) + wnbb3(3))*hn/2;
    
    %delx = wnbb3(1)*hn; % wnbb3(1)   %（2.3）矩形积分法 对于输出为角速率其精度是远远不够的
     %dely =  wnbb3(2)*hn;  % wnbb3(2)
     %delz =  wnbb3(3)*hn;  % wnbb3(3)

    
    % del1 = (wnbb1 + wnbb2)*hn/4;   %又有专门针对角增量的算法
    % del2 = (wnbb2 + wnbb3)*hn/4; 
    % Sumdel = (wnbb1 + 4*wnbb2 + wnbb3)*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) = q(1)*q(1)+q(2)*q(2)-q(3)*q(3)-q(4)*q(4);
	T(2) = 2*(q(2)*q(3)-q(1)*q(4));
	T(3) = 2*(q(2)*q(4)+q(1)*q(3));

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

	T(7) = 2*(q(2)*q(4)-q(1)*q(3));
	T(8) = 2*(q(3)*q(4)+q(1)*q(2));
   T(9) = q(1)*q(1)-q(2)*q(2)-q(3)*q(3)+q(4)*q(4);
   
   attitude(1) = asin(T(8))/Rad_Deg;             %纵摇角(-90 ,90)
	attitude(2) = atan(-T(7)/T(9))/Rad_Deg;       %横摇角 (-90,90)
	attitude(3) = atan(T(2)/T(5))/Rad_Deg;        %航向角(0,360) 航向角 右手反螺旋
 
   attpT(k) = pitchT(3)/Rad_Deg;  %Ture
   attrT(k) = rollT(3)/Rad_Deg;
   attyT(k) = yawT(3)/Rad_Deg;
   
   attp(k) = attitude(1); %姿态解算值
   attr(k) = attitude(2);
   atty(k) = attitude(3);

   dp(k) = attpT(k)- attp(k);
   dr(k) = attrT(k)- attr(k);
   dy(k) = attyT(k)- atty(k);    
     
   
 end
 
 i = 1:Tn;
  subplot(3,2,1)
  plot(i*hn,dp(i),'r')
  subplot(3,2,2)
  plot(i*hn,dr(i),'r')
  subplot(3,2,3)
  plot(i*hn,dy(i),'r')