ephemeris.c
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2010 Swift Navigation Inc.
00003  * Contact: Henry Hallam <henry@swift-nav.com>
00004  *
00005  * This source is subject to the license found in the file 'LICENSE' which must
00006  * be be distributed together with this source. All other rights reserved.
00007  *
00008  * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
00009  * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
00010  * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
00011  */
00012 
00013 #include "linear_algebra.h"
00014 #include "ephemeris.h"
00015 #include <math.h>
00016 #include <stdlib.h>
00017 #include <stdio.h>
00018 
00019 #define NAV_OMEGAE_DOT 7.2921151467e-005
00020 #define NAV_GM 3.986005e14
00021 
00022 int calc_sat_pos(double pos[3], double vel[3],
00023              double *clock_err, double *clock_rate_err,
00024              const ephemeris_t *ephemeris,
00025              gps_time_t tot)
00026 {
00027   /****************************************************************************
00028    * Calculate satellite position, velocity and clock offset from ephemeris
00029    * Taken from IS-GPS-200D, Section 20.3.3.3.3.1 and Table 20-IV
00030    *
00031    * Usage: unsigned int ProcessEphemeris(unsigned int week, double secs,
00032    *                              unsigned int sv, nav_info_t *Eph)
00033    *  week: GPS week number
00034    *  sec: GPS seconds of the week of time of transmission
00035    *  sv: Satellite vehicle number
00036    *    Eph: Ephemeris data structure located within channel structure
00037    ****************************************************************************/
00038 
00039   double tempd1 = 0, tempd2, tempd3;
00040   double tdiff;
00041   double a;     // semi major axis
00042   double ma, ma_dot;    // mean anomoly and first derivative (mean motion)
00043   double ea, ea_dot, ea_old;  // eccentric anomoly, first deriv, iteration var
00044   double einstein;    // relativistic correction
00045   double al, al_dot;    // argument of lattitude and first derivative
00046   double cal, cal_dot;    // corrected argument of lattitude and first deriv
00047   double r, r_dot;    // radius and first derivative
00048   double inc, inc_dot;    // inclination and first derivative
00049   double x, x_dot, y, y_dot;  // position in orbital plan and first derivatives
00050   double om, om_dot;    // omega and first derivatives
00051 
00052 
00053   // Satellite clock terms
00054   // Seconds from clock data reference time (toc)
00055   tdiff = gpsdifftime(tot, ephemeris->toc);
00056 
00057   *clock_err = ephemeris->af0 + tdiff * (ephemeris->af1 + tdiff * ephemeris->af2) - ephemeris->tgd;
00058   *clock_rate_err = ephemeris->af1 + 2.0 * tdiff * ephemeris->af2;
00059 
00060   // Seconds from the time from ephemeris reference epoch (toe)
00061   tdiff = gpsdifftime(tot, ephemeris->toe);
00062 
00063   // If tdiff is too large our ephemeris isn't valid, maybe we want to wait until we get a
00064   // new one? At least let's warn the user.
00065   // TODO: this doesn't exclude ephemerides older than a week so could be made better.
00066   if (abs(tdiff) > 4*3600)
00067     printf(" WARNING: using ephemeris older (or newer!) than 4 hours.\n");
00068 
00069   // Calculate position per IS-GPS-200D p 97 Table 20-IV
00070   a = ephemeris->sqrta * ephemeris->sqrta;  // [m] Semi-major axis
00071   ma_dot = sqrt (NAV_GM / (a * a * a)) + ephemeris->dn; // [rad/sec] Corrected mean motion
00072   ma = ephemeris->m0 + ma_dot * tdiff;  // [rad] Corrected mean anomaly
00073 
00074   // Iteratively solve for the Eccentric Anomaly (from Keith Alter and David Johnston)
00075   ea = ma;      // Starting value for E
00076 
00077   do {
00078     ea_old = ea;
00079     tempd1 = 1.0 - ephemeris->ecc * cos (ea_old);
00080     ea = ea + (ma - ea_old + ephemeris->ecc * sin (ea_old)) / tempd1;
00081   } while (fabs (ea - ea_old) > 1.0E-14);
00082   ea_dot = ma_dot / tempd1;
00083 
00084   // Relativistic correction term
00085   einstein = -4.442807633E-10 * ephemeris->ecc * ephemeris->sqrta * sin (ea);
00086 
00087   // Begin calc for True Anomaly and Argument of Latitude
00088   tempd2 = sqrt (1.0 - ephemeris->ecc * ephemeris->ecc);
00089   al = atan2 (tempd2 * sin (ea), cos (ea) - ephemeris->ecc) + ephemeris->w; // [rad] Argument of Latitude = True Anomaly + Argument of Perigee
00090   al_dot = tempd2 * ea_dot / tempd1;
00091 
00092   // Calculate corrected argument of latitude based on position
00093   cal = al + ephemeris->cus * sin (2.0 * al) + ephemeris->cuc * cos (2.0 * al);
00094   cal_dot =
00095     al_dot * (1.0 +
00096         2.0 * (ephemeris->cus * cos (2.0 * al) -
00097          ephemeris->cuc * sin (2.0 * al)));
00098 
00099   // Calculate corrected radius based on argument of latitude
00100   r =
00101     a * tempd1 + ephemeris->crc * cos (2.0 * al) +
00102     ephemeris->crs * sin (2.0 * al);
00103   r_dot =
00104     a * ephemeris->ecc * sin (ea) * ea_dot +
00105     2.0 * al_dot * (ephemeris->crs * cos (2.0 * al) -
00106         ephemeris->crc * sin (2.0 * al));
00107 
00108   // Calculate inclination based on argument of latitude
00109   inc =
00110     ephemeris->inc + ephemeris->inc_dot * tdiff +
00111     ephemeris->cic * cos (2.0 * al) + ephemeris->cis * sin (2.0 * al);
00112   inc_dot =
00113     ephemeris->inc_dot + 2.0 * al_dot * (ephemeris->cis * cos (2.0 * al) -
00114            ephemeris->cic * sin (2.0 * al));
00115 
00116   // Calculate position and velocity in orbital plane
00117   x = r * cos (cal);
00118   y = r * sin (cal);
00119   x_dot = r_dot * cos (cal) - y * cal_dot;
00120   y_dot = r_dot * sin (cal) + x * cal_dot;
00121 
00122   // Corrected longitude of ascenting node
00123   om_dot = ephemeris->omegadot - NAV_OMEGAE_DOT;
00124   om = ephemeris->omega0 + tdiff * om_dot - NAV_OMEGAE_DOT * ephemeris->toe.tow;
00125 
00126   // Compute the satellite's position in Earth-Centered Earth-Fixed coordiates
00127   pos[0] = x * cos (om) - y * cos (inc) * sin (om);
00128   pos[1] = x * sin (om) + y * cos (inc) * cos (om);
00129   pos[2] = y * sin (inc);
00130 
00131   tempd3 = y_dot * cos (inc) - y * sin (inc) * inc_dot;
00132 
00133   // Compute the satellite's velocity in Earth-Centered Earth-Fixed coordiates
00134   vel[0] = -om_dot * pos[1] + x_dot * cos (om) - tempd3 * sin (om);
00135   vel[1] = om_dot * pos[0] + x_dot * sin (om) + tempd3 * cos (om);
00136   vel[2] = y * cos (inc) * inc_dot + y_dot * sin (inc);
00137 
00138   *clock_err += einstein;
00139 
00140   return 0;
00141 }
00142 
00143 double predict_range(double rx_pos[3],
00144                      gps_time_t tot,
00145                      ephemeris_t *ephemeris)
00146 {
00147   double sat_pos[3];
00148   double sat_vel[3];
00149   double temp[3];
00150   double clock_err, clock_rate_err;
00151 
00152   calc_sat_pos(sat_pos, sat_vel, &clock_err, &clock_rate_err, ephemeris, tot);
00153 
00154   vector_subtract(3, sat_pos, rx_pos, temp); // temp = sat_pos - rx_pos
00155   return vector_norm(3, temp);
00156 }
00157 
00158 


enu
Author(s): Mike Purvis
autogenerated on Fri Jan 3 2014 11:21:07