angle2.m

本人的呕心力作：角速度匹配传递对准matlab程序！绝对没有问题

    close all;
    clc;
    clear all;
    Rad_Deg = 0.01745329;
    hn=0.1;
    betx =2.146/2;
    bety=2.146/2;
    betz=2.146/2;
    Tn=1000/hn;
    fid=fopen('dat.txt','r');
    data=load('data.txt','r');
    %        Cnb(1,1) = cos(roll)*cos(yaw)+sin(roll)*sin(pitch)*sin(yaw); 
    %        Cnb(1,2) = -cos(roll)*sin(yaw)+sin(roll)*sin(pitch)*cos(yaw); 
    %        Cnb(1,3) = -sin(roll)*cos(pitch);
    % 	   Cnb(2,1) = cos(pitch)*sin(yaw); 
    %        Cnb(2,2) = cos(pitch)*cos(yaw);  
    %        Cnb(2,3) = sin(pitch);
    % 	   Cnb(3,1) = sin(roll)*cos(yaw)-cos(roll)*sin(pitch)*sin(yaw); 
    %        Cnb(3,2) = -sin(roll)*sin(yaw)-cos(roll)*sin(pitch)*cos(yaw);
    %        Cnb(3,3) = cos(roll)*cos(pitch);
    %        Tbn=Cnb';
    bx=data(:,1);
    by=data(:,2);
    bz=data(:,3);
    
    X=zeros(9,1);
    X(1)=2*Rad_Deg;
    X(2)=3*Rad_Deg;
    X(3)=-4*Rad_Deg;
    X(4)=0.4*Rad_Deg;
    X(5)=0.4*Rad_Deg;
    X(6)=0.4*Rad_Deg;
    X(7)=0;
    X(8)=0;
    X(9)=0;
    Q77=4*betx*betx*betx*1e-2*hn;
    Q88=4*bety*bety*bety*1e-2*hn;
    Q99=4*betz*betz*betz*1e-2*hn;
    Q=[0 0 0 0 0 0 0   0   0;
       0 0 0 0 0 0 0   0   0;
       0 0 0 0 0 0 0   0   0;
       0 0 0 0 0 0 0   0   0;
       0 0 0 0 0 0 0   0   0;
       0 0 0 0 0 0 0   0   0;
       0 0 0 0 0 0 Q77 0   0;
       0 0 0 0 0 0 0   Q88 0;
       0 0 0 0 0 0 0   0   Q99];
    R=[2*1e-3 0      0;
       0      2*1e-3 0;
       0      0      2*1e-3];

    P=[0.5     0     0     0     0     0     0     0     0;
        0     0.5     0     0     0     0     0     0     0;
        0     0     0.5     0     0     0     0     0     0;    
        0     0     0     0.5     0     0     0     0     0;  
        0     0     0     0     0.5     0     0     0     0;
        0     0     0     0     0     0.5     0     0     0;
        0     0     0     0     0     0     0.5     0     0;
        0     0     0     0     0     0     0     0.5     0;
        0     0     0     0     0     0     0     0     0.5];

     for k=1:Tn;
       raw=fscanf(fid,'%f',[18,1]);
     H=[0        raw(9)    -raw(8)     0        raw(9)     -raw(8)   0  0  0;
       -raw(9)    0        raw(7)     -raw(9)     0         raw(7)   0  0  0;
       raw(8)   -raw(7)      0        raw(8)    -raw(7)     0        0  0  0];
   
     A=[0   0    0    0          0         0           0        0      0;
        0   0    0    0          0         0           0        0      0;
        0   0    0    0          0         0           0        0      0;
        0   0    0    0          0         0           1        0      0;
        0   0    0    0          0         0           0        1      0;
        0   0    0    0          0         0           0        0      1;
        0   0    0    -betx*betx 0         0           -2*betx  0      0;
        0   0    0    0         -bety*bety 0           0       -2*bety 0;
        0   0    0    0          0         -betz*betz  0       0       -2*betz];
      B=zeros(9,9);
      B(7,7)=1;
      B(8,8)=1;
      B(9,9)=1;
      [F,G]=c2d(A,B,hn);
      F;
      G;
  
      FT = F';
      HT = H';
      BB=P*HT;
      Z=[raw(7)-raw(1);
      raw(8)-raw(2);
      raw(9)-raw(3)];
  
     z(k)=Z(1);
      XK = F*X;
      PK = F*P*FT+G*Q*G';
      K = PK*HT*inv(H*PK*HT+R);
      X = XK+K*(Z-H*XK);
      P = (eye(9)-K*H)*PK;
  
      pitchT=raw(13);
      rollT=raw(14);
      yawT=raw(15);
      pitchB=raw(16);
      rollB=raw(17);
      yawB=raw(18);
          Cnm(1,1) = cos(rollT)*cos(yawT)+sin(rollT)*sin(pitchT)*sin(yawT); 
          Cnm(1,2) = -cos(rollT)*sin(yawT)+sin(rollT)*sin(pitchT)*cos(yawT); 
          Cnm(1,3) = -sin(rollT)*cos(pitchT);
	      Cnm(2,1) = cos(pitchT)*sin(yawT); 
          Cnm(2,2) = cos(pitchT)*cos(yawT);  
          Cnm(2,3) = sin(pitchT);
	      Cnm(3,1) = sin(rollT)*cos(yawT)-cos(rollT)*sin(pitchT)*sin(yawT); 
          Cnm(3,2) = -sin(rollT)*sin(yawT)-cos(rollT)*sin(pitchT)*cos(yawT);
          Cnm(3,3) = cos(rollT)*cos(pitchT);
          

          Cms(1,1) = cos(X(2)+X(5))*cos(-X(3)-X(6))+sin(X(2)+X(5))*sin(X(1)+X(4))*sin(-X(3)-X(6)); 
          Cms(1,2) = -cos(X(2)+X(5))*sin(-X(3)-X(6))+sin(X(2)+X(5))*sin(X(1)+X(4))*cos(-X(3)-X(6)); 
          Cms(1,3) = -sin(X(2)+X(5))*cos(X(1)+X(4));
	      Cms(2,1) = cos(X(1)+X(4))*sin(-X(3)-X(6)); 
          Cms(2,2) = cos(X(1)+X(4))*cos(-X(3)-X(6));  
          Cms(2,3) = sin(X(1)+X(4));
	      Cms(3,1) = sin(X(2)+X(5))*cos(-X(3)-X(6))-cos(X(2)+X(5))*sin(X(1)+X(4))*sin(-X(3)-X(6)); 
          Cms(3,2) = -sin(X(2)+X(5))*sin(-X(3)-X(6))-cos(X(2)+X(5))*sin(X(1)+X(4))*cos(-X(3)-X(6));
          Cms(3,3) = cos(X(2)+X(5))*cos(X(1)+X(4));
          
          Cns = Cms*Cnm; 
            
          
          CnbT(1,1) = cos(rollB)*cos(yawB)+sin(rollB)*sin(pitchB)*sin(yawB); 
          CnbT(1,2) = -cos(rollB)*sin(yawB)+sin(rollB)*sin(pitchB)*cos(yawB); 
          CnbT(1,3) = -sin(rollB)*cos(pitchB);
	      CnbT(2,1) = cos(pitchB)*sin(yawB); 
          CnbT(2,2) = cos(pitchB)*cos(yawB);  
          CnbT(2,3) = sin(pitchB);
	      CnbT(3,1) = sin(rollB)*cos(yawB)-cos(rollB)*sin(pitchB)*sin(yawB); 
          CnbT(3,2) = -sin(rollB)*sin(yawB)-cos(rollB)*sin(pitchB)*cos(yawB);
          CnbT(3,3) = cos(rollB)*cos(pitchB);
          errC=CnbT*inv(Cns);
          
     data(k)=X(1)/Rad_Deg; 
     datb(k)=X(2)/Rad_Deg;
     datc(k)=X(3)/Rad_Deg;
     datd(k)=X(4)/Rad_Deg; 
     date(k)=X(5)/Rad_Deg;
     datf(k)=X(6)/Rad_Deg;
  
%   datg(k)=-errC(3,2)/Rad_Deg; 
%   dath(k)= errC(3,1)/Rad_Deg;
%   dati(k)=-errC(2,1)/Rad_Deg;
     datg(k)= errC(3,2)/Rad_Deg; %%by me
     dath(k)= errC(3,1)/Rad_Deg;
     dati(k)= errC(2,1)/Rad_Deg;
   
     end;
     fclose(fid);
     
     t=1:Tn;
     figure(1);
     plot(t,data(t));
     figure(2)
     plot(t,datb(t));
     figure(3)
     plot(t,datc(t));
     

%      figure(2);
%      subplot(1,3,1);
%      plot(t,datd);
%      subplot(1,3,2);
%      plot(t,date);
%      subplot(1,3,3);
%      plot(t,datf);
% 
     figure(4);
     plot(t,datg(t),'b',t,-bx(t)/Rad_Deg,'r');
     figure(5)
     plot(t,dath(t),'b',t,by(t)/Rad_Deg,'r');
     figure(6)
     plot(t,dati(t),'b',t,bz(t)/Rad_Deg,'r');