singldif.pas

The idea behind differential GPS is to remove as much errors as possible from the range measurements

program singldif;

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, Trel: real;
   alpha, Rrs, Crm: real;
   prn, i: integer;
   eph: array[1..32,1..16] of real;
   Prs, Pms, Pmsc: vec32;
   Xs: mat96;
   SV: vecb32;
   Xr, Xm, 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}

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

{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;
      B[i1,j1] := A[i,j]
      end
   end;
sub := + B[1,1]*(B[2,2]*B[3,3] - B[2,3]*B[3,2])
       - B[2,1]*(B[1,2]*B[3,3] - B[3,2]*B[1,3])
       + B[3,1]*(B[1,2]*B[2,3] - B[1,3]*B[2,2]);
end; {function sub}

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

procedure solve(Xs    : mat96;    {array with 3 columns and 32 rows
                                   for the coordinates of the sat's}
                SV    : vecb32;   {valid prn's}
                P     : vec32;    {pseudoranges}
            var Xr    : vec3;     {input of initial guess, output of
                                   final position}
            var Cr    : real;     {receiver clock error}
            var status: boolean); {true: calculation OK, false: no solution}

{procedure solve requires the following types to be declared in the
 main body of the program:
 type
    mat96 = array[1..32,1..3] of real;
    vecb32 = array[1..32] of boolean;
    vec32 = array[1..32] of real;
    vec3 = array[1..3] of real;
    mat16 = array[1..4,1..4] of real;}

var
   prn, it, i, j, k: integer;
   R, L: array[1..32] of real;
   A: array[1..32,1..4] of real;
   AL: array[1..4] of real;
   AA, AAi: mat16;
   n: longint;
   det: real;
   D: array[1..4] of real;


begin {procedure solve}

it := 0; {iteration counter}

repeat {iterations}

   it :=it + 1; {increase iteration counter}

   for prn := 1 to 32 do if SV[prn] then
      begin
      R[prn] :=  {range from receiver to satellite}
         sqrt((Xr[1] - Xs[prn,1]) * (Xr[1] - Xs[prn,1])
            + (Xr[2] - Xs[prn,2]) * (Xr[2] - Xs[prn,2])
            + (Xr[3] - Xs[prn,3]) * (Xr[3] - Xs[prn,3]));
      L[prn] := P[prn] - R[prn]; {range residual value}
      for k := 1 to 3 do A[prn,k] := (Xr[k] - Xs[prn,k]) / R[prn];
      A[prn,4] := -1.0 {A is the geometry matrix or model matrix}
      end;

   For k :=1 to 4 do {calculate A.L}
      begin
      AL[k] := 0.0;
      for prn := 1 to 32 do if SV[prn] then
         AL[k] := AL[k] + A[prn,k] * L[prn]
      end;

   for k := 1 to 4 do for i := 1 to 4 do {calculate A.A}
      begin
      AA[k,i] :=0.0;
      for prn := 1 to 32 do if SV[prn] then
         AA[k,i] := AA[k,i] + A[prn,k] * A[prn,i]
      end;

   {invert A.A}
   det := + AA[1,1] * sub(AA,1,1) - AA[2,1] * sub(AA,2,1)
          + AA[3,1] * sub(AA,3,1) - AA[4,1] * sub(AA,4,1);
   if det = 0.0 then status := false else
      begin

      status := true;

      for k := 1 to 4 do for i := 1 to 4 do
         begin
         n:= k + i; if odd(n) then j := -1 else j :=1;
         AAi[k,i] := j * sub(AA,i,k) / det
         end;

      {calculate (invA.A).(A.L)}
      for k := 1 to 4 do
         begin
         D[k] := 0.0;
         for i := 1 to 4 do D[k] := D[k] + AAi[k,i] * AL[i]
         end;

      {update position}
      for k := 1 to 3 do Xr[k] := Xr[k] + D[k];

      end;

   until (it = 6){there is something wrong if more than 6 iterations are required}
      or ((abs(D[1]) + abs(D[2]) + abs(D[3])) < 1.0E-2) {iteration criterion}
         or (not(status)); {calculation not succeeded}

Cr := D[4]; {receiver clock error}

if it = 6 then begin writeln('solve it : ',it); status := false end; {iteration not succeeded}

end; {procedure solve}

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

begin {main}
{the following data should be available:
 1. Pseudoranges at ref rcvr with receiver time of reception for each SV
 2. Ephemeris for each SV
 3. Pseudoranges at mov rcvr}

{open input datafile}
assign(inp,'inpsd.txt'); reset(inp);

{read GPS time of reception and ref rcvr coordinates}
readln(inp); {skip comment line}
readln(inp,Trc,Xr[1],Xr[2],Xr[3]);

{read pseudoranges}
readln(inp); {skip comment line}
for prn := 1 to 32 do SV[prn] := false;
repeat
   read(inp,prn);
   if prn <> 0 then
      begin
      readln(inp,Prs[prn],Pms[prn]);
      SV[prn] := true
      end
   else readln(inp);
   until prn = 0;

readln(inp); {skip comment line}
{read ephemeris data}
repeat
   readln(inp,prn);
   for i := 1 to 16 do readln(inp,eph[prn,i]);
   until eof(inp);
close(inp);

{open output data file}
assign(out,'outsd.txt'); rewrite(out);

for prn := 1 to 32 do if SV[prn] then begin {do for each SV}

   {calculate transit time and time of transmission}
   tau := Prs[prn] / c;
   Ttr := Trc - tau;

   {calculate SV position and correct for earth rotation}
   for i := 1 to 16 do tmp16[i] := eph[prn,i];
   satpos(tmp16,Ttr,Trel,tmp3);
   alpha := tau * We;
   Xs[prn,1] := + tmp3[1] * cos(alpha) + tmp3[2] * sin(alpha);
   Xs[prn,2] := - tmp3[1] * sin(alpha) + tmp3[2] * cos(alpha);
   Xs[prn,3] := + tmp3[3];
   writeln(out,'SV     : ',prn:2,Xs[prn,1]:15:3,Xs[prn,2]:15:3,Xs[prn,3]:15:3);

   {calculate differential corrected pseudorange}
   Rrs := sqrt((Xr[1] - Xs[prn,1]) * (Xr[1] - Xs[prn,1])
             + (Xr[2] - Xs[prn,2]) * (Xr[2] - Xs[prn,2])
             + (Xr[3] - Xs[prn,3]) * (Xr[3] - Xs[prn,3]));
   Pmsc[prn] := Pms[prn] - (Prs[prn] - Rrs);

   end; {do for each SV}

{calculate receiver position}
for i := 1 to 3 do Xm[i] := Xr[i]; {initial guess for mov rcvr position}
solve(Xs,SV,Pmsc,Xm,Crm,status);
if not status then
   begin writeln('Error in solve - check input data'); exit end;
writeln(out,'Pos XYZ: ',Xm[1]:12:3,Xm[2]:12:3,Xm[3]:12:3,Crm:12:3);
{convert back to Lat, Lon, Alt}
XYZ2LLA(Xm,Xlla);
writeln(out,'Pos LLA: ',Xlla[1]*180.0/pi:15:8,Xlla[2]*180.0/pi:15:8,Xlla[3]:12:3);

close(out)
end. {main}