Расчет координат спутников в системе GPS в Matlab

clc;clear all;close all;
g.tow=197430;
g.flags=20;
g.iodc=12;
g.toc=201584;
g.ura=1;
g.healthS=0;
g.wn=694;
g.tgd=-0.000000004656613;
g.af2=0.000000000000000;
g.af1=0.000000000012278;
g.af0=-0.000288312323391;
g.toe=201584;
g.iode=12;
g.rootA=5153.762765884399414;
g.ecc=0.010925623239018;
g.m0=0.304183455184102;
g.omega0=-0.207362390588969;
g.inc0=0.306331528816372;
g.argPer=0.514487708918750;
g.deln=0.000000001370950;
g.omegaDot=-0.000000002564661;
g.incDot=0.000000000002956;
g.crc=177.031250000000000;
g.crs=-148.875000000000000;
g.cuc=-0.000007700175047;
g.cus=0.000010803341866;
g.cic=-0.000000176951289;
g.cis=-0.000000111758709;
 
tpr=28818.000;%время приемника
g_pdelay=0.073280739894899; %псевдозадержка
 
%1)2) по методичке
ti_gps = tpr + 86400*round((g.tow - tpr)/86400);
t_ka = ti_gps - g_pdelay;
dt_ka = g.af0 + g.af1*(t_ka - g.toc) + g.af2*((t_ka - g.toc)^2)- g.tgd;
 
t_pr = mod(t_ka - dt_ka,604800); % время приемника на момент предшествия   
 
%3) по вычисление приращения показаний часов системы GPS на интервале
%времени между узловым моментом toe и моментом предшествия
t_k = t_pr - g.toe; %Time from ephemeris reference epoch
%Постоянные:
%скорость вращения Земли
OMEGAeDOT=7.2921151467e-005;
%или
%OMEGAeDOT=0;
mu=398600500000000;
F_CONST = 4.442807633E-10;
%Corrected mean motion
n0 = sqrt((mu)/(g.rootA^6)); 
n = n0 + g.deln; 
 
%Mean anomaly
 
Mk = g.m0 + n*t_k; 
%Kepler's Equation for Eccentric Anomaly
%(may be solved by iteration) (radians)
 
E(1) = Mk;
eps = 1e-9;
i = 2;
while (1)
   
E(i) = E(i-1) - ((E(i-1) - g.ecc * sin(E(i-1)) - Mk) / (1 - g.ecc * cos(E(i-1)) ));
if( abs(E(i) - E(i-1)) < eps)
   break 
end
i = i + 1 ;
end
Ek = E(length(E)); 
 
%True Anomaly
 
v = atan2 ((sqrt(1-g.ecc^2) * sin(Ek)) , (cos(Ek) - g.ecc));
Ek = acos((g.ecc+cos(v))/(1+g.ecc*cos(v))); 
 
%Argument of Latitude
Fk =v + g.argPer;
 
% Second Harmonic Perturbations:
% Argument of Latitude Correction
% Radius Correction
% Inclination Correction
dUk = g.cus * sin(2*Fk) + g.cuc * cos (2*Fk);
drk = g.crs * sin(2*Fk) + g.crc * cos (2*Fk);
dik = g.cis * sin(2*Fk) + g.cic * cos (2*Fk);
 
% Corrected Argument of Latitude
% Corrected Inclination
% Corrected Radius
 
Uk = Fk + dUk;
rk = g.rootA^2 * (1 - g.ecc * cos (Ek)) + drk;
ik = g.inc0 + dik + g.incDot*t_k;
 
%Positions in orbital plane.
 
xk = rk * cos(Uk);
yk = rk * sin(Uk);
 
%Corrected longitude of ascending node
 
OMEGAk = g.omega0 + (g.omegaDot-OMEGAeDOT) * t_k - OMEGAeDOT * g.toe;
 
%Earth-fixed coordinates.
x = xk * cos(OMEGAk) - yk * cos(ik) * sin(OMEGAk)
y = xk * sin(OMEGAk) + yk * cos(ik)*cos(OMEGAk)
z = yk * sin(ik)
 
%радиус вектор от центра земли
r = sqrt(x^2 + y^2 + z^2)/1000
%пересчет системы координат
a = 6378137; 
e = 0.006694379990141316996137335400448; 
 
D = sqrt(x*x + y*y);
 
if (D == 0)
    B = (z*pi)/(2*abs(z))
    L = 0
    H = z*sin(B) - a * sqrt (1 - e*e * sin(B) * sin(B)) 
end
 
if (D ~= 0)
   La = abs(asin(y/D));
   
if ((y < 0)& (x > 0) )
    L = 2 * pi - La;
end
 
if ((y < 0)& (x < 0) )
        L = pi + La;
end  
 
if ((y > 0)& (x < 0) )
        L = pi - La;
end  
 
if ((y > 0)& (x > 0) )
        L = La;
end  
 
if ((y == 0) & (x > 0) )
        L = 0;
end  
 
if ((y == 0) & (x < 0) )
        L = pi;
end 
 
end
    
if (z == 0 )
    B = 0;
    H = D - a;
else
 
 r = sqrt (x*x + y*y + z*z);
 c = asin (z/r);
 p = (e * e * a)/(2 * r);   
  
  s1 = 0;
 
  while(1)
      b = c + s1;
      s2 = asin ( (p * sin(2*b))  / sqrt( 1 - e*e*sin(b)*sin(b))  ) ;
      if (abs(s2 - s1) < 4.85e-10)
          break
      end
      s1 = s2;
  end
  
  B = b;
  H = D * cos (B) + z * sin(B) - a * sqrt (1 - e*e * sin(B)*sin(B));
      
end