single_sample.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';
%**************************************************************************
%**************************************************************************


%%等效旋转矢量单子样算法求取标准圆锥运动姿态角：
alfa=1/57.3;
w=2*pi;
pitch=0;
head=0;
roll=0.1/57.3;
i=1;
T=0.01;
WnbbA_old=[0;0;0];

for t=0:0.01:24
    Wnbb=[-2*w*(sin(alfa/2))^2;-w*sin(alfa)*cos(w*t);w*sin(alfa)*cos(w*t)];
        %%%%%%%%%%%%%%四元数法求姿态矩阵%%%%%%%%%%%%%%%
      Q=[cos(head/2)*cos(pitch/2)*cos(roll/2)+sin(head/2)*sin(pitch/2)*sin(roll/2);
         cos(head/2)*sin(pitch/2)*cos(roll/2)+sin(head/2)*cos(pitch/2)*sin(roll/2);
         cos(head/2)*cos(pitch/2)*sin(roll/2)-sin(head/2)*sin(pitch/2)*cos(roll/2);
         -1.0*sin(head/2)*cos(pitch/2)*cos(roll/2)+cos(head/2)*sin(pitch/2)*sin(roll/2)];

      WnbbA=Wnbb*T;
      WnbbA0=sqrt(WnbbA(1,1)^2+WnbbA(2,1)^2+WnbbA(3,1)^2);

      WnbbX=[0,          -WnbbA(1,1), -WnbbA(2,1), -WnbbA(3,1);
             WnbbA(1,1),  0,           WnbbA(3,1), -WnbbA(2,1);
             WnbbA(2,1), -WnbbA(3,1),   0,          WnbbA(1,1);
             WnbbA(3,1),  WnbbA(2,1),  -WnbbA(1,1),   0         ];

      c_q=cos(WnbbA0/2);
      if( WnbbA0<=1.0e-15 ) d_q=0.5; else  d_q=sin(WnbbA0/2)/WnbbA0;  end

      %%%%%%%%%%%等效转动矢量修正算法%%%%%%%%%%%%%%%
      WnbbA_e=cross(WnbbA_old,WnbbA);
      WnbbX_e=[0,            -WnbbA_e(1,1), -WnbbA_e(2,1), -WnbbA_e(3,1);
               WnbbA_e(1,1),  0,             WnbbA_e(3,1), -WnbbA_e(2,1);
               WnbbA_e(2,1), -WnbbA_e(3,1),    0,           WnbbA_e(1,1);
               WnbbA_e(3,1),  WnbbA_e(2,1),  -WnbbA_e(1,1),   0         ];


      Q=( c_q*eye(4)+d_q*(WnbbX+1/12*WnbbX_e) )*Q;%修正算法

      WnbbA_old=WnbbA;

      %%%%%%%%%%四元数规范化%%%%%%%%%
      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
        %横滚角度（单位：度）
       attiN2(i,:)=attiN';
         pitch=attiN(2,1)*pi/180;
       roll=attiN(1,1)*pi/180;
       head=attiN(3,1)*pi/180;
            i=i+1;
end

%俯仰比较：     
figure(1);plot(tt,attiN1(:,2),'r',tt,attiN2(:,2),'g');grid;
%横滚比较：
figure(2);plot(tt,attiN1(:,1),'r',tt,attiN2(:,1),'g');grid;
%航向比较：
figure(3);plot(tt,attiN1(:,3),'r',tt,attiN2(:,3),'g');grid;