ogr_navsolve.c

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

  thet = atan( pos.z * EARTH_MAJ_AXIS / (p*EARTH_MIN_AXIS));
  esq  = 1.0 - EARTH_MIN_AXIS*EARTH_MIN_AXIS / (EARTH_MAJ_AXIS*EARTH_MAJ_AXIS);
  epsq = EARTH_MAJ_AXIS*EARTH_MAJ_AXIS / (EARTH_MIN_AXIS*EARTH_MIN_AXIS) - 1.0;

  res.lat = atan( (pos.z + epsq*EARTH_MIN_AXIS * pow( sin( thet), 3.0)) /
                  (p     - esq*EARTH_MAJ_AXIS*pow( cos( thet), 3.0)));
  res.lon = atan2( pos.y, pos.x);

  n = EARTH_MAJ_AXIS*EARTH_MAJ_AXIS /
    sqrt( EARTH_MAJ_AXIS * EARTH_MAJ_AXIS * cos( res.lat) * cos( res.lat) +
          EARTH_MIN_AXIS * EARTH_MIN_AXIS * 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 = EARTH_MAJ_AXIS * EARTH_MAJ_AXIS / 
      sqrt( EARTH_MAJ_AXIS * EARTH_MAJ_AXIS * 
            cos( pos.lat) * cos( pos.lat) + 
            EARTH_MIN_AXIS * EARTH_MIN_AXIS * 
            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 = (EARTH_MIN_AXIS*EARTH_MIN_AXIS /
           (EARTH_MAJ_AXIS*EARTH_MAJ_AXIS)*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.e8 && 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++)
  {
/*
 *   Compute line of sight vectors
 */
    r = sqrt( pow( track_sat[i].x-x, 2.0) +
              pow( track_sat[i].y-y, 2.0)+
              pow( track_sat[i].z-z, 2.0));
    ms[1][i] = (track_sat[i].x-x) / r;
    ms[2][i] = (track_sat[i].y-y) / r;
    ms[3][i] = (track_sat[i].z-z) / r;
    ms[4][i] = 1.0;
  }

  for (i=1; i<=nsl; i++)
  {
    msx = ms[1][i]*north.x + ms[2][i]*north.y + ms[3][i]*north.z;
    msy = ms[1][i]*east.x  + ms[2][i]*east.y;
    msz = ms[1][i]*up.x    + ms[2][i]*up.y    + ms[3][i]*up.z;

    ms[1][i] = msx;
    ms[2][i] = msy;
    ms[3][i] = msz;
    ms[4][i] = 1.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 DIAGONALS OF 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);

  ddx = f1*(k1*p1-l1*o1) + g1*(l1*n1-j1*p1) + h1*(j1*o1-k1*n1);
  ddy = a1*(k1*p1-l1*o1) + c1*(l1*m1-i1*p1) + d1*(i1*o1-k1*m1);
  ddz = a1*(f1*p1-h1*n1) + b1*(h1*m1-e1*p1) + d1*(e1*n1-f1*m1);
  ddt = a1*(f1*k1-g1*j1) + b1*(g1*i1-e1*k1) + c1*(e1*j1-f1*i1);

  if ( denom <= 0.0 )
  {
    Dops.hdop = 1.0e6;      // something went wrong
    Dops.vdop = 1.0e6;      // set dops to a large number
    Dops.tdop = 1.0e6;
    Dops.pdop = 1.0e6;
    Dops.gdop = 1.0e6;
  }
  else
  {
    Dops.hdop = sqrt( (ddx+ddy) / denom);
    Dops.vdop = sqrt( ddz / denom);
    Dops.tdop = sqrt( ddt / denom);
    Dops.pdop = sqrt( Dops.vdop*Dops.vdop + Dops.hdop*Dops.hdop);
    Dops.gdop = sqrt( Dops.pdop*Dops.pdop + Dops.tdop*Dops.tdop);
  }
  return;
}

#endif

#ifdef USE_OPENSRCGPS_NAVFIX

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

PARAMETERS 
  az       - azimuth
  el       - elevation
  gps_time - time

PURPOSE
  This function corrects the pseudoranges with a tropospheric model
  and the broadcast ionospheric message corrections.

WRITTEN BY
  Clifford Kelley

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

static double tropo_iono( double az, double el, double gps_time)
{
  double d_Trop, d_Ion, 
         psi, phi, 
         lambdai, phim, per, 
         x, F, amp, t, alt_factor;

// troposphere model
  if ( current_loc.hae > 200000.0) 
    alt_factor = 0.0;
  else if ( current_loc.hae < 0.0) 
    alt_factor = 1.0;
  else 
    alt_factor = exp( -current_loc.hae * 1.33e-4);