stdalone.pas

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

      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. Pseudorange with receiver time of reception for each SV
 2. Ephemeris and almanac for each SV
 3. Iono coefficients}

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

{read GPS time of reception}
readln(inp); {skip comment line}
readln(inp,Trc);

{read iono coefficients}
readln(inp); {skip comment line}
for i := 1 to 8 do readln(inp,ion[i]);

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

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

{user input of start position}
write('Start position Lat [deg.dec], Lon [deg.dec], Alt [m] : ');
   readln(Xlla[1],Xlla[2],Xlla[3]);
Xlla[1] := Xlla[1] * pi / 180.0; Xlla[2] :=Xlla[2] * pi / 180.0;
{convert lat, ln, alt to ECEF X, Y, Z}
LLA2XYZ(Xlla,Xr);

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

{assuming the receiver clock error and initial position not sufficiently
 good known, I make two passes through the processing steps}

{PASS 1}
writeln(out,'PASS 1');

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

   {set all transit times to nominal value and calculate time of transmission}
   tau := 0.075;
   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 azimuth and elevation}
   for i := 1 to 3 do tmp3[i] := Xs[prn,i];
   calcAzEl(tmp3,Xr,Az,El,status);
   if not status then
      begin writeln('Error in calcAzEl - check input data'); exit end;
   writeln(out,'Az, El : ',prn:2,Az*180.0/pi:11:3,El*180.0/pi:10:3);

   {calculate pseudorange corrections and apply to pseudoranges}

   {clock correction}
   T := Ttr - clk[prn,2];
   {correct for week crossover}
   if T >  302400 then T := T - 604800;
   if T < -302400 then T := T + 604800;

   dTclck := + clk[prn,5] + clk[prn,4] * T + clk[prn,3] * T * T
             + Trel - clk[prn,1];

   {iono correction}
   Lat := Xlla[1] ; Lon := Xlla[2];
   ionocorr(ion,Lat,Lon,Az,El,Ttr,dTiono);

   {tropo correction using standard atmosphere values}
   dRtrop := + 2.312 / sin(sqrt(El * El + 1.904E-3))
             + 0.084 / sin(sqrt(El * El + 0.6854E-3));

   writeln(out,'Corr   : ',prn:2,dTclck*c:11:3,dTiono*c:10:3,dRtrop:10:3);

   {correct pseudorange}
   Pcor[prn] := Praw[prn] + dTclck * c - dTiono * c - dRtrop

   end; {do for each SV}

{calculate receiver position}
solve(Xs,SV,Pcor,Xr,Cr,status);
if not status then
   begin writeln('Error in solve - check input data'); exit end;
writeln(out,'Pos XYZ: ',Xr[1]:12:3,Xr[2]:12:3,Xr[3]:12:3,Cr:12:3);
{convert back to Lat, Lon, Alt}
XYZ2LLA(Xr,Xlla);

{PASS 2 - The receiver position and -clock error is now well enough known
 to calculate the final pseudorange corrections}
writeln(out); writeln(out,'PASS 2');

{correct receiver clock}
Trc := Trc + Cr / c;

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

   {recalculate transit time and time of transmission}
   tau := (Pcor[prn] + Cr) / c;
   Ttr := Trc - tau;

   {recalculate 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);

   {recalculate azimuth and elevation}
   for i := 1 to 3 do tmp3[i] := Xs[prn,i];
   calcAzEl(tmp3,Xr,Az,El,status);
   if not status then
      begin writeln('Error in calcAzEl - check input data'); exit end;
   writeln(out,'Az, El : ',prn:2,Az*180.0/pi:11:3,El*180.0/pi:10:3);

   {recalculate pseudorange corrections and apply to pseudoranges}

   {clock correction}
   T := Ttr - clk[prn,2];
   {correct for week crossover}
   if T >  302400 then T := T - 604800;
   if T < -302400 then T := T + 604800;
   dTclck := + clk[prn,5] + clk[prn,4] * T + clk[prn,3] * T * T
             + Trel - clk[prn,1];

   {iono correction}
   Lat := Xlla[1]; Lon := Xlla[2];
   ionocorr(ion,Lat,Lon,Az,El,Ttr,dTiono);

   {tropo correction using standard atmosphere values}
   dRtrop := + 2.312 / sin(sqrt(El * El + 1.904E-3))
             + 0.084 / sin(sqrt(El * El + 0.6854E-3));

   writeln(out,'Corr   : ',prn:2,dTclck*c:11:3,dTiono*c:10:3,dRtrop:10:3);

   {correct pseudorange}
   Pcor[prn] := Praw[prn] + dTclck * c - dTiono * c - dRtrop + Cr

   end; {do for each SV}

{calculate receiver position}
solve(Xs,SV,Pcor,Xr,Cr,status);
if not status then
   begin writeln('Error in solve - check input data'); exit end;
writeln(out,'Pos XYZ: ',Xr[1]:12:3,Xr[2]:12:3,Xr[3]:12:3,Cr:12:3);
{convert back to Lat, Lon, Alt}
XYZ2LLA(Xr,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}