stdalone.pas

calculate satellite positions, to correct pseudoranges and finally to calculate the receivers positi

program stdalone;

type
   mat96 = array[1..32,1..3] of real;
   vecb32 = array[1..32] of boolean;
   vec32 = array[1..32] of real;
   vec16 = array[1..16] of real;
   vec8 = array[1..8] of real;
   vec3 = array[1..3] of real;
   mat16 = array[1..4,1..4] of real;

const
   We = 7.292115E-5;            {WGS-84 earth rotation rate}
   c = 299792458.0;             {WGS-84 speed of light}
   pi = 3.1415926535898         {WGS 84 value of pi};
   Wedot = 7.2921151467E-5;     {WGS 84 value of earth's rotation rate}
   mu = 3.986005E+14;           {WGS 84 value of earth's univ. grav. par.}
   F = -4.442807633E-10;        {relativistic correction term constant}
   a = 6378137.0;               {WGS-84 earth's semi major axis}
   b = 6356752.31;              {WGS-84 earth's semi minor axis}
   e1sqr = (a*a - b*b) / (a*a); {first  numerical eccentricity}
   e2sqr = (a*a - b*b) / (b*b); {second numerical eccentricity}

var
   inp, out: text;
   Ttr, tau, Trc, T, Trel: real;
   Az, El, Cr, alpha, dTclck, dTiono, dRtrop, Lat, Lon: real;
   prn, i: integer;
   eph: array[1..32,1..16] of real;
   clk: array[1..32,1..5] of real;
   ion: vec8;
   Praw, Pcor: vec32;
   Xs: mat96;
   SV: vecb32;
   P: vec32;
   Xr, Xlla, tmp3: vec3;
   status: boolean;
   tmp16: vec16;
   tmp5: array[1..5] of real;

{***************************************************************************}

procedure LLA2XYZ(Xi : vec3;      {lat [rad], lon [rad] alt [m]}
              var Xo : vec3);     {ECEF X [m], Y [m] , Z[m]}
{this procedure converts WGS-84 Lat, Lon and Alt [above ellipsoid]
 to ECEF XYZ}

var
   N: real;
begin
N := a / sqrt(1.0 - e1sqr * sin(Xi[1]) * sin(Xi[1]));
Xo[1] := (N                 + Xi[3]) * cos(Xi[1]) * cos(Xi[2]);
Xo[2] := (N                 + Xi[3]) * cos(Xi[1]) * sin(Xi[2]);
Xo[3] := (N * (1.0 - e1sqr) + Xi[3]) * sin(Xi[1]);
end; {procedure LLA2XYZ}

{***************************************************************************}

procedure XYZ2LLA(Xi : vec3;      {ECEF X [m], Y [m] , Z[m]}
              var Xo : vec3);     {lat [rad], lon [rad] alt [m]}
{this procedure converts WGS-84 ECEF XYZ to Lat, Lon, Alt [above ellipsoid]}

var
   p, T, sT, cT, N, sig: real;
begin
p := sqrt(Xi[1] * Xi[1] + Xi[2] * Xi[2]);
T := arctan((Xi[3] * a) / (p * b));
sT := sin(T); cT := cos(T);
Xo[1] := arctan((Xi[3] + e2sqr * b * sT * sT * sT)
         / (p - e1sqr * a * cT * cT * cT));
if Xi[2] <> 0.0 then sig := Xi[2] / abs(Xi[2]) else sig := 1.0;
if Xi[1] = 0.0 then Xo[2] := sig * pi / 2.0 else
   begin
   Xo[2] := arctan(Xi[2]/Xi[1]);
   if (Xi[1] < 0.0) and (Xi[2] >= 0.0) then Xo[2] := Xo[2] + pi;
   if (Xi[1] < 0.0) and (Xi[2] < 0.0) then Xo[2] := Xo[2] - pi
   end;
N := a / sqrt(1.0 - e1sqr * sin(Xo[1]) * sin(Xo[1]));
Xo[3] := p / cos(Xo[1]) - N;

end; {procedure XYZ2LLA}

{***************************************************************************}

procedure satpos(eph : vec16;    {ephemeris}
                 Ttr : real;     {satellite GPS time}
             var Trel: real;     {relativistic correction term}
             var X   : vec3);    {satellite position}

var
  M0, dn, ec, A, W0, i0, w, Wdot, Cuc, Cus, Crc, Crs, Cic, Cis, Toe, Idot: real;
  T, n0, n, M, E, Eold, snu, cnu, nu, phi, du, dr, di, u, r, i, Xdash, Ydash, Wc: real;
  k: integer;

begin
{for clarity of code, copy the ephemeris parameters and convert to radians}
Crs := eph[1];
dn  := eph[2] * pi;
M0  := eph[3] * pi;
Cuc := eph[4];
ec  := eph[5];
Cus := eph[6];
A   := eph[7] * eph[7];
Toe := eph[8];
Cic := eph[9];
W0  := eph[10] * pi;
Cis := eph[11];
i0  := eph[12] * pi;
Crc := eph[13];
w   := eph[14] * pi;
Wdot:= eph[15] * pi;
idot:= eph[16] * pi;

T:= Ttr - Toe;
if T >  302400 then T := T - 604800;
if T < -302400 then T := T + 604800;

n0 := sqrt(mu / (A*A*A));
n := n0 + dn;

M := M0 + n*T;
E := M; {start value for E}
repeat
   Eold := E;
   E := M + ec * sin(E);
   until abs(E - Eold) < 1.0e-8;

snu := sqrt(1 - ec*ec) * sin(E) / (1 - ec*cos(E)); 
cnu := (cos(E) - ec) / (1 - ec*cos(E)); 
if cnu = 0 then nu := pi/2 * snu / abs(snu)
else if (snu = 0) and (cnu > 0) then nu := 0
else if (snu = 0) and (cnu < 0) then nu := pi
else nu := arctan(snu/cnu)
+ ord(cnu<0) * pi * snu / abs(snu);

phi := nu + w;

du := Cuc*cos(2*phi) + Cus*sin(2*phi);
dr := Crc*cos(2*phi) + Crs*sin(2*phi);
di := Cic*cos(2*phi) + Cis*sin(2*phi);

u := phi + du;
r := A*(1 - ec*cos(E)) + dr;
i := i0 + idot*T +di;

Xdash := r*cos(u);
Ydash := r*sin(u);

Wc:= W0 + (Wdot - Wedot)*T - Wedot*Toe;

X[1] := Xdash*cos(Wc) - Ydash*cos(i)*sin(Wc);
X[2] := Xdash*sin(Wc) + Ydash*cos(i)*cos(Wc);
X[3] := Ydash*sin(i);

Trel := F * ec * eph[7] * sin(E) {relativistic correction term}

end; {procedure satpos}

{***************************************************************************}

procedure calcAzEl(Xs,              {satellite ECEF XYZ}
                   Xu  :  vec3;     {user ECEF XYZ}
               var Az,              {azimuth [rad]}
                   El  : real;      {elevation [rad]}
               var stat: boolean);  {calculation succeeded: stat = true}

var
   R, p, x, y, z, s: real;
   e: array[1..3,1..3] of real;
   i, k: integer;
   d: vec3;

begin

x := Xu[1];
y := Xu[2];
z := Xu[3];
p := sqrt(x*x + y*y);
if p = 0.0 then stat := false else stat := true;

if stat then
   begin

   R := sqrt(x*x + y*y + z*z);
   e[1,1] := - y / p;
   e[1,2] := + x / p;
   e[1,3] := 0.0;
   e[2,1] := - x*z / (p*R);
   e[2,2] := - y*z / (p*R);
   e[2,3] := p / R;
   e[3,1] := x / R;
   e[3,2] := y / R;
   e[3,3] := z / R;

   for k := 1 to 3 do
      begin
      d[k] := 0.0;
      for i := 1 to 3 do d[k] := d[k] + (Xs[i] - Xu[i]) * e[k,i]
      end;

   s := d[3] / sqrt(d[1]*d[1] + d[2]*d[2] + d[3]*d[3]);
   if s = 1.0 then El := 0.5 * pi else El := arctan(s / sqrt(1.0 - s*s));

   if (d[2] = 0.0) and (d[1] > 0.0) then Az := 0.5 * pi else
      if (d[2] = 0.0) and (d[1] < 0.0) then Az := 1.5 * pi else
         begin
         Az := arctan(d[1] / d[2]);
         if d[2] < 0.0 then Az := Az + pi else
            if (d[2] > 0.0) and (d[1] < 0.0) then Az := Az + 2.0 * pi
         end;

   end;

end; {procedure calcAzEl}

{***************************************************************************}

procedure ionocorr (ion   :vec8;    {iono correction coefficients from
                                     nav message}
                    Latu,           {user's latitude [rad]}
                    Lonu,           {user's longitude [rad]}
                    Az,             {SV azimuth [rad]}
                    El,             {SV elevation [rad]}
                    Ttr   : real;   {SV signal transmission time [sec]}
                var dTiono: real);  {Ionospheric delay [sec]}

var
  phi, Lati, Loni, Latm, T, F, x, per, amp: real;
  a0, a1, a2, a3, b0, b1, b2, b3: real;

begin

{for clarity copy array}
a0 := ion[1];
a1 := ion[2];
a2 := ion[3];
a3 := ion[4];
b0 := ion[5];
b1 := ion[6];
b2 := ion[7];
b3 := ion[8];

{convert from radians to semicircles}
Latu := Latu / pi; Lonu := Lonu / pi; Az := Az / pi; El := El / pi;

{calculation}
phi := 0.0137 / (El + 0.11) - 0.022;
Lati := Latu + phi * cos (Az * pi);
if Lati > +0.416 then Lati := +0.416 else if Lati < -0.416 then Lati := -0.416;
Loni := Lonu + phi * sin(Az * pi) / cos(Lati * pi);
Latm := Lati + 0.064 * cos((Loni - 1.617) * pi);
T := 4.32E+4 * Loni + Ttr;
while T >= 86400 do T := T - 86400 else while T < 0 do T := T + 86400;
F := 1.0 + 16.0 * (0.53 - El) * (0.53 - El) * (0.53 - El);
per := b0 + b1 * Latm + B2 * Latm * Latm + b3 * Latm * Latm * Latm;
if per < 72000.0 then per := 72000.0;
x := 2 * pi * (T - 50400.0) / per;
amp := a0 + a1 * Latm + A2 * Latm * Latm + a3 * Latm * Latm * Latm;
if amp < 0.0 then amp := 0.0;
if abs(x) >= 1.57 then dTiono := F * 5.0E-9 else
   dTiono := F * (5.0E-9 + amp * (1.0 - x * x / 2.0 + x * x * x * x /24.0))

end; {procedure ionocorr}

{***************************************************************************}

{At many places in the following procedure solve the subdeterminant value of a
 4 x 4 array is required. For convenience the function sub is defined below}

function sub (A : mat16;      {input 4 x 4 array}
              r,              {row number to be deleted}
              c : integer     {column number to be deleted}
              ) : double;     {value of 3 x 3 subdeterminant}

var
  B: array[1..3,1..3] of double;
  i, i1, j, j1: integer;

begin
i1 := 0;
for i := 1 to 4 do if i <> r then
   begin
   i1 := i1 + 1;
   j1 := 0;
   for j := 1 to 4 do if j <> c then
      begin
      j1 := j1 + 1;