conical_motion .m

采用四元数法的标准圆锥运动的姿态角仿真程序

%%标准圆锥运动姿态角曲线：
alfa=1/57.3;
w=2*pi;
i=1;
for t=0:0.01:24
    tt(i)=i;
    Q=[cos(alfa/2);0;sin(alfa/2)*cos(w*t);sin(alfa/2)*sin(w*t)];
     %%%%%%%%%%四元数规范化%%%%%%%%%
      tmp_Q=sqrt(Q(1,1)^2+Q(2,1)^2+Q(3,1)^2+Q(4,1)^2);
      for kc=1:4
        Q(kc,1)=Q(kc,1)/tmp_Q;    
      end

      %%%%%%%%%%获取姿态矩阵%%%%%%%%%
      Cbn=[Q(2,1)^2+Q(1,1)^2-Q(4,1)^2-Q(3,1)^2, 2*(Q(2,1)*Q(3,1)+Q(1,1)*Q(4,1)), 2*(Q(2,1)*Q(4,1)-Q(1,1)*Q(3,1));
           2*(Q(2,1)*Q(3,1)-Q(1,1)*Q(4,1)), Q(3,1)^2-Q(4,1)^2+Q(1,1)^2-Q(2,1)^2,  2*(Q(3,1)*Q(4,1)+Q(1,1)*Q(2,1));
           2*(Q(2,1)*Q(4,1)+Q(1,1)*Q(3,1)), 2*(Q(3,1)*Q(4,1)-Q(1,1)*Q(2,1)), Q(4,1)^2-Q(3,1)^2-Q(2,1)^2+Q(1,1)^2]; 

      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      %求姿态(横滚、俯仰、航向）
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      attiN(1,1)=atan(-Cbn(1,3)/Cbn(3,3));
      attiN(2,1)=atan(Cbn(2,3)/sqrt(Cbn(2,1)*Cbn(2,1)+Cbn(2,2)*Cbn(2,2)));
      attiN(3,1)=atan(Cbn(2,1)/Cbn(2,2));
        %单位：弧度

      %象限判断
      attiN(1,1)=attiN(1,1)*180.0/pi;
      attiN(2,1)=attiN(2,1)*180.0/pi;
      attiN(3,1)=attiN(3,1)*180.0/pi;
        % 单位：度

      if(Cbn(2,2)<0 )    
       attiN(3,1)=180.0+attiN(3,1);
      else 
        if(Cbn(2,1)<0) attiN(3,1)=360.0+attiN(3,1); end
      end
        %航向角度（单位：度）
     


      if(Cbn(3,3)<0)
       if(Cbn(1,3)>0) attiN(1,1)=180.0-attiN(1,1); end
       if(Cbn(1,3)<0) attiN(1,1)=-(180.0+attiN(1,1)); end
      end
        %横滚角度（单位：度）
        attiN1(i,:)=attiN';
        i=i+1;
        
end
tt=tt';