huangalign.m

组合导航的程序

% 我在为自己工作，我要实现我伟大的想法！！！
% 巧妙的设计仿真环境！！！

  clear all;
  Rad_Deg = pi/180.0;
  latiT = 45.7796*Rad_Deg;  %当地纬度
  wie = 7.2921158e-5;
  g = 9.807;
  
  pitch0 = 0;
  yaw0 = 0;
  roll0 = 0;  %初始位置 东北天
  
  G_Drift = 0.1*Rad_Deg/3600.0;  % IMU 实际指标 
  A_bias = 1e-4*g; 
  T_p = 8;               %晃动周期的影响？ 
  T_r = 10;
  T_y = 6;
  yawK = 0;
  
   Kgx = 0.001;
   Kgy = 0.001;
   Kgz = 0.001;
   
   Kax = 0.001;
	Kay = 0.001;
	Kaz = 0.001; 
  
 roll_m = 0.005*Rad_Deg;%15*Rad_Deg;  %化弧 晃动幅值对解析粗对准的影响？
 pitch_m = 0.002*Rad_Deg;%12*Rad_Deg;
 yaw_m = 0.003*Rad_Deg;%10*Rad_Deg;
 q = [1  
      0 
      0 
      0];
   
  fbR = 0.0;
  wibR = 0.0; 
   
 hn = 0.02;   %采样周期为hn/2，四元数的更新周期
 Tn = 2000;
 
 
