ogr_navsolve2.c

The OpenGPSRec receiver software runs on the real time operating system RTAI-Linux. It compiles with

/* 
 * IN:   ephemeris  eph   
 *       satellite GPS time  Ttr
 * OUT:  relativistic correction term  Trel
 *       satellite position  X  
 */
static void satellite_position_old( UINT16 prn, double eph[], double Ttr, double *Trel, double *X)
{
  double M0, dn, ec, A, W0, i0, w, Wdot, 
         Cuc, Cus, Crc, Crs, Cic, Cis, Toe,
         Idot, T, n0, n, M, E, Eold, 
         snu, cnu, nu, phi, du, dr, di, u, r, i,
         Xdash, Ydash, Wc;

//  static int first = 0;

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

#if 0
  if ( prn == SelPRN)
  {
    printf(" dn=%.8e M0=%.8e ec=%.8e A=%.8e Toe=%.8e W0=%.8e i0%.8e w=%.8e Wdot=%.8e Idot=%.8e\n", 
      dn, M0, ec, A, Toe, W0, i0, w, Wdot, Idot);
  }
#endif

  T = Ttr - Toe;
  if ( T > 302400.0)
    T -= 604800.0;
  else if (T < -302400.0)
    T += 604800.0;

//  printf(" T=%.12e Toe=%.12e\n", T, Toe);

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

  M = M0 + n * T;  
  E = M;   /* start value for E */

//  printf(" dn=%.9e Mo=%.9e ec=%.9e A=%.9e Toe=%.9e W0=%.9e i0=%.9e w=%.9e Wdot=%.9e idot=%.9e\n", 
//    dn, M0, ec, A, Toe, W0, i0, w, Wdot, Idot);

  do 
  {
    Eold = E;
    E = M + ec * sin( E);
  } while ( fabs( E - Eold) >= 1.0e-8);

//  printf(" M0=%.12e n=%.12e T=%.12e Ttr=%.12e Toe=%.12e E=%.12e\n", M0, n, T, Ttr, Toe, E);
//  printf(" E=%.9e M=%.9e ec=%.9e\n", E, M, ec);

  snu = sqrt(1 - ec * ec) * sin(E) / (1 - ec * cos(E));
  cnu = (cos(E) - ec) / (1 - ec * cos(E));

  nu = atan2( snu, cnu);

  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 - OMEGAE) * T - OMEGAE * Toe;

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

#if 1
  if ( prn == SelPRN)
  {
    printf(" du=%.8e dr=%.8e di=%.8e Xd=%.8e Yd=%.8e Wc=%.8e, X=[%.8e %.8e %.8e]\n", 
      du, dr, di, Xdash, Ydash, Wc, X[0], X[1], X[2]);
  }
#endif

  *Trel = FCONST * ec * eph[6] * sin( E);   /* relativistic correction term */

  return;
}
#endif


#if 0
/*
 * ECEF X [m], Y [m] , Z[m]
 * lat [rad], lon [rad] alt [m]
 * this procedure converts WGS-84 ECEF XYZ to Lat, Lon, Alt 
 * [above ellipsoid]
 */
static void xyz2lla( double *Xi, double *Xo)
{
  double p, T, sT, cT, N;
//  double Xi[3];

//  Xi[0] = Xi0[0] - OS_DX;
//  Xi[1] = Xi0[1] - OS_DY;
//  Xi[2] = Xi0[2] - OS_DZ;

  p = sqrt( Xi[0] * Xi[0] + Xi[1] * Xi[1]);
  T = atan( Xi[2] * EARTH_MAJ_AXIS / (p * EARTH_MIN_AXIS));

  sT = sin( T);
  cT = cos( T);

  Xo[0] = atan((Xi[2] + E2SQR * EARTH_MIN_AXIS * sT * sT * sT) / 
               (p - E1SQR * EARTH_MAJ_AXIS * cT * cT * cT));

  Xo[1] = atan2( Xi[1], Xi[0]);

  N = EARTH_MAJ_AXIS / sqrt(1.0 - E1SQR * sin( Xo[0]) * sin( Xo[0]));
  Xo[2] = p / cos( Xo[0]) - N;

  return;
}

/* 
 * lat [rad], lon [rad] alt [m]
 * ECEF X [m], Y [m] , Z[m]
 * this procedure converts WGS-84 Lat, Lon and Alt 
 * [above ellipsoid] to ECEF XYZ
 */
static void lla2xyz( double *Xi, double *Xo)
{

  double N;

  N = EARTH_MAJ_AXIS / sqrt(1.0 - E1SQR * sin( Xi[0]) * sin( Xi[0]));

  Xo[0] = (N + Xi[2]) * cos( Xi[0]) * cos( Xi[1]);
  Xo[1] = (N + Xi[2]) * cos( Xi[0]) * sin( Xi[1]);
  Xo[2] = (N * (1.0 - E1SQR) + Xi[2]) * sin( Xi[0]);

//  Xo[0] += OS_DX;
//  Xo[1] += OS_DY;
//  Xo[2] += OS_DZ;

  return;
}
#endif


