1  #include <math.h>


2  #include <sstream>


3  #include <iostream>


4  #include <iomanip>


5  #include <cstring>


6 


7  #include <newmatio.h>


8 


9  #include "ephemeris.h"


10  #include "bncutils.h"


11  #include "timeutils.h"


12  #include "bnctime.h"


13 


14  using namespace std;


15 


16  //


17  ////////////////////////////////////////////////////////////////////////////


18  bool t_eph::isNewerThan(const t_eph* eph) const {


19  if (_GPSweek > eph>_GPSweek 


20  (_GPSweek == eph>_GPSweek && _GPSweeks > eph>_GPSweeks)) {


21  return true;


22  }


23  else {


24  return false;


25  }


26  }


27 


28  // Set GPS Satellite Position


29  ////////////////////////////////////////////////////////////////////////////


30  void t_ephGPS::set(const gpsephemeris* ee) {


31  ostringstream prn;


32  prn << 'G' << setfill('0') << setw(2) << ee>satellite;


33 


34  _prn = prn.str();


35 


36  // TODO: check if following two lines are correct


37  _GPSweek = ee>GPSweek;


38  _GPSweeks = ee>TOE;


39 


40  _TOW = ee>TOW;


41  _TOC = ee>TOC;


42  _TOE = ee>TOE;


43  _IODE = ee>IODE;


44  _IODC = ee>IODC;


45 


46  _clock_bias = ee>clock_bias ;


47  _clock_drift = ee>clock_drift ;


48  _clock_driftrate = ee>clock_driftrate;


49 


50  _Crs = ee>Crs;


51  _Delta_n = ee>Delta_n;


52  _M0 = ee>M0;


53  _Cuc = ee>Cuc;


54  _e = ee>e;


55  _Cus = ee>Cus;


56  _sqrt_A = ee>sqrt_A;


57  _Cic = ee>Cic;


58  _OMEGA0 = ee>OMEGA0;


59  _Cis = ee>Cis;


60  _i0 = ee>i0;


61  _Crc = ee>Crc;


62  _omega = ee>omega;


63  _OMEGADOT = ee>OMEGADOT;


64  _IDOT = ee>IDOT;


65 


66  _TGD = ee>TGD;


67  }


68 


69  // Compute GPS Satellite Position (virtual)


70  ////////////////////////////////////////////////////////////////////////////


71  void t_ephGPS::position(int GPSweek, double GPSweeks,


72  double* xc,


73  double* vv) const {


74 


75  static const double secPerWeek = 7 * 86400.0;


76  static const double omegaEarth = 7292115.1467e11;


77  static const double gmWGS = 398.6005e12;


78 


79  memset(xc, 0, 4*sizeof(double));


80  memset(vv, 0, 3*sizeof(double));


81 


82  double a0 = _sqrt_A * _sqrt_A;


83  if (a0 == 0) {


84  return;


85  }


86 


87  double n0 = sqrt(gmWGS/(a0*a0*a0));


88  double tk = GPSweeks  _TOE;


89  if (GPSweek != _GPSweek) {


90  tk += (GPSweek  _GPSweek) * secPerWeek;


91  }


92  double n = n0 + _Delta_n;


93  double M = _M0 + n*tk;


94  double E = M;


95  double E_last;


96  do {


97  E_last = E;


98  E = M + _e*sin(E);


99  } while ( fabs(EE_last)*a0 > 0.001 );


100  double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0  _e) )*tan( E/2 ) );


101  double u0 = v + _omega;


102  double sin2u0 = sin(2*u0);


103  double cos2u0 = cos(2*u0);


