finealign.m

组合导航的程序

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%Fine alignment
%dx(t) = F(t)x(t)+G(t)w(t);
%x(t) = (phix, phiz, dy, dz)'
%y(t) = (phiy,dx)'
%w(t) = (driftx+wdx,driftz+wdz,biasy+ady,biasz+adz)'
%v(t) = (drifty+wdy,biasx+adx)'
%x(k+1) = A(k+1,k)x(k)+B(k)w(k) %两个二阶系统
%y(k+1) = AA(k+1,k)y(k)+BB(k)v(k) %二阶系统

clear all;
 
Rad_D = 0.01745329;  %Rad_Degree 
g = 9.807;
R = 6378393;
wie = 7.2921158e-5;  %弧度/秒
wieN = wie*cos(45.7796*Rad_D); 
wieZ = wie*sin(45.7796*Rad_D);
T = 1;

phix0 = 2*Rad_D/60;  %角分 化为弧度
phiy0 = 2*Rad_D/60;  %调平至关重要!
phiz0 = 180*Rad_D/60;  
dx0 = 0;
dy0 = 0;
dz0 = 0; 

 del1 = 0.01;  
 omega1 = 0.01;
%Kx = (del1^2+omega1^2)/(2*del1*g);
%Kdx = 2*del1;
Kx = del1/g;
Kdx = 2*del1;

%%%%%%%%%%%%%%%%%%%
del2 = 0.01/3.0;
omega2 = 0.01/3.0;
%Ky = (del2^2 + omega2^2)/(g*del2);%del2/g;
%Kz = (del2^4 + 2*(del2*omega2)^2 + omega2^4)/(4*g*wieN*del2^2);%omega2*omega2/(4*g*wieN);
Ky0 = 2*del2/g;
Kz0 = del2^2/(g*wieN);
Kdy0 = 2*del2; 
Kdz0 = 2*del2;

w1 =  0.01*Rad_D/3600+0.05*Rad_D/3600;
w2 =  0.01*Rad_D/3600+0.05*Rad_D/3600;
w3 =  1e-3+5e-4;   %零偏和干扰加速度
w4 =  1e-3+5e-4;

v1 =  0.1*Rad_D/3600+0.05*Rad_D/3600;
v2 =  1e-3+5e-4;

x = [phix0, phiz0, dy0, dz0]';  
y = [phiy0, dx0]';


   omega2 = 0.05/1.414/3;
   wieN = 7.29e-5*cos(45.7796*pi/180);
   Kdx = (1 + 1.414)*omega2;
   Kdy = (1 + 1.414)*omega2;
   Kdz = (1 + 1.414)*omega2;
   Kx =4*omega2/(1+1.414)/g;
   Ky = 4*omega2/(1+1.414)/g;
   Kz = 4*omega2*omega2/(5.828*wieN*g);
   KAz = Kz*wieN;


for k =1:600*6/T 

%      Ky = Ky0;
%      Kz = Kz0;
%      Kdy = Kdy0; 
%      Kdz = Kdz0;
   
   
F = [0       -wieN	  -Ky    0       
     0         0        0     Kz   
     g*Kdy     0       -Kdy   0   
     0         0        Kdz  -Kdz];
       
G = [-1   0    0   0   
      0  -1    0   0   
      0   0   Kdy  0   
      0   0    0   0 ];
w = [w1,w2,w3,w4]';

%%%%%%%%%%%%%%%%%%%%%%%%%
F1 = [ 0         Kx
     -g*Kdx     -Kdx];
  
G1 = [-1       0
      0       Kdx];
   
 v = [v1, v2]';
  
  [Phi,Gama] = c2d(F,G,T);
 [Phi1,Gama1] = c2d(F1,G1,T);
  
    
    x = Phi*x+ Gama*w ; 
    y = Phi1*y+ Gama1*v ; 
    
    alpha(k+1)= x(1);
    gama(k+1)= x(2);
     ddy(k+1)= x(3);
    ddz(k+1) = x(4);
    
    beta(k+1) = y(1);
    ddx(k+1) = y(2);
 end
 
    alpha(1) = phix0;
    gama(1) = phiz0;
     ddy(1)= dy0;
     ddz(1)= dz0;
     
     beta(1) = phiy0;
      ddx(1) = dx0;
     
i=1:1:600*6/T;
  subplot(3,2,1)
  plot(i,alpha(i)*60/Rad_D)
  subplot(3,2,2)
  plot(i,gama(i)*60/Rad_D)
  subplot(3,2,3)
  plot(i,ddy(i),'r')
  subplot(3,2,4)
  plot(i,ddz(i),'r')
  subplot(3,2,5)
  plot(i,beta(i)*60/Rad_D)
  subplot(3,2,6)
  plot(i,ddx(i),'r')