for k = 1:Tn  %时钟基准0时刻开始/k 表示四元数的第k次更新  仿真时间t = Tn*hn;
 
 q0 = q; 
 for i = 1:3 %获取龙格库塔法的采样值wpb 和 理论值pitchT，rollT，yawT。 
 pitchT(i) = pitch_m*sin( 2*pi*(k-(3-i)/2)*hn/T_p + pitch0*Rad_Deg); %真实姿态角 TRUE
 rollT(i) =  roll_m*sin( 2*pi*(k-(3-i)/2)*hn/T_r + roll0*Rad_Deg);
 yawT(i) =   yaw_m*sin( 2*pi*(k-(3-i)/2)*hn/T_y + yaw0*Rad_Deg) + yawK; %时间t的函数
	                                                     
 wpT(i) = 2*pitch_m* cos( 2*pi*(k-(3-i)/2)*hn/T_p+pitch0*Rad_Deg )*pi/T_p; %真实姿态角速度  
 wrT(i) = 2*roll_m* cos( 2*pi*(k-(3-i)/2)*hn/T_r+roll0*Rad_Deg )*pi/T_r;   
 wyT(i) = 2*yaw_m* cos( 2*pi*(k-(3-i)/2)*hn/T_y+yaw0*Rad_Deg )*pi/T_y;    %时间t的函数
 end
 
 
 for i = 1:3
    
 wnbb(i,1) =  sin(rollT(i))*cos(pitchT(i))*wyT(i) + cos(rollT(i))*wpT(i);
 wnbb(i,2) = -sin(pitchT(i))*wyT(i) + wrT(i);
 wnbb(i,3) = -cos(rollT(i))*cos(pitchT(i))*wyT(i) + sin(rollT(i))*wpT(i);
 end

 for i = 1:3  
    wnbb1(i) = wnbb(1,i);
    wnbb2(i) = wnbb(2,i);
    wnbb3(i) = wnbb(3,i);
 end
  
	wien(1) = 0;
	wien(2) = wie*cos(latiT);
	wien(3) = wie*sin(latiT);
   
   wenn(1) = 0;
	wenn(2) = 0;
	wenn(3) = 0;
   winn = wien + wenn;
     
   
  	%获取方向余弦矩阵Cnb1,Cnb2,Cnb3
     
   for i = 1:3  
   Cnb11(i) = cos(rollT(i))*cos(yawT(i))+sin(rollT(i))*sin(pitchT(i))*sin(yawT(i)); 
	Cnb12(i) = -cos(rollT(i))*sin(yawT(i))+sin(rollT(i))*sin(pitchT(i))*cos(yawT(i));
	Cnb13(i) = -sin(rollT(i))*cos(pitchT(i));
	
	Cnb21(i) = cos(pitchT(i))*sin(yawT(i));
	Cnb22(i) = cos(pitchT(i))*cos(yawT(i));
	Cnb23(i) = sin(pitchT(i));
	
	Cnb31(i) = sin(rollT(i))*cos(yawT(i))-cos(rollT(i))*sin(pitchT(i))*sin(yawT(i));
	Cnb32(i) = -sin(rollT(i))*sin(yawT(i))-cos(rollT(i))*sin(pitchT(i))*cos(yawT(i));
   Cnb33(i) = cos(rollT(i))*cos(pitchT(i));
   end   
   
   Tnb1 = [Cnb11(1)  Cnb12(1)  Cnb13(1) 
           Cnb21(1)  Cnb22(1)  Cnb23(1) 
           Cnb31(1)  Cnb32(1)  Cnb33(1)];
        
   Tnb2 = [Cnb11(2)  Cnb12(2)  Cnb13(2) 
           Cnb21(2)  Cnb22(2)  Cnb23(2) 
           Cnb31(2)  Cnb32(2)  Cnb33(2)];
        
   Tnb3 = [Cnb11(3)  Cnb12(3)  Cnb13(3) 
           Cnb21(3)  Cnb22(3)  Cnb23(3) 
           Cnb31(3)  Cnb32(3)  Cnb33(3)];
   
   
     
   winb1 = Tnb1*winn';
   winb2 = Tnb2*winn';
   winb3 = Tnb3*winn';
      
   wib1T = winb1 + wnbb1'; %陀螺的无误差输出(t, t+ hn/2 , t+hn)
   wib2T = winb2 + wnbb2';
   wib3T = winb3 + wnbb3';
  
  
	wib1(1) = (1 + Kgx)*wib1T(1) + G_Drift; %仿真陀螺仪输出 （t ， t+hn/2,  t+hn）
	wib1(2) = (1 + Kgy)*wib1T(2) + G_Drift;
	wib1(3) = (1 + Kgz)*wib1T(3) + G_Drift;
   
   wib2(1) = (1 + Kgx)*wib2T(1) + G_Drift;
	wib2(2) = (1 + Kgy)*wib2T(2) + G_Drift;
	wib2(3) = (1 + Kgz)*wib2T(3) + G_Drift;
   
   wib3(1) = (1 + Kgx)*wib3T(1) + G_Drift;
	wib3(2) = (1 + Kgy)*wib3T(2) + G_Drift;
	wib3(3) = (1 + Kgz)*wib3T(3) + G_Drift;


	fn = [0  0 g]';
    
   fb1T = Tnb1*fn;
   fb2T = Tnb2*fn;
   fb3T = Tnb3*fn;

   fb1(1) = (1 + Kax)*fb1T(1) + A_bias;%仿真加速度计的输出（t ， t+hn/2,  t+hn）%可以提供扰动加速度
	fb1(2) = (1 + Kay)*fb1T(2) + A_bias;
	fb1(3) = (1 + Kaz)*fb1T(3) + A_bias;
   
   fb2(1) = (1 + Kax)*fb2T(1) + A_bias;
	fb2(2) = (1 + Kay)*fb2T(2) + A_bias;
	fb2(3) = (1 + Kaz)*fb2T(3) + A_bias;
   
   fb3(1) = (1 + Kax)*fb3T(1) + A_bias;
	fb3(2) = (1 + Kay)*fb3T(2) + A_bias;
   fb3(3) = (1 + Kaz)*fb3T(3) + A_bias;
   
   
    fbR = fbR + fb2 + fb3;           %求取平均值
    wibR = wibR + wib2 + wib3;
   
   %初始对准程序仿真 （模拟现实对准）
   if k == 1000
      fbR = fbR/2000.0;
      wibR = wibR/2000.0;
    
      
  temp = g*wie*cos(latiT);
  
  TR(1,1) =  (-wibR(3)*fbR(2) + wibR(2)*fbR(3))/temp;
  TR(1,2) =  (-wibR(1)*fbR(3) + wibR(3)*fbR(1))/temp;
  TR(1,3) =  (-wibR(2)*fbR(1) + wibR(1)*fbR(2))/temp;

  TR(2,1) = -fbR(1)*tan(latiT)/g + wibR(1)/(wie*cos(latiT));
  TR(2,2) = -fbR(2)*tan(latiT)/g + wibR(2)/(wie*cos(latiT));
  TR(2,3) = -fbR(3)*tan(latiT)/g + wibR(3)/(wie*cos(latiT));

  TR(3,1) =  fbR(1)/g;
  TR(3,2) =  fbR(2)/g;
  TR(3,3) =  fbR(3)/g;
  
  Coar_p = asin(TR(3,2));
  Coar_r = atan(-TR(3,1)/TR(3,3));
  Coar_y = atan(TR(1,2)/TR(2,2));
  
  
  %四元数初始化
  qR(1) = sqrt(1 + TR(1,1) + TR(2,2) + TR(3,3))/2.0;
  qR(2) = (TR(3,2) - TR(2,3))/(4*qR(1));
  qR(3) = (TR(1,3) - TR(3,1))/(4*qR(1));
  qR(4) = (TR(2,1) - TR(1,2))/(4*qR(1));
  
  Coa_p = asin(2*(qR(3)*qR(2)+qR(1)*qR(2)));
  Coa_r = atan(2*(qR(2)*qR(2)-qR(1)*qR(3))/(qR(1)^2-qR(2)^2-qR(3)^2+qR(4)^2));
  Coa_y = atan(2*(qR(2)*qR(3)-qR(1)*qR(2))/(qR(1)^2-qR(2)^2+qR(3)^2-qR(4)^2));
  
   end

 
 %(1) 四阶龙格库塔法 (针对输出为角速率的算法)
 sysW1 =[ 0           -wnbb(1,1)  -wnbb(1,2)  -wnbb(1,3)
           wnbb(1,1)     0         wnbb(1,3)  -wnbb(1,2)
           wnbb(1,2)  -wnbb(1,3)     0         wnbb(1,1)
           wnbb(1,3)   wnbb(1,2)  -wnbb(1,1)     0      ];
        
 sysW2 =[  0          -wnbb(2,1)  -wnbb(2,2)  -wnbb(2,3)
           wnbb(2,1)     0         wnbb(2,3)  -wnbb(2,2)
           wnbb(2,2)  -wnbb(2,3)     0         wnbb(2,1)
           wnbb(2,3)   wnbb(2,2)  -wnbb(2,1)     0      ];
        
 sysW3 =[ 0           -wnbb(3,1)  -wnbb(3,2)  -wnbb(3,3)
           wnbb(3,1)     0         wnbb(3,3)  -wnbb(3,2)
           wnbb(3,2)  -wnbb(3,3)     0         wnbb(3,1)
           wnbb(3,3)   wnbb(3,2)  -wnbb(3,1)     0      ];
        
    
       
     
 K1 = sysW1*q0/2.0; %四维
 q0 = q + hn*K1/2.0;
 
 K2 = sysW2*q0/2.0;  
 q0 = q + hn*K2/2.0; 
 
 K3 = sysW2*q0/2.0;  
 
 q0 = q + hn*K3; 
 K4 = sysW3*q0/2.0;  
 K = (K1 + 2*K2 + 2*K3 + K4)/6.0;
 q = q + K*hn;
 
   %(2)没有补偿的增量法，精度太差，进行圆锥补偿后，精度如何？ 
 
   %delx = (wnbb1(1)+4*wnbb2(1)+wnbb3(1))*hn/6; %(2.1)辛普森积分法 （又有专门针对角增量的算法）
   %dely = (wnbb1(2)+4*wnbb2(2)+wnbb3(2))*hn/6;
   %delz = (wnbb1(3)+4*wnbb2(3)+wnbb3(3))*hn/6;
   
    %delx = (wnbb1(1) + wnbb3(1))*hn/2;   %（2.2）矩形积分法
    %dely = (wnbb1(2) + wnbb3(2))*hn/2;
    %delz = (wnbb1(3) + wnbb3(3))*hn/2;
    
    %del1 = (wnbb1 + wnbb2)*hn/4;   %又有专门针对角增量的算法
    %del2 = (wnbb2 + wnbb3)*hn/4; 
    %Sumdel = (wnbb1 + 4*wnbb2 + wnbb3)*hn/6.0;
    
    % A = [   0        -del1(3)     del1(2)
    %        del1(3)     0         -del1(1)
    %       -del1(2)   del1(1)     0       ] ;
         
    % B = [del2(1) del2(2)  del2(3)]';
     
   %delb = A*B;   
     
   %Newdel = del1' + del2' + 2*delb/3.0;  %采用不同的积分方法
   %Newdel = Sumdel'+ 2*delb/3.0;     %(Good)
   
   %delx = Newdel(1);
   %dely = Newdel(2);
   %delz = Newdel(3);


   % del = sqrt(delx*delx+dely*dely+delz*delz); %平方项
   % Ac = cos(del/2); %1- del/8 + del*del/384;  
   % As = sin(del/2)/del; %0.5- del/48 + del*del/3840;    % delta = 0   如何处理？
  
 %   p(1) = Ac;        %四元数的变化量
 %   p(2) = As*delx; 
 %   p(3) = As*dely;
 %   p(4) = As*delz;
  
 %   y(1) = q(1)*p(1)-q(2)*p(2)-q(3)*p(3)-q(4)*p(4);
 %   y(2) = q(1)*p(2)+q(2)*p(1)+q(3)*p(4)-q(4)*p(3);
 %   y(3) = q(1)*p(3)+q(3)*p(1)+q(4)*p(2)-q(2)*p(4);
 %   y(4) = q(1)*p(4)+q(4)*p(1)+q(2)*p(3)-q(3)*p(2);
   
 %   for j =1:4
 %     q(j) = y(j);
 %   end
  
   T(1) = q(1)*q(1)+q(2)*q(2)-q(3)*q(3)-q(4)*q(4);
	T(2) = 2*(q(2)*q(3)-q(1)*q(4));
	T(3) = 2*(q(2)*q(4)+q(1)*q(3));

	T(4) = 2*(q(2)*q(3)+q(1)*q(4));
	T(5) = q(1)*q(1)-q(2)*q(2)+q(3)*q(3)-q(4)*q(4);
	T(6) = 2*(q(3)*q(4)-q(1)*q(2));

	T(7) = 2*(q(2)*q(4)-q(1)*q(3));
	T(8) = 2*(q(3)*q(4)+q(1)*q(2));
   T(9) = q(1)*q(1)-q(2)*q(2)-q(3)*q(3)+q(4)*q(4);
   
   attitude(1) = asin(T(8))/Rad_Deg;             %纵摇角(-90 ,90)
	attitude(2) = atan(-T(7)/T(9))/Rad_Deg;       %横摇角 (-90,90)
	attitude(3) = atan(T(2)/T(5))/Rad_Deg;        %航向角(0,360) 航向角 右手反螺旋
 
   attpT(k) = pitchT(3)/Rad_Deg;  %Ture
   attrT(k) = rollT(3)/Rad_Deg;
   attyT(k) = yawT(3)/Rad_Deg;
   
   attp(k) = attitude(1); %姿态解算值
   attr(k) = attitude(2);
   atty(k) = attitude(3);

   dp(k) = attpT(k)- attp(k);
   dr(k) = attrT(k)- attr(k);
   dy(k) = attyT(k)- atty(k);    
     
   
 end
 
 i = 1:Tn;
  subplot(3,2,1)
  plot(i*hn,dp(i),'r')
  subplot(3,2,2)
  plot(i*hn,dr(i),'r')
  subplot(3,2,3)
  plot(i*hn,dy(i),'r')