104  double r = a0*(1  _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;


105  double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;


106  double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;


107  double xp = r*cos(u);


108  double yp = r*sin(u);


109  double OM = _OMEGA0 + (_OMEGADOT  omegaEarth)*tk 


110  omegaEarth*_TOE;


111 


112  double sinom = sin(OM);


113  double cosom = cos(OM);


114  double sini = sin(i);


115  double cosi = cos(i);


116  xc[0] = xp*cosom  yp*cosi*sinom;


117  xc[1] = xp*sinom + yp*cosi*cosom;


118  xc[2] = yp*sini;


119 


120  double tc = GPSweeks  _TOC;


121  if (GPSweek != _GPSweek) {


122  tc += (GPSweek  _GPSweek) * secPerWeek;


123  }


124  xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc


125   4.442807633e10 * _e * sqrt(a0) *sin(E);


126 


127  // Velocity


128  // 


129  double tanv2 = tan(v/2);


130  double dEdM = 1 / (1  _e*cos(E));


131  double dotv = sqrt((1.0 + _e)/(1.0  _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)


132  * dEdM * n;


133  double dotu = dotv + (_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;


134  double dotom = _OMEGADOT  omegaEarth;


135  double doti = _IDOT + (_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;


136  double dotr = a0 * _e*sin(E) * dEdM * n


137  + (_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;


138  double dotx = dotr*cos(u)  r*sin(u)*dotu;


139  double doty = dotr*sin(u) + r*cos(u)*dotu;


140 


141  vv[0] = cosom *dotx  cosi*sinom *doty // dX / dr


142   xp*sinom*dotom  yp*cosi*cosom*dotom // dX / dOMEGA


143  + yp*sini*sinom*doti; // dX / di


144 


145  vv[1] = sinom *dotx + cosi*cosom *doty


146  + xp*cosom*dotom  yp*cosi*sinom*dotom


147   yp*sini*cosom*doti;


148 


149  vv[2] = sini *doty + yp*cosi *doti;


150  }


151 


152 


153  // Derivative of the state vector using a simple force model (static)


154  ////////////////////////////////////////////////////////////////////////////


155  ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv) {


156 


157  // State vector components


158  // 


159  ColumnVector rr = xv.rows(1,3);


160  ColumnVector vv = xv.rows(4,6);


161 


162  // Acceleration


163  // 


164  static const double GM = 398.60044e12;


165  static const double AE = 6378136.0;


166  static const double OMEGA = 7292115.e11;


167  static const double C20 = 1082.63e6;


168 


169  double rho = rr.norm_Frobenius();


170  double t1 = GM/(rho*rho*rho);


171  double t2 = 3.0/2.0 * C20 * (GM*AE*AE) / (rho*rho*rho*rho*rho);


172  double t3 = OMEGA * OMEGA;


173  double t4 = 2.0 * OMEGA;


174  double z2 = rr(3) * rr(3);


175 


176  // Vector of derivatives


177  // 


178  ColumnVector va(6);


179  va(1) = vv(1);


180  va(2) = vv(2);


181  va(3) = vv(3);


182  va(4) = (t1 + t2*(1.05.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2);


183  va(5) = (t1 + t2*(1.05.0*z2/(rho*rho)) + t3) * rr(2)  t4*vv(1);


184  va(6) = (t1 + t2*(3.05.0*z2/(rho*rho)) ) * rr(3);


185 


186  return va;


187  }


188 


189  // Compute Glonass Satellite Position (virtual)


190  ////////////////////////////////////////////////////////////////////////////


191  void t_ephGlo::position(int GPSweek, double GPSweeks,


192  double* xc, double* vv) const {


193 


194  static const double secPerWeek = 7 * 86400.0;


195  static const double nominalStep = 10.0;


196 


197  memset(xc, 0, 4*sizeof(double));


198  memset(vv, 0, 3*sizeof(double));


199 


200  double dtPos = GPSweeks  _tt;


201  if (GPSweek != _GPSweek) {


202  dtPos += (GPSweek  _GPSweek) * secPerWeek;


203  }


204 


205  int nSteps = int(fabs(dtPos) / nominalStep) + 1;


206  double step = dtPos / nSteps;


207 


208  for (int ii = 1; ii <= nSteps; ii++) {


209  _xv = rungeKutta4(_tt, _xv, step, glo_deriv);


210  _tt += step;


211  }


212 


213  // Position and Velocity


214  // 


215  xc[0] = _xv(1);


216  xc[1] = _xv(2);


217  xc[2] = _xv(3);


218 


219  vv[0] = _xv(4);


220  vv[1] = _xv(5);


221  vv[2] = _xv(6);


222 


223  // Clock Correction


224  // 


225  double dtClk = GPSweeks  _GPSweeks;


226  if (GPSweek != _GPSweek) {


227  dtClk += (GPSweek  _GPSweek) * secPerWeek;


228  }


229  xc[3] = _tau + _gamma * dtClk;


230  }


231 


232  // IOD of Glonass Ephemeris (virtual)


233  ////////////////////////////////////////////////////////////////////////////


234  int t_ephGlo::IOD() const {


235 


236  bool old = false;


237 


238  if (old) { // 5 LSBs of iod are equal to 5 LSBs of tb


239  unsigned int tb = int(fmod(_GPSweeks,86400.0)); //sec of day


240  const int shift = sizeof(tb) * 8  5;


241  unsigned int iod = tb << shift;


242  return (iod >> shift);


243  }


244  else {


245  bncTime tGPS(_GPSweek, _GPSweeks);


246  int hlpWeek = _GPSweek;


247  int hlpSec = int(_GPSweeks);


248  int hlpMsec = int(_GPSweeks * 1000);


249  updatetime(&hlpWeek, &hlpSec, hlpMsec, 0);


250  bncTime tHlp(hlpWeek, hlpSec);


251  double diffSec = tGPS  tHlp;


252  bncTime tMoscow = tGPS + diffSec;


253  return int(tMoscow.daysec() / 900);


254  }


255  }


256 


257  // Set Glonass Ephemeris


258  ////////////////////////////////////////////////////////////////////////////


259  void t_ephGlo::set(const glonassephemeris* ee) {


260 


261  ostringstream prn;


262  prn << 'R' << setfill('0') << setw(2) << ee>almanac_number;


263  _prn = prn.str();


264 


265  int ww = ee>GPSWeek;


266  int tow = ee>GPSTOW;


267  updatetime(&ww, &tow, ee>tb*1000, 0); // Moscow > GPS


268 


269  _GPSweek = ww;


270  _GPSweeks = tow;


271  _E = ee>E;


272  _tau = ee>tau;


273  _gamma = ee>gamma;


274  _x_pos = ee>x_pos;


275  _x_velocity = ee>x_velocity;


276  _x_acceleration = ee>x_acceleration;


277  _y_pos = ee>y_pos;


278  _y_velocity = ee>y_velocity;


279  _y_acceleration = ee>y_acceleration;


280  _z_pos = ee>z_pos;


281  _z_velocity = ee>z_velocity;


282  _z_acceleration = ee>z_acceleration;


283  _health = 0;


284  _frequency_number = ee>frequency_number;


285  _tki = ee>tk3*60*60; if (_tki < 0) _tki += 86400;


286 


287  // Initialize status vector


288  // 


289  _tt = _GPSweeks;


290 


291  _xv(1) = _x_pos * 1.e3;


292  _xv(2) = _y_pos * 1.e3;


293  _xv(3) = _z_pos * 1.e3;


294  _xv(4) = _x_velocity * 1.e3;


295  _xv(5) = _y_velocity * 1.e3;


296  _xv(6) = _z_velocity * 1.e3;


297  }

