%dx(t) = F(t)x(t)+G(t)w(t);
%x(t) = (dvx, dvy, phix, phiy, phiz)'
%w(t) = (biasx+adx,biasy+ady,driftx+wdx,drifty+wdy,driftz+wdz)'

%x(k+1) = A(k+1,k)x(k)+B(k)w(k);


clear all;

Rad_D = 0.01745329;  %Rad_Degree 
g = 9.8;
R = 6378393;
wie = 7.2921158e-5;
wieN = wie*cos(45.7796*Rad_D); 
wieZ = wie*sin(45.7796*Rad_D);
T = 60;

dvx0 = 1;  
dvy0 = 1;  
phix0 = 2*Rad_D;
phiy0 = 2*Rad_D; 
phiz0 = 5*Rad_D; 
w1 =  1e-4+5e-5;   %零偏和干擾加速度
w2 =  1e-4+5e-5;
w3 =  0.01*Rad_D/3600+0.005*Rad_D/3600;
w4 =  0.01*Rad_D/3600+0.005*Rad_D/3600;
w5 =  0.01*Rad_D/3600+0.005*Rad_D/3600;

 x = [dvx0, dvy0, phix0, phiy0,phiz0]';  


F = [0       0	  0     -g     0  
     0       0     g      0     0
     0     -1/R    0    wieZ   -wieN
     1/R     0   -wieZ    0      0
     0       0   wieN     0      0];
   
  
G = [1   0    0   0   0
     0   1    0   0   0
     0   0   -1   0   0
     0   0    0  -1   0
     0   0    0   0  -1];
w = [w1,w2,w3,w4,w5]';

[Phi,Gama] = c2d(F,G,T);
  
 for k =1:86400/T    %12 hours
    x = Phi*x+ Gama*w ; 
    ddvx(k+1)= x(1);
    ddvy(k+1)= x(2);
    alpha(k+1)= x(3);
    beta(k+1) = x(4);
    gama(k+1) = x(5);
    
 end
 
    ddvx(1) = dvx0;
    ddvy(1) = dvy0;
    alpha(1)= phix0;
    beta(1) = phiy0;
    gama(1) = phiz0;
   
i=1:1:86400/T;
  subplot(3,2,1)
  plot(i,ddvx(i),'r')
  subplot(3,2,2)
  plot(i,ddvy(i),'r')
  subplot(3,2,3)
  plot(i,alpha(i)/Rad_D)
  subplot(3,2,4)
  plot(i,beta(i)/Rad_D)
  subplot(3,2,5)
  plot(i,gama(i)/Rad_D)