angle1.m

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

close all;clear all;
clc;
Rad_Deg = 0.01745329;
Re  = 6378393.0;
WIE = 7.2921158e-5;
Lat = 45.7796*Rad_Deg;
Lon = 126.6705*Rad_Deg;
e   = 3.367e-3;
g0  = 9.78049+30*1e-5*9.78049;
hn=0.1;
fid=fopen('dat.txt','w');
fp=fopen('data.txt','w');
%信号发生
%inertial sensor quality factor  
  G_Drift=0.01*Rad_Deg/3600.0;
  A_bias =1e-4*g0; 
  Gm_Drift = [ G_Drift; G_Drift; G_Drift];  
  Am_bias  = [ A_bias;  A_bias;  A_bias ];
   
  Kgx = 0.0001;Kgy = 0.0001; Kgz = 0.0001;
  
  Kgro=[1+Kgx    0        0;
        0        1+Kgy    0;
        0        0        1+Kgz];
    
  Kax = 0.0001;Kay = 0.0001;Kaz = 0.0001;
  Kacc=[1+Kax    0        0;
        0        1+Kay    0;
        0        0        1+Kaz];                               
%constant misalignment
mx=1*Rad_Deg;
my=2*Rad_Deg;
mz=-3*Rad_Deg;
%dynamic bending
betx=2.146/2;
bety=2.146/2;
betz=2.146/2;

bx=0.0*Rad_Deg;
by=0.0*Rad_Deg;
bz=0.0*Rad_Deg;
%dynamic bending derivative
dx=0*Rad_Deg;
dy=0*Rad_Deg;
dz=0*Rad_Deg;
ddx=0*Rad_Deg;
ddy=0*Rad_Deg;
ddz=0*Rad_Deg;
%cycle of swaying
T_p = 8;  T_r = 10;  T_y = 6;
pitch0  = 30.0*Rad_Deg;  roll0  = 30.0*Rad_Deg;  yaw0  = 30.0*Rad_Deg; %初始相位
pitchK  =  0.0*Rad_Deg;  rollK  = 0.0*Rad_Deg;   yawK  = 90.0*Rad_Deg; %摇摆中心  
pitch_m =  2.0*Rad_Deg;  roll_m = 3.0*Rad_Deg;   yaw_m = 5.0*Rad_Deg;  %摇摆幅度
%velocity acceleration etc
v=[0;
   0;
   0];
a=[0;
   0;
   0];
Tn=1000/hn;
for k=1:Tn;
 pitchT = pitch_m  *sin( 2*pi*k*hn/T_p + pitch0) + pitchK; 
 rollT  = roll_m   *sin( 2*pi*k*hn/T_r + roll0 ) + rollK;
 yawT   = yaw_m    *sin( 2*pi*k*hn/T_y + yaw0  ) + yawK; 
 wpT    = 2*pitch_m*cos( 2*pi*k*hn/T_p + pitch0)*pi/T_p;  
 wrT    = 2*roll_m *cos( 2*pi*k*hn/T_r + roll0 )*pi/T_r;   
 wyT    = 2*yaw_m  *cos( 2*pi*k*hn/T_y + yaw0  )*pi/T_y;   
 wnbb(1,1) =  sin(rollT) *cos(pitchT)*wyT + cos(rollT)*wpT;
 wnbb(2,1) = -sin(pitchT)*wyT + wrT;
 wnbb(3,1) = -cos(rollT) *cos(pitchT)*wyT + sin(rollT)*wpT;
 
 Rxt = Re*(1+e*sin(Lat)*sin(Lat));       
 Ryt = Re*(1-2*e+3*e*sin(Lat)*sin(Lat)); 
 g1 = g0 + 0.051799*sin(Lat)*sin(Lat);
 
 v0 = v; v(1) = v(1)+a(1)*hn;  v(2) = v(2)+a(2)*hn;
 Lon = Lon+(v0(1)+v(1))/2.0*hn/(Rxt*cos(Lat)); Lat = Lat+(v0(2)+v(2))/2.0*hn/Ryt;
 wien=[0;
       WIE*cos(Lat);
       WIE*sin(Lat)];
       
 wenn =[ -v(2)/Ryt;
         v(1)/Rxt;
         v(1)*tan(Lat)/Rxt];
 winn = wien + wenn;
 
 Cnb(1,1) = cos(rollT)*cos(yawT)+sin(rollT)*sin(pitchT)*sin(yawT); 
 Cnb(1,2) =-cos(rollT)*sin(yawT)+sin(rollT)*sin(pitchT)*cos(yawT); 
 Cnb(1,3) =-sin(rollT)*cos(pitchT);
 Cnb(2,1) = cos(pitchT)*sin(yawT); 
 Cnb(2,2) = cos(pitchT)*cos(yawT);  
 Cnb(2,3) = sin(pitchT);
 Cnb(3,1) = sin(rollT)*cos(yawT)-cos(rollT)*sin(pitchT)*sin(yawT); 
 Cnb(3,2) =-sin(rollT)*sin(yawT)-cos(rollT)*sin(pitchT)*cos(yawT);
 Cnb(3,3) = cos(rollT)*cos(pitchT);
 
 
 Cbn=Cnb';
 winb = Cnb * winn;
 wibT = winb + wnbb;
 fn(1,1) = a(1)-(2*wien(3)+wenn(3))*v(2);
 fn(2,1) = a(2)+(2*wien(3)+wenn(3))*v(1); 
 fn(3,1) = a(3)-(2*wien(2)+wenn(2))*v(1)+(2*wien(1)+wenn(1))*v(2)+g1;
 fbT = Cnb*fn; 
     
 
 %slave IMU measurement
