double_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;
h=0.02;
syms tao;

for t=0:0.01:24
    Wnbb=[-2*w*(sin(alfa/2))^2;-w*sin(alfa)*sin(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)];
     if t>0
         %一个迭代周期
         %一个周期内采样两次
         for ii=0:1        
             Wnbb2=Wnbb;
             Wnbb1=Wnbb2;  
             if ii==1
                     a=2*Wnbb1-Wnbb2;
                     b=2*(Wnbb2-Wnbb1)/h;
                     theta1=double(int(a+b*tao,0,0.02/2));
                     theta2=double(int(a+b*tao,0.02/2,0.02));
                     fai=theta1+theta2+cross(theta1,theta2);
                     theta0=sqrt(fai(1,1)^2+fai(2,1)^2+fai(3,1)^2);
                     faiX=[0,          -fai(1,1), -fai(2,1), -fai(3,1);
                         fai(1,1),  0,           fai(3,1), -fai(2,1);
                         fai(2,1), -fai(3,1),   0,          fai(1,1);
                         fai(3,1),  fai(2,1),  -fai(1,1),   0         ];
                     Q=(cos(theta0/2)*eye(4)+ sin(theta0/2)/theta0*faiX)*Q;%修正算法
                     %
             end
         end
         %
             %%%%%%%%%%四元数规范化%%%%%%%%%
          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
            %横滚角度（单位：度）
         
     end 
       attiN3(i,:)=attiN';
       i=i+1;
       pitch=attiN(2,1)*pi/180;
       roll=attiN(1,1)*pi/180;
       head=attiN(3,1)*pi/180;
end
     
%俯仰比较：     
figure(1);plot(tt,attiN1(:,2),'r',tt,attiN3(:,2),'g');grid;
%横滚比较：
figure(2);plot(tt,attiN1(:,1),'r',tt,attiN3(:,1),'g');grid;
%航向比较：
figure(3);plot(tt,attiN1(:,3),'r',tt,attiN3(:,3),'g');grid;