/*
 *  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
 *
 * IN:  A  input 4 x 4 array
 * IN:  r  row number to be deleted
 * IN:  c_  column number to be deleted
 * OUT:    value of 3 x 3 subdeterminant
 */
static double sub( double A[4][4], long r, long c_)
{
  double B[3][3];
  long i, i1, j, j1;

  i1 = 0;
  for (i = 0; i <= 3; i++) 
  {
    if (i + 1 != r)   
    {
      i1++;
      j1 = 0;
      for (j = 1; j <= 4; j++) 
      {
        if (j != c_) 
        {
          j1++;
          B[i1-1][j1-1] = A[i][j-1];
        }
      }
    }
  }
  return ( B[0][0] * (B[1][1] * B[2][2] - B[1][2] * B[2][1]) + 
           B[1][0] * (B[2][1] * B[0][2] - B[0][1] * B[2][2]) + 
           B[2][0] * (B[0][1] * B[1][2] - B[0][2] * B[1][1]));
} 

/*
 *
 *  IN  : Xs   array with 3 columns and 32 rows for the coordinates of the sat's
 *  IN  : SV   valid prn's
 *  IN  : P    pseudoranges
 *  IN  : Xr   input of initial final position
 *  OUT : Cr   receiver clock error
 *  OUT : status   TRUE: calculation OK, FALSE: no solution
 */
static int solve( int n_max, ECEF *rcvr_pos, ECEF xmit_pos[], 
  double *P, double *Cr)
{
  int retcode = 0;
  long it = 0, i, j, k, n;
  double R[NOFCHN], L[NOFCHN],
         A[NOFCHN][4],
         AL[4],
         AA[4][4], 
         AAi[4][4],
         det,
         D[4],
         Xs[NOFCHN][3],  // sat position
         Xr[3];          // rcvr position

  Xr[0] = rcvr_pos->x;
  Xr[1] = rcvr_pos->y;
  Xr[2] = rcvr_pos->z;

  for ( n = 0; n < n_max; n++) 
  {
    Xs[n][0] = xmit_pos[n].x;
    Xs[n][1] = xmit_pos[n].y;
    Xs[n][2] = xmit_pos[n].z;
  }

  do 
  {   /* iterations */

    it++;   /* increase iteration counter */

    for (n = 0; n < n_max; n++) 
    {
      R[n] = sqrt((Xr[0] - Xs[n][0]) * (Xr[0] - Xs[n][0]) +
                  (Xr[1] - Xs[n][1]) * (Xr[1] - Xs[n][1]) +
                  (Xr[2] - Xs[n][2]) * (Xr[2] - Xs[n][2]));

/* range from receiver to satellite */
      L[n] = P[n] - R[n];   /* range residual value */

/* A is the geometry matrix or model matrix */
      for ( k = 0; k < 3; k++)
        A[n][k] = (Xr[k] - Xs[n][k]) / R[n];

/* normalised user->satellite vectors */
      A[n][3] = -1.0;   
    }

    for (k = 0; k < 4; k++) 
    {     /*  calculate A.L  */
      AL[k] = 0.0;
      for (n = 0; n < n_max; n++) 
        AL[k] += A[n][k] * L[n];
    }

    for (k = 0; k < 4; k++) 
    {
      for (i = 0; i < 4; i++) 
      {     /*  calculate A.A  */
        AA[k][i] = 0.0;
        for ( n = 0; n < n_max; n++) 
          AA[k][i] += A[n][k] * A[n][i];
      }
    }

/* invert A.A */
    det = AA[0][0] * sub( AA, 1L, 1L) - AA[1][0] * sub( AA, 2L, 1L) + 
          AA[2][0] * sub( AA, 3L, 1L) - AA[3][0] * sub( AA, 4L, 1L);

    if ( det == 0.0)
/* there is something wrong if more than 6 iterations are required */
      retcode = OGRERR_DET_ZERO;
    else 
    {
      retcode = 0;

      for (k = 1; k <= 4; k++)
      {
        for (i = 1; i <= 4; i++) 
        {
          n = k + i;
          if (n & 1)
            j = -1;
          else
            j = 1;
          AAi[k-1][i-1] = j * sub( AA, i, k) / det;
        }
      }

/* calculate (invA.A).(A.L) */
      for (k = 0; k < 4; k++) 
      {
        D[k] = 0.0;
        for (i = 0; i < 4; i++)
          D[k] += AAi[k][i] * AL[i];
      }

/* update position */
      for (k = 0; k < 3; k++)
        Xr[k] += D[k];

    }
  } while ( it != 6 && retcode == 0 &&   
            fabs( D[0]) + fabs( D[1]) + fabs( D[2]) >= 1.0e-2);
  

  rcvr_pos->x = Xr[0];
  rcvr_pos->y = Xr[1];
  rcvr_pos->z = Xr[2];

  *Cr = D[3];   /* receiver clock error */

  if ( it == 6) 
    retcode = OGRERR_MAXITER_REACHED;

  return (retcode);
} 

#if 0
/*
 *
 *  IN  : Xs   array with 3 columns and 32 rows for the coordinates of the sat's
 *  IN  : SV   valid prn's
 *  IN  : P    pseudoranges
 *  IN  : Xr   input of initial final position
 *  OUT : Cr   receiver clock error
 *  OUT : status   TRUE: calculation OK, FALSE: no solution
 */