ddx=-betx*betx*bx-2*betx*dx+1e-2*randn(1);
ddy=-bety*bety*by-2*bety*dy+1e-2*randn(1);
ddz=-betz*betz*bz-2*betz*dz+1e-2*randn(1);
 dx=dx+ddx*hn;
 dy=dy+ddy*hn;
 dz=dz+ddz*hn;
 bx=bx+dx*hn;
 by=by+dy*hn;
 bz=bz+dz*hn;
 bx1(k)=bx;
 by1(k)=by;
 bz1(k)=bz;
 fprintf(fp,'%12.6f\t%12.6f\t%12.6f\n',bx,by,bz);
 Cns1(1,1) = cos(my+by)*cos(mz+bz)+sin(my+by)*sin(mx+bx)*sin(mz+bz); 
 Cns1(1,2) =-cos(my+by)*sin(mz+bz)+sin(my+by)*sin(mx+bx)*cos(mz+bz); 
 Cns1(1,3) =-sin(my+by)*cos(mx+bx);
 Cns1(2,1) = cos(mx+bx)*sin(mz+bz); 
 Cns1(2,2) = cos(mx+bx)*cos(mz+bz);  
 Cns1(2,3) = sin(mx+bx);
 Cns1(3,1) = sin(my+by)*cos(mz+bz)-cos(my+by)*sin(mx+bx)*sin(mz+bz); 
 Cns1(3,2) =-sin(my+by)*sin(mz+bz)-cos(my+by)*sin(mx+bx)*cos(mz+bz);
 Cns1(3,3) = cos(my+by)*cos(mx+bx);
 Cns=Cns1*Cnb;

%  Cns(1,1) = cos(rollT+my+by)*cos(yawT+mz+bz)+sin(rollT+my+by)*sin(pitchT+mx+bx)*sin(yawT+mz+bz); 
%  Cns(1,2) =-cos(rollT+my+by)*sin(yawT+mz+bz)+sin(rollT+my+by)*sin(pitchT+mx+bx)*cos(yawT+mz+bz); 
%  Cns(1,3) =-sin(rollT+my+by)*cos(pitchT+mx+bx);
%  Cns(2,1) = cos(pitchT+mx+bx)*sin(yawT+mz+bz); 
%  Cns(2,2) = cos(pitchT+mx+bx)*cos(yawT+mz+bz);  
%  Cns(2,3) = sin(pitchT+mx+bx);
%  Cns(3,1) = sin(rollT+my+by)*cos(yawT+mz+bz)-cos(rollT+my+by)*sin(pitchT+mx+bx)*sin(yawT+mz+bz); 
%  Cns(3,2) =-sin(rollT+my+by)*sin(yawT+mz+bz)-cos(rollT+my+by)*sin(pitchT+mx+bx)*cos(yawT+mz+bz);
%  Cns(3,3) = cos(rollT+my+by)*cos(pitchT+mx+bx);
 
 wnbbS(1,1) =  sin(rollT) *cos(pitchT)*wyT + cos(rollT)*wpT;
 wnbbS(2,1) = -sin(pitchT)*wyT + wrT;
 wnbbS(3,1) = -cos(rollT) *cos(pitchT)*wyT + sin(rollT)*wpT;
 
 mdx=[dx;
      dy;
      dz];
 wibsT=Cns*winn+Cns1*wnbbS+mdx;
 
 fbsT = Cns*fn;

  pitchB=asin(Cns(2,3));
  
  rollB=atan(-Cns(1,3)/Cns(3,3));
  if(Cns(3,3)<0);
      if(rollB<0)
          pitchB=pitchB+pi;
      else
          pitchB=pitchB-pi;
      end;
          
  end;
  yawB=-atan(-Cns(2,1)/Cns(2,2));
  if(Cns(2,2)<0);
      yawB=pi+yawB;
  end; 
  if(Cns(2,2)>0&yawB<0);
          yawB=2*pi+yawB;
  end;
 wibs=Kgro*wibsT+ Gm_Drift;
 fbs =Kacc*fbsT + Am_bias;
 fprintf(fid,'%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t',wibs(1),wibs(2),wibs(3),fbs(1),fbs(2),fbs(3),wibT(1),wibT(2),wibT(3),fbT(1),fbT(2),fbT(3));
 fprintf(fid,'%f\t%f\t%f\t%f\t%f\t%f\n',pitchT,rollT,yawT,pitchB,rollB,yawB);
 
 TT=Cns*inv(Cnb);
 
 ww1(k)=wibs(1)/Rad_Deg;
 ww2(k)=wibs(2)/Rad_Deg;
 ww3(k)=wibs(3)/Rad_Deg;
 ww4(k)=wibT(1)/Rad_Deg;
 ww5(k)=wibT(2)/Rad_Deg;
 ww6(k)=wibT(3)/Rad_Deg;
 
 vv1(k)=pitchT/Rad_Deg;
 vv2(k)=rollT/Rad_Deg;
 vv3(k)=yawT/Rad_Deg;
 vv4(k)=pitchB/Rad_Deg;
 vv5(k)=rollB/Rad_Deg;
 vv6(k)=yawB/Rad_Deg;
end;
fclose(fid);
fclose(fp);
t=1:Tn;

figure(1);
subplot(1,3,1);
plot(t,ww1,t,ww4);
subplot(1,3,2);
plot(t,ww2,t,ww5);
subplot(1,3,3);
plot(t,ww3,t,ww6);

figure(2);
subplot(1,3,1);
plot(t,vv1,t,vv4);

subplot(1,3,2);
plot(t,vv2,t,vv5);

subplot(1,3,3);
plot(t,vv3,t,vv6);