tuisuan2.m

来自「组合导航的程序」· M 代码 · 共 67 行

%wn, we 北向和东向上的流速噪声 
%ws, wk 为白噪声
%dx(t) = F(t)x(t)+G(t)w(t);
%x(t) = (dphi, dlamda, dvn, dve, ds , dK)'
%w(t) = (wn, we, ws , wk)'

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


clear all;

Rad_D = 0.01745329;  %Rad_Degree 
dphi0 = 2*Rad_D/60;  % unit: radian
dlamda0 = 2*Rad_D/60;  % unit: radian
ds0 = 0.4*1852/3600;
dve0 = 0.1; %unit  m/s
dvn0 = 0.1; %unit  m/s
dK0 = 2*Rad_D/60;  % unit: radian


 x = [dphi0, dlamda0, dvn0, dve0, ds0, dK0]';  


betaL = 1/7200.0; 
betaS = 1/7200.0;
betaK = 1/7200.0;
K = 45;
s = 4*1852/3600; %4knot
T = 10;
K = K*Rad_D;
F = [0   0	     1      0    cos(K)  -s*sin(K)
     0   0     0      1    sin(K)  s*cos(K)
     0   0  -betaL    0     0         0
     0   0     0    -betaL  0         0
     0   0     0      0     -betaS    0
     0   0     0      0     0        -betaK];
  
G = [0  0  0  0
     0  0  0  0
     1  0  0  0
     0  1  0  0
     0  0  1  0
     0  0  0  1];
w = 0.0001*randn(4,1);
[Phi,Gama] = c2d(F,G,T);
  
 for k =1:1000   %%the kalman filter
    
    x = Phi*x+ Gama*w ; 
    dphi(k+1)= x(1);
    dlamda(k+1)= x(2);
    
 end
 
    dphi(1)= dphi0;
    dlamda(1)= dlamda0;
         
   
i=1:1:1000;
  subplot(2,2,1)
  plot(i,dphi(i),'r')
  subplot(2,2,2)
  plot(i,dlamda(i),'r')