static void solve_old( double Xs[][3], int *SV, double *P, double *Xr, 
  double *Cr, int *status)
{
  long prn, it, i, j, k, n;
  double R[32], L[32],
         A[32][4],
         AL[4],
         AA[4][4], 
         AAi[4][4],
         det,
         D[4];

  it = 0;   /* iteration counter */

  do 
  {   /* iterations */

    it++;   /* increase iteration counter */

    for (prn = 0; prn <= 31; prn++) 
    {
      if (SV[prn])
      {
        R[prn] = sqrt((Xr[0] - Xs[prn][0]) * (Xr[0] - Xs[prn][0]) +
                      (Xr[1] - Xs[prn][1]) * (Xr[1] - Xs[prn][1]) +
                      (Xr[2] - Xs[prn][2]) * (Xr[2] - Xs[prn][2]));

/* range from receiver to satellite */
        L[prn] = P[prn] - R[prn];   /*range residual value*/

        for (k = 0; k <= 2; k++)
        {
          A[prn][k] = (Xr[k] - Xs[prn][k]) / R[prn];
        }
/* normalised user->satellite vectors */

        A[prn][3] = -1.0;   /*A is the geometry matrix or model matrix*/
      }
    }

    for (k = 0; k <= 3; k++) 
    {     /*  calculate A.L  */
      AL[k] = 0.0;
      for (prn = 0; prn <= 31; prn++) 
      {
        if (SV[prn])
          AL[k] += A[prn][k] * L[prn];
      }
    }

    for (k = 0; k <= 3; k++) 
    {
      for (i = 0; i <= 3; i++) 
      {     /*  calculate A.A  */
        AA[k][i] = 0.0;
        for (prn = 0; prn <= 31; prn++) 
        {
          if (SV[prn])
            AA[k][i] += A[prn][k] * A[prn][i];
        }
      }
    }

/* invert A.A */
    det = AA[0][0] * sub(AA, 1L, 1L) - AA[1][0] * sub(AA, 2L, 1L) + 
          AA[2][0] * sub(AA, 3L, 1L) - AA[3][0] * sub(AA, 4L, 1L);

    if (det == 0.0)
/* there is something wrong if more than 6 iterations are required */
      *status = FALSE;
    else 
    {
      *status = TRUE;

      for (k = 1; k <= 4; k++)
      {
        for (i = 1; i <= 4; i++) 
        {
          n = k + i;
          if (n & 1)
            j = -1;
          else
            j = 1;
          AAi[k-1][i-1] = j * sub(AA, i, k) / det;
        }
      }

/* calculate (invA.A).(A.L) */
      for (k = 0; k <= 3; k++) 
      {
        D[k] = 0.0;
        for (i = 0; i <= 3; i++)
        {
          D[k] += AAi[k][i] * AL[i];
        }
      }

/* update position */
      for (k = 0; k <= 2; k++)
        Xr[k] += D[k];
    }
  } while (it != 6 &&    /* there is something wrong if more than 6 iterations are required */
           fabs(D[0]) + fabs(D[1]) + fabs(D[2]) >= 1.0e-2  &&  /* iteration criterion */
           *status);                                           /* calculation not succeeded */
  
  *Cr = D[3];   /*receiver clock error*/

  if (it == 6) 
  {
//    printf("solve it : %12ld\n", it);
    *status = FALSE;
  }  /*iteration not succeeded*/
  return;
} 
#endif

#if 0
/*
 * find position from pseudorange data 
 */
static int calc_rcvr_pos_old( double Xlla[], double Xr[], double *Trc, 
  int SV[], double eph[32][16], double clk[32][5], double ion[], double Praw[])
{
  int prn, loop;
  static int status;
  static double  tmp3[3];
  static double  Ttr, tau, Trel, Tcor, Az, El, Cr, 
                 alpha, dTclck, dTiono,
                 dRtrop, Lat, Lon;
  
  static double Pcor[32];
  static double Xs[32][3];
//  static double P[32];
//  static double Xr[3];

//  Xlla[0] = Xlla[0] * M_PI / 180.0; /* convert angle from degrees to radians - david */
//  Xlla[1] = Xlla[1] * M_PI / 180.0; /* convert angle from degrees to radians - david  */

/*  convert lat, ln, alt to ECEF X, Y, Z  */
  lla2xyz( Xlla, Xr);

//  gotoxy(1,24);
//  printf("Xlla = [%e %e %e] \n", Xlla[0]*180.0/M_PI, Xlla[1]*180.0/M_PI, Xlla[2]);
//  printf("Xr   = [%e %e %e] \n", Xr[0], Xr[1], Xr[2]);

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

  for ( loop = 0; loop < 2; loop++) 
  {
    for ( prn = 1; prn <= 32; prn++) 
    {
      if ( SV[prn-1]) 
      { 
/*  
 * set all transit times to nominal value and calculate time of transmission
 */
        if ( loop == 0)
        {
          tau = 0.075;
          Cr  = 0.0;