tuisuan.m

组合导航的程序

%vn,ve    北向和东向海流速度
%s         船对水的速度；
%K         航向
%dphi      纬度误差
%dlamda    经度误差
%dvn,dve  北向和东向海流误差
%ds       船速误差
%dK       航向误差

K = 90;
dve = 0.1; %unit  m/s
dvn = 0.1; %unit  m/s
Rad_D = 0.01745329;  %Rad_Degree 

dK = 2*Rad_D/60;  % unit: radian

ds = 0.1;
s = 15*1852/3600; %15 knot 

dphi0 = 2*Rad_D/60;  % unit: radian
dlamda0 =2*Rad_D/60; %unit: radian

step = 10;
K = K*Rad_D;

f_dphi = dvn + ds*cos(K) - s*sin(K)*dK;
f_lamda = dve + ds*sin(K) + s*cos(K)*dK;

dphi = dphi0;  
dlamda = dlamda0; 
for j =1:86400/step
         
   k11 = f_dphi;
   k21 = f_lamda;
   y1 = dphi;  
   y2 = dlamda;  
    
   dphi = y1+step*k11;
   dlamda = y2+step*k21;

   phi(j+1) = dphi*60/Rad_D;
  lamda(j+1) = dlamda*60/Rad_D;
  
end     

 phi(1) = dphi0*60/Rad_D;
 lamda(1) = dlamda0*60/Rad_D;

i=1:1:86400/step+1;

  subplot(2,2,1)
  plot(i,phi(i),'r')
  subplot(2,2,2)
  plot(i,lamda(i),'r')