ogr_solveosg.c

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

  delal  = GPS_Eph[n].cuc * cos( 2.0*aol) + 
           GPS_Eph[n].cus * sin( 2.0*aol);
//  correction to inclination
  delinc = GPS_Eph[n].cic * cos( 2.0*aol) + 
           GPS_Eph[n].cis * sin( 2.0*aol);

/*
 *  r IS THE RADIUS OF SATELLITE ORBIT AT TIME T
 */
  r   = GPS_Eph[n].sqra * GPS_Eph[n].sqra * 
        (1.0 - GPS_Eph[n].ety * cos( Ek)) + delr;
  aol += delal;
  inc = GPS_Eph[n].inc0 + delinc + GPS_Eph[n].idot * d_toe;

/*
 *  position in orbital plane
 */
  xp = r * cos( aol);
  yp = r * sin( aol);

/*
 *  Wk IS THE CORRECTED LONGITUDE OF THE ASCENDING NODE
 */
  Wk = GPS_Eph[n].W0 + 
       (GPS_Eph[n].Wdot - OMEGAE) * d_toe -
       OMEGAE * GPS_Eph[n].toe;

/*
 *  position in Earth-centered, Earth-fixed coordinates
 */
  res.x  = xp * cos( Wk) - yp * cos( inc) * sin( Wk);
  res.y  = xp * sin( Wk) + yp * cos( inc) * cos( Wk);
  res.z  = yp * sin( inc);

/*
 *  include relativistic correction term
 */
  res.tb = bclk + FCONST * GPS_Eph[n].ety * GPS_Eph[n].sqra * sin( Ek);

  res.az = 0.0;
  res.el = 0.0;

  if ( rec_pos_xyz.x || rec_pos_xyz.y || rec_pos_xyz.z)
  {
//               fprintf(stream,"x=%20.10lf,y=%20.10lf,z=%20.10lf\n",rec_pos_xyz.x,
//               rec_pos_xyz.y,rec_pos_xyz.z);

/*
 *  CALCULATE THE POSITION OF THE RECEIVER
 */
    xn  = -cos( rec_pos_llh.lon) * sin( rec_pos_llh.lat);
    yn  = -sin( rec_pos_llh.lon) * sin( rec_pos_llh.lat);
    zn  =  cos( rec_pos_llh.lat);

    xe  = -sin( rec_pos_llh.lon);
    ye  =  cos( rec_pos_llh.lon);

/*
 *  DETERMINE IF A CLEAR LINE OF SIGHT EXISTS
 */
    xls = res.x - rec_pos_xyz.x;
    yls = res.y - rec_pos_xyz.y;
    zls = res.z - rec_pos_xyz.z;

    range1 = sqrt( xls*xls + yls*yls + zls*zls);

    ralt = sqrt( rec_pos_xyz.x * rec_pos_xyz.x + 
                 rec_pos_xyz.y * rec_pos_xyz.y + 
                 rec_pos_xyz.z * rec_pos_xyz.z);

    tdot = ( rec_pos_xyz.x * xls + 
             rec_pos_xyz.y * yls + 
             rec_pos_xyz.z * zls) / range1 / ralt;

    xls = xls / range1;
    yls = yls / range1;
    zls = zls / range1;

    if ( tdot >= 1.0 )
      b = 0.0;
    else if ( tdot <= -1.0 )
      b = PI;
    else
      b = acos( tdot);

    res.el = PI / 2.0 - b;

    xaz = xe * xls + ye * yls;
    yaz = xn * xls + yn * yls + zn * zls;

    res.az = atan2( xaz, yaz);
  } // ---   if ( rec_pos_xyz.x || rec_pos_xyz.y || rec_pos_xyz.z) ---

  return (res);
}

#endif 

