hangrg2.m

组合导航的程序

 Rad_Deg = pi/180.0;

  pitch0 = 0;
  yaw0 = 0;
  roll0 = 0;  %初始位置 东北天
  
  T_p = 8;
  T_r = 10;
  T_y =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，四元数的更新周期
 Tn = 2000;
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
 
% 四阶龙格库塔法 
 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;

  %delx = wnbb(1);   %又有专门针对角增量的算法
  %dely = wnbb(2);
  %delz = wnbb(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);
  
  %y(1) = p(1)*q(1)-p(2)*q(2)-p(3)*q(3)-p(4)*q(4);% 对的
  %y(2) = p(1)*q(2)+p(2)*q(1)+p(3)*q(4)-p(4)*q(3);
  %y(3) = p(1)*q(3)+p(3)*q(1)+p(4)*q(2)-p(2)*q(4);
  %y(4) = p(1)*q(4)+p(4)*q(1)+p(2)*q(3)-p(3)*q(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,dp(i),'r')
  subplot(3,2,2)
  plot(i,dr(i),'r')
  subplot(3,2,3)
  plot(i,dy(i),'r')