/*******************************************************************************
FUNCTION velocity()
RETURNS  None.

PARAMETERS None.

PURPOSE  To convert velocity from ecef to local level axes

WRITTEN BY
  Clifford Kelley

*******************************************************************************/
void velocity( LLH rp_llh)
{
  double  pt, pb,
          dxn, dyn, dzn,
          dn_mag, dxe, dye, de_mag,
          north, east;

  pt = (MAJAXIS * MAJAXIS -
        MINAXIS*MINAXIS) *
       sin( rp_llh.lat) * cos( rp_llh.lat);

  pb = sqrt( MAJAXIS * MAJAXIS *
        cos( rp_llh.lat) * cos( rp_llh.lat) +
        MINAXIS * MINAXIS *
        sin( rp_llh.lat) * sin( rp_llh.lat));

  dxn = MAJAXIS * MAJAXIS * pt / (pb*pb*pb) *
    cos( rp_llh.lat) * cos( rp_llh.lon) -
    (MAJAXIS * MAJAXIS / pb + rp_llh.hae) *
    sin( rp_llh.lat) * cos( rp_llh.lon);

  dyn = MAJAXIS * MAJAXIS * pt / (pb*pb*pb) *
    cos( rp_llh.lat) * sin( rp_llh.lon) -
    (MAJAXIS * MAJAXIS / pb + rp_llh.hae) *
    sin( rp_llh.lat) * sin( rp_llh.lon);

  dzn = MINAXIS * MINAXIS * pt / (pb*pb*pb) *
    sin( rp_llh.lat) + (MINAXIS * MINAXIS / pb + rp_llh.hae) *
    cos( rp_llh.lat);

  dn_mag = sqrt( dxn*dxn + dyn*dyn + dzn*dzn);     // north magnitude

  if ( dn_mag == 0.0)
    north = 0.0;
  else
    north = (rpvt.xv * dxn + rpvt.yv * dyn + rpvt.zv * dzn) / dn_mag;

  dxe = -(MAJAXIS*MAJAXIS / pb + rp_llh.hae) *
    cos( rp_llh.lat) * sin( rp_llh.lon);

  dye = (MAJAXIS*MAJAXIS / pb + rp_llh.hae) *
    cos( rp_llh.lat) * cos( rp_llh.lon);

  de_mag = sqrt( dxe*dxe + dye*dye);             // east magnitude

  if ( de_mag==0.0)
    east = 0.0;
  else
    east = (rpvt.xv*dxe + rpvt.yv*dye) / de_mag;

#if 0
  dxu = cos( rp_llh.lat) * cos( rp_llh.lon);
  dyu = cos( rp_llh.lat) * sin( rp_llh.lon);
  dzu = sin( rp_llh.lat);

  up = rpvt.xv*dxu + rpvt.yv*dyu + rpvt.zv*dzu;   // up magnitude
#endif

  Rcvr_Info.speed   = sqrt( north*north + east*east);
  Rcvr_Info.heading = atan2( east, north);

//  if ( out_vel)
//    fprintf( stream, "vel north=%lf, east=%lf, up=%lf\n",
//      north, east, up);

  return;
}

/*******************************************************************************
FUNCTION ecef_to_llh()
RETURNS  position in longitude/latitude/height coordinates

PARAMETERS
  Position in Earth-centered, earth-fixed coordinates

PURPOSE
  transform from Earth-centered, earth-fixed coordinates to
  longitude/latitude/height coordinates

WRITTEN BY
  Clifford Kelley

*******************************************************************************/

LLH ecef_to_llh( ECEF pos)
{
  double p, n, 
         thet, 
         sT, cT;
  LLH res;

  if ( pos.x == 0.0 && pos.y == 0.0 && pos.z == 0.0)
  {
    res.lon = 0.0;
    res.lat = 0.0;
    res.hae = 0.0;
    return (res);
  }

  p    = sqrt( pos.x * pos.x + pos.y * pos.y);
  thet = atan( pos.z * MAJAXIS / (p * MINAXIS));

//  esq  = 1.0 - MINAXIS*MINAXIS / (MAJAXIS*MAJAXIS);
//  epsq = MAJAXIS*MAJAXIS / (MINAXIS*MINAXIS) - 1.0;

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

  res.lat = atan2( pos.z + E2SQR * MINAXIS * sT * sT * sT,
                   p     - E1SQR * MAJAXIS * cT * cT * cT);
  res.lon = atan2( pos.y, pos.x);

  n = MAJAXIS*MAJAXIS /
    sqrt( MAJAXIS * MAJAXIS * cos( res.lat) * cos( res.lat) +
          MINAXIS * MINAXIS * sin( res.lat) * sin( res.lat));

  res.hae = p / cos( res.lat) - n;

  return (res);
}

/*******************************************************************************
FUNCTION llh_to_ecef()
RETURNS  position in Earth-centered, earth-fixed coordinates

PARAMETERS
  Position in longitude/latitude/height coordinates

PURPOSE
  transform from longitude/latitude/height coordinates to
  Earth-centered, earth-fixed coordinates

WRITTEN BY
  Clifford Kelley

*******************************************************************************/

ECEF llh_to_ecef( LLH pos)
{
  double n;
  ECEF res;

  n = MAJAXIS * MAJAXIS / 
      sqrt( MAJAXIS * MAJAXIS * cos( pos.lat) * cos( pos.lat) + 
            MINAXIS * MINAXIS * sin( pos.lat) * sin( pos.lat));

  res.x = (n + pos.hae) * cos( pos.lat) * cos( pos.lon);
  res.y = (n + pos.hae) * cos( pos.lat) * sin( pos.lon);
  res.z = (MINAXIS*MINAXIS / (MAJAXIS*MAJAXIS) * n + pos.hae)*sin( pos.lat);

  return (res);
}



#ifdef USE_OPENSRCGPS_NAVFIX

/*******************************************************************************
FUNCTION pos_vel_time( INT16)
RETURNS  None.

PARAMETERS 
  nsl : number of pseudoranges measured

PURPOSE
  This routine processes the all-in-view pseudorange to arrive
  at a receiver position

INPUTS:
    pseudo_range[nsl] Vector of measured range from satellites to the receiver
    sat_location[nsl][3] Array of satellite locations in ECEF when the signal was sent
    nsl      number of satellites used

OUTPUTS:
    RP[3]    VECTOR OF RECEIVER POSITION IN ECEF (X,Y,Z)
    CBIAS    RECEIVER CLOCK BIAS FROM GPS TIME

VARIABLES USED:
    C        SPEED OF LIGHT IN VACUUM IN M/S
    S[6][5]  MATRIX USED FOR SOLVING FOR RECEIVER POSITION CORRECTION
    B[5]     RESULTING RECEIVER CLOCK BIAS & POSITION CORRECTIONS
    X,Y,Z    TEMPORARY RECEIVER POSITION
    T        TEMPORARY RECEIVER CLOCK BIAS
    R[5]     RANGE FROM RECEIVER TO SATELLITES

IF THE POSITION CANNOT BE DETERMINED THE RESULT OF RP
WILL BE (0,0,0) THE CENTER OF THE EARTH

WRITTEN BY
  Clifford Kelley

*******************************************************************************/

static PVT pos_vel_time( INT16 nsl, ECEF d_sat[])
{
  INT16  i, j, k, 
         nits;
  double dd[5][5], r, 
         ms[5][13], 
         pm[5][13], 
         bm[13], 
         br[5],
         correct_mag = 0.0, x, y, z, t,
         a1, b1, c1, d1, e1, f1, g1, h1, 
         i1, j1, k1, l1, m1, n1, o1, p1,
         denom, alpha;
  PVT    res;

//
//  USE ITERATIVE APPROACH TO SOLVING FOR THE POSITION OF
//  THE RECEIVER
//
  nits = 0;
  t = 0.0;
  x = rec_pos_xyz.x;
  y = rec_pos_xyz.y;
  z = rec_pos_xyz.z;

  do
  {
    for ( i=1; i<=nsl; i++)
    {
/*
 *   Compute range in ECI at the time of arrival at the receiver
 */
      alpha = (dt[i]-t)*OMEGAE;
      r = sqrt( pow( track_sat[i].x*cos(alpha) - track_sat[i].y*sin(alpha)-x, 2.0)+
                pow( track_sat[i].y*cos(alpha) + track_sat[i].x*sin(alpha)-y, 2.0)+
                pow( track_sat[i].z-z, 2.0));
      bm[i] = r-(dt[i]-t)*LIGHTVEL;
      ms[1][i] = (track_sat[i].x*cos(alpha) - track_sat[i].y*sin(alpha)-x)/r;
      ms[2][i] = (track_sat[i].y*cos(alpha) + track_sat[i].x*sin(alpha)-y)/r;
      ms[3][i] = (track_sat[i].z-z)/r;
      ms[4][i] = 1.0;
    }

    a1=0.0; b1=0.0; c1=0.0; d1=0.0;
    e1=0.0; f1=0.0; g1=0.0; h1=0.0;
    i1=0.0; j1=0.0; k1=0.0; l1=0.0;
    m1=0.0; n1=0.0; o1=0.0; p1=0.0;

    for (k=1; k<=nsl; k++)
    {
      a1 += ms[1][k]*ms[1][k];
      b1 += ms[1][k]*ms[2][k];
      c1 += ms[1][k]*ms[3][k];
      d1 += ms[1][k]*ms[4][k];
//       e1+=ms[2][k]*ms[1][k];
      f1 += ms[2][k]*ms[2][k];
      g1 += ms[2][k]*ms[3][k];
      h1 += ms[2][k]*ms[4][k];
//       i1+=ms[3][k]*ms[1][k];
//       j1+=ms[3][k]*ms[2][k];
      k1 += ms[3][k]*ms[3][k];
      l1 += ms[3][k]*ms[4][k];
//       m1+=ms[1][k];
//       n1+=ms[2][k];
//       o1+=ms[3][k];
      p1 += ms[4][k];
    }
    o1=l1;
    m1=d1;
    n1=h1;
    e1=b1;
    i1=c1;
    j1=g1;

//
//  SOLVE FOR THE MATRIX INVERSE
//
    denom = (k1*p1-l1*o1)*(a1*f1-b1*e1) + 
            (l1*n1-j1*p1)*(a1*g1-c1*e1) + 
            (j1*o1-k1*n1)*(a1*h1-d1*e1) + 
            (l1*m1-i1*p1)*(c1*f1-b1*g1) + 
            (i1*o1-k1*m1)*(d1*f1-b1*h1) + 
            (i1*n1-j1*m1)*(c1*h1-d1*g1);

    dd[1][1] = f1*(k1*p1-l1*o1) + g1*(l1*n1-j1*p1) + h1*(j1*o1-k1*n1);
    dd[1][2] = e1*(l1*o1-k1*p1) + g1*(i1*p1-l1*m1) + h1*(k1*m1-i1*o1);
    dd[1][3] = e1*(j1*p1-n1*l1) - i1*(f1*p1-n1*h1) + m1*(f1*l1-j1*h1);
    dd[1][4] = e1*(n1*k1-j1*o1) + i1*(f1*o1-n1*g1) + m1*(j1*g1-f1*k1);
    dd[2][1] = b1*(l1*o1-k1*p1) + j1*(c1*p1-d1*o1) + n1*(d1*k1-c1*l1);
    dd[2][2] = a1*(k1*p1-l1*o1) + c1*(l1*m1-i1*p1) + d1*(i1*o1-k1*m1);
    dd[2][3] = a1*(l1*n1-j1*p1) + i1*(b1*p1-n1*d1) + m1*(j1*d1-b1*l1);
    dd[2][4] = a1*(j1*o1-n1*k1) - i1*(b1*o1-n1*c1) + m1*(b1*k1-c1*j1);
    dd[3][1] = b1*(g1*p1-h1*o1) - f1*(c1*p1-o1*d1) + n1*(c1*h1-d1*g1);
    dd[3][2] = a1*(o1*h1-g1*p1) + e1*(c1*p1-o1*d1) + m1*(d1*g1-c1*h1);
    dd[3][3] = a1*(f1*p1-h1*n1) + b1*(h1*m1-e1*p1) + d1*(e1*n1-f1*m1);
    dd[3][4] = a1*(n1*g1-f1*o1) + e1*(b1*o1-c1*n1) + m1*(c1*f1-b1*g1);
    dd[4][1] = b1*(h1*k1-g1*l1) + f1*(c1*l1-d1*k1) + j1*(d1*g1-c1*h1);
    dd[4][2] = a1*(g1*l1-h1*k1) - e1*(c1*l1-d1*k1) + i1*(c1*h1-d1*g1);
    dd[4][3] = a1*(j1*h1-f1*l1) + e1*(b1*l1-d1*j1) + i1*(d1*f1-b1*h1);
    dd[4][4] = a1*(f1*k1-g1*j1) + b1*(g1*i1-e1*k1) + c1*(e1*j1-f1*i1);

    if ( denom <= 0.0 )
    {
      res.x = 0.0;      // something went wrong
      res.y = 0.0;      // set solution to center of earth
      res.z = 0.0;
      res.dt = 0.0;
    }
    else
    {
      for (i=1; i<=4; i++)
      {
        for (j=1; j<=4; j++) 
          dd[i][j] = dd[i][j] / denom;
      }
      for (i=1; i<=nsl; i++)
      {
        for (j=1; j<=4; j++)
        {
          pm[j][i] = 0.0;
          for (k=1; k<=4; k++)
            pm[j][i] += dd[j][k]*ms[k][i];
        }
      }
      for ( i=1; i<=4; i++)
      {
        br[i] = 0.0;
        for (j=1; j<=nsl; j++)
          br[i] += bm[j]*pm[i][j];
      }

      nits++;

      x += br[1];
      y += br[2];
      z += br[3];
      t -= br[4] / LIGHTVEL;

//   printf("%lf,%lf,%lf,%20.10lf\n",x,y,z,t);
//   printf("%lf,%lf,%lf,%lf\n",br[1],br[2],br[3],br[4]);
      correct_mag = sqrt( br[1] * br[1] + br[2] * br[2] + br[3] * br[3]);
    }
  } while ( correct_mag > 0.01 && 
            correct_mag < 1.0e8 && 
            nits < 10);

  res.x  = x;
  res.y  = y;
  res.z  = z;
  res.dt = t;

/*
 *  Now for Velocity
 */
  for (i=1; i<=nsl; i++)
  {
    alpha = (dt[i]-t)*OMEGAE;
    r = sqrt( pow( track_sat[i].x*cos( alpha) - track_sat[i].y*sin( alpha)-x, 2.0)+
              pow( track_sat[i].y*cos( alpha) + track_sat[i].x*sin( alpha)-y, 2.0)+
              pow( track_sat[i].z - z, 2.0));

    bm[i] = ((track_sat[i].x * cos( alpha) - 
              track_sat[i].y * sin( alpha) - x) * d_sat[i].x +
             (track_sat[i].y * cos( alpha) + 
              track_sat[i].x * sin( alpha) - y) * d_sat[i].y +
             (track_sat[i].z - z) * d_sat[i].z) / r - meas_dop[i] * LAMBDA;

//    fprintf(out," %d meas_dop= %f bm= %f\n",i,meas_dop[i],bm[i]);
  }

  for (i=1; i<=4; i++)
  {
    br[i] = 0.0;
    for ( j=1; j<=nsl; j++)
      br[i] += bm[j]*pm[i][j];
  }

//  fprintf(out," %f  %f  %f  %f\n",pm[1][1],pm[1][2],pm[1][3],pm[1][4]);
//  fprintf(out," %f  %f  %f  %f\n",pm[2][1],pm[2][2],pm[2][3],pm[2][4]);
//  fprintf(out," %f  %f  %f  %f\n",pm[3][1],pm[3][2],pm[3][3],pm[3][4]);
//  fprintf(out," %f  %f  %f  %f\n",pm[4][1],pm[4][2],pm[4][3],pm[4][4]);

  res.xv = br[1] + y*OMEGAE;    //  get rid of earth
  res.yv = br[2] - x*OMEGAE;    //  rotation rate
  res.zv = br[3];

//  fprintf(out,"vel  x=%f,y=%f,z=%f\n",res.xv,res.yv,res.zv);
  res.df = br[4] / LIGHTVEL * 1000000.0;

  return (res);
}

#endif


#ifdef USE_OPENSRCGPS_NAVFIX

/*******************************************************************************
FUNCTION dops( INT16 nsl)

RETURNS  None.

PARAMETERS None.

PURPOSE
  This routine computes the dops

INPUTS:
  track_sat[nsl][3]  array of satellite locations in ECEF 
                     when the signal was sent
  nsl                number of satellites used
                     receiver position

OUTPUTS:
  hdop : horizontal dilution of precision
  vdop : vertical dilution of precision
  tdop : time dilution of precision
  pdop : position dilution of precision
  gdop : geometric dilution of precision (rss of pdop & tdop)

WRITTEN BY
  Clifford Kelley

*******************************************************************************/

static void dops( INT16 nsl)
{
  INT16  i, k;
  double ddx, ddy, ddz, 
         ddt, r, 
         rxy, 
         ms[5][13], 
         x, y, z,
         a1 = 0.0, b1 = 0.0, c1 = 0.0, d1 = 0.0, 
         e1 = 0.0, f1 = 0.0, g1 = 0.0, h1 = 0.0,
         i1 = 0.0, j1 = 0.0, k1 = 0.0, l1 = 0.0, 
         m1 = 0.0, n1 = 0.0, o1 = 0.0, p1 = 0.0, 
         denom,
         msx, msy, msz;
  ECEF   north, east, up;

  x       = rec_pos_xyz.x;
  y       = rec_pos_xyz.y;
  z       = rec_pos_xyz.z;
  r       = sqrt( x*x + y*y + z*z);
  rxy     = sqrt( x*x + y*y);
  north.x = -x/rxy * z/r;
  north.y = -y/rxy * z/r;
  north.z = rxy/r;
  east.x  = -y/rxy;
  east.y  = x/rxy;
//east.z=0.0; just for completeness
  up.x    = x/r;
  up.y    = y/r;
  up.z    = z/r;

  for (i=1; i<=nsl; i++)