Index: trunk/BNS/bns.cpp =================================================================== --- trunk/BNS/bns.cpp (revision 883) +++ trunk/BNS/bns.cpp (revision 884) @@ -39,5 +39,5 @@ _bnseph = new t_bnseph(parent); - connect(_bnseph, SIGNAL(newEph(gpsEph*)), this, SLOT(slotNewEph(gpsEph*))); + connect(_bnseph, SIGNAL(newEph(t_eph*)), this, SLOT(slotNewEph(t_eph*))); connect(_bnseph, SIGNAL(newMessage(QByteArray)), this, SLOT(slotMessage(const QByteArray))); @@ -229,15 +229,15 @@ // //////////////////////////////////////////////////////////////////////////// -void t_bns::slotNewEph(gpsEph* ep) { +void t_bns::slotNewEph(t_eph* ep) { QMutexLocker locker(&_mutex); t_ephPair* pair; - if ( !_ephList.contains(ep->prn) ) { + if ( !_ephList.contains(ep->prn()) ) { pair = new t_ephPair(); - _ephList.insert(ep->prn, pair); - } - else { - pair = _ephList[ep->prn]; + _ephList.insert(ep->prn(), pair); + } + else { + pair = _ephList[ep->prn()]; } @@ -246,6 +246,5 @@ } else { - if (ep->GPSweek > pair->eph->GPSweek || - (ep->GPSweek == pair->eph->GPSweek && ep->TOC > pair->eph->TOC)) { + if (ep->isNewerThan(pair->eph)) { delete pair->oldEph; pair->oldEph = pair->eph; @@ -328,5 +327,5 @@ QString prn; ColumnVector xx(4); - gpsEph* ep = 0; + t_eph* ep = 0; if (oldEph == 0) { @@ -344,5 +343,5 @@ if ( _ephList.contains(prn) ) { t_ephPair* pair = _ephList[prn]; - prn = pair->eph->prn; + prn = pair->eph->prn(); xx = pair->xx; ep = pair->oldEph; @@ -379,5 +378,5 @@ // //////////////////////////////////////////////////////////////////////////// -void t_bns::processSatellite(gpsEph* ep, int GPSweek, double GPSweeks, +void t_bns::processSatellite(t_eph* ep, int GPSweek, double GPSweeks, const QString& prn, const ColumnVector& xx, struct ClockOrbit::SatData* sd) { @@ -386,6 +385,5 @@ ColumnVector vv(3); - satellitePosition(GPSweek, GPSweeks, ep, xB(1), xB(2), xB(3), xB(4), - vv(1), vv(2), vv(3)); + ep->position(GPSweek, GPSweeks, xB, vv); ColumnVector dx = xx.Rows(1,3) - xB.Rows(1,3); @@ -397,5 +395,5 @@ if (sd) { sd->ID = prn.mid(1).toInt(); - sd->IOD = int(ep->IODE); + sd->IOD = ep->IOD(); sd->Clock.DeltaA0 = dClk; sd->Orbit.DeltaRadial = rsw(1); @@ -406,8 +404,8 @@ if (_outStream) { QString line; - line.sprintf("%d %.1f %s %3d %3d %8.3f %8.3f %8.3f %8.3f\n", - GPSweek, GPSweeks, ep->prn.toAscii().data(), - int(ep->IODC), int(ep->IODE), dClk, rsw(1), rsw(2), rsw(3)); - *_outStream << line; + line.sprintf("%d %.1f %s %3d %8.3f %8.3f %8.3f %8.3f\n", + GPSweek, GPSweeks, ep->prn().toAscii().data(), + ep->IOD(), dClk, rsw(1), rsw(2), rsw(3)); + *_outStream << line; _outStream->flush(); } Index: trunk/BNS/bns.h =================================================================== --- trunk/BNS/bns.h (revision 883) +++ trunk/BNS/bns.h (revision 884) @@ -28,6 +28,6 @@ ColumnVector xx; - gpsEph* eph; - gpsEph* oldEph; + t_eph* eph; + t_eph* oldEph; }; @@ -45,5 +45,5 @@ private slots: - void slotNewEph(gpsEph* ep); + void slotNewEph(t_eph* ep); void slotNewConnection(); void slotMessage(const QByteArray msg); @@ -55,5 +55,5 @@ void openCaster(); void readEpoch(); - void processSatellite(gpsEph* ep, int GPSweek, double GPSweeks, + void processSatellite(t_eph* ep, int GPSweek, double GPSweeks, const QString& prn, const ColumnVector& xx, struct ClockOrbit::SatData* sd); Index: trunk/BNS/bnseph.cpp =================================================================== --- trunk/BNS/bnseph.cpp (revision 883) +++ trunk/BNS/bnseph.cpp (revision 884) @@ -85,37 +85,47 @@ void t_bnseph::readEph() { - gpsEph* ep = new gpsEph; - - bool flagGlonass = false; - - const int NUMLINES = 8; - - for (int ii = 1; ii <= NUMLINES; ii++) { - + + t_eph* eph = 0; + + QByteArray line = _socket->readLine(); + QTextStream in(line); + QString prn; + + in >> prn; + + int numlines = 0; + if (prn.indexOf('R') != -1) { + eph = new t_ephGlo(); + numlines = 4; + } + else { + eph = new t_ephGPS(); + numlines = 8; + } + + QStringList lines; + lines << line; + + for (int ii = 2; ii <= numlines; ii++) { QByteArray line = _socket->readLine(); - - if (flagGlonass) { - if (ii == 4) { - delete ep; - return; - } - else { - continue; - } - } - - QTextStream in(line); + lines << line; + } + + eph->read(lines); + + emit(newEph(eph)); +} + +// Read GPS Ephemeris +//////////////////////////////////////////////////////////////////////////// +void t_ephGPS::read(const QStringList& lines) { + + for (int ii = 1; ii <= lines.size(); ii++) { + QTextStream in(lines.at(ii-1).toAscii()); if (ii == 1) { - in >> ep->prn; - - if (ep->prn.indexOf('R') != -1) { - flagGlonass = true; - continue; - } - double year, month, day, hour, minute, second; - in >> year >> month >> day >> hour >> minute >> second - >> ep->clock_bias >> ep->clock_drift >> ep->clock_driftrate; + in >> _prn >> year >> month >> day >> hour >> minute >> second + >> _clock_bias >> _clock_drift >> _clock_driftrate; if (year < 100) year += 2000; @@ -124,31 +134,224 @@ QTime(int(hour), int(minute), int(second)), Qt::UTC); int week; - GPSweekFromDateAndTime(dateTime, week, ep->TOC); - ep->GPSweek = week; + GPSweekFromDateAndTime(dateTime, week, _TOC); + _GPSweek = week; } else if (ii == 2) { - in >> ep->IODE >> ep->Crs >> ep->Delta_n >> ep->M0; + in >> _IODE >> _Crs >> _Delta_n >> _M0; } else if (ii == 3) { - in >> ep->Cuc >> ep->e >> ep->Cus >> ep->sqrt_A; + in >> _Cuc >> _e >> _Cus >> _sqrt_A; } else if (ii == 4) { - in >> ep->TOE >> ep->Cic >> ep->OMEGA0 >> ep->Cis; + in >> _TOE >> _Cic >> _OMEGA0 >> _Cis; } else if (ii == 5) { - in >> ep->i0 >> ep->Crc >> ep->omega >> ep->OMEGADOT; + in >> _i0 >> _Crc >> _omega >> _OMEGADOT; } else if (ii == 6) { - in >> ep->IDOT; + in >> _IDOT; } else if (ii == 7) { double hlp, health; - in >> hlp >> health >> ep->TGD >> ep->IODC; + in >> hlp >> health >> _TGD >> _IODC; } else if (ii == 8) { - in >> ep->TOW; - } - } - - emit(newEph(ep)); -} + in >> _TOW; + } + } +} + +// Compute GPS Satellite Position +//////////////////////////////////////////////////////////////////////////// +void t_ephGPS::position(int GPSweek, double GPSweeks, ColumnVector& xc, + ColumnVector& vv) const { + + const static double secPerWeek = 7 * 86400.0; + const static double omegaEarth = 7292115.1467e-11; + const static double gmWGS = 398.6005e12; + + if (xc.Nrows() < 4) { + xc.ReSize(4); + } + xc = 0.0; + + if (vv.Nrows() < 3) { + vv.ReSize(3); + } + vv = 0.0; + + double a0 = _sqrt_A * _sqrt_A; + if (a0 == 0) { + return; + } + + double n0 = sqrt(gmWGS/(a0*a0*a0)); + double tk = GPSweeks - _TOE; + if (GPSweek != _GPSweek) { + tk += (GPSweek - _GPSweek) * secPerWeek; + } + double n = n0 + _Delta_n; + double M = _M0 + n*tk; + double E = M; + double E_last; + do { + E_last = E; + E = M + _e*sin(E); + } while ( fabs(E-E_last)*a0 > 0.001 ); + double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) ); + double u0 = v + _omega; + double sin2u0 = sin(2*u0); + double cos2u0 = cos(2*u0); + double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0; + double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0; + double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0; + double xp = r*cos(u); + double yp = r*sin(u); + double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk - + omegaEarth*_TOE; + + double sinom = sin(OM); + double cosom = cos(OM); + double sini = sin(i); + double cosi = cos(i); + xc(1) = xp*cosom - yp*cosi*sinom; + xc(2) = xp*sinom + yp*cosi*cosom; + xc(3) = yp*sini; + + double tc = GPSweeks - _TOC; + if (GPSweek != _GPSweek) { + tc += (GPSweek - _GPSweek) * secPerWeek; + } + xc(4) = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc + - 4.442807633e-10 * _e * sqrt(a0) *sin(E); + + // Velocity + // -------- + double tanv2 = tan(v/2); + double dEdM = 1 / (1 - _e*cos(E)); + double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2) + * dEdM * n; + double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv; + double dotom = _OMEGADOT - omegaEarth; + double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv; + double dotr = a0 * _e*sin(E) * dEdM * n + + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv; + double dotx = dotr*cos(u) - r*sin(u)*dotu; + double doty = dotr*sin(u) + r*cos(u)*dotu; + + vv(1) = cosom *dotx - cosi*sinom *doty // dX / dr + - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA + + yp*sini*sinom*doti; // dX / di + + vv(2) = sinom *dotx + cosi*cosom *doty + + xp*cosom*dotom - yp*cosi*sinom*dotom + - yp*sini*cosom*doti; + + vv(3) = sini *doty + yp*cosi *doti; +} + +// Compare Time +//////////////////////////////////////////////////////////////////////////// +bool t_ephGPS::isNewerThan(const t_eph* ep) const { + + const t_ephGPS* eph = dynamic_cast(ep); + if (!eph) { + return false; + } + + if (_GPSweek > eph->_GPSweek || + (_GPSweek == eph->_GPSweek && _TOC > eph->_TOC)) { + return true; + } + else { + return false; + } +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_ephGlo::read(const QStringList& lines) { + + for (int ii = 1; ii <= lines.size(); ii++) { + QTextStream in(lines.at(ii-1).toAscii()); + + if (ii == 1) { + double year, month, day, hour, minute, second; + in >> _prn >> year >> month >> day >> hour >> minute >> second + >> _tau >> _gamma; + + if (year < 100) year += 2000; + + QDateTime dateTime(QDate(int(year), int(month), int(day)), + QTime(int(hour), int(minute), int(second)), Qt::UTC); + int week; + GPSweekFromDateAndTime(dateTime, week, _GPSTOW); + _GPSweek = week; + } + else if (ii == 2) { + in >>_x_pos >> _x_velocity >> _x_acceleration >> _flags; + } + else if (ii == 3) { + in >>_y_pos >> _y_velocity >> _y_acceleration >> _frequency_number; + } + else if (ii == 4) { + in >>_z_pos >> _z_velocity >> _z_acceleration >> _E; + } + } +} + +// +//////////////////////////////////////////////////////////////////////////// +bool t_ephGlo::isNewerThan(const t_eph* ep) const { + return false; +} + +// +//////////////////////////////////////////////////////////////////////////// +int t_ephGlo::IOD() const { + return 0; +} + +// Derivative of the state vector using a simple force model (static) +//////////////////////////////////////////////////////////////////////////// +ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv) { + + // State vector components + // ----------------------- + ColumnVector rr = xv.rows(1,3); + ColumnVector vv = xv.rows(4,6); + + // Acceleration + // ------------ + const static double GM = 398.60044e12; + const static double AE = 6378136.0; + const static double OMEGA = 7292115.e-11; + const static double C20 = -1082.63e-6; + + double rho = rr.norm_Frobenius(); + double t1 = -GM/(rho*rho*rho); + double t2 = 3.0/2.0 * C20 * (GM*AE*AE) / (rho*rho*rho*rho*rho); + double t3 = OMEGA * OMEGA; + double t4 = 2.0 * OMEGA; + double z2 = rr(3) * rr(3); + + // Vector of derivatives + // --------------------- + ColumnVector va(6); + va(1) = vv(1); + va(2) = vv(2); + va(3) = vv(3); + va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2); + va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1); + va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3); + + return va; +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_ephGlo::position(int GPSweek, double GPSweeks, ColumnVector& xc, + ColumnVector& vv) const { + +} + Index: trunk/BNS/bnseph.h =================================================================== --- trunk/BNS/bnseph.h (revision 883) +++ trunk/BNS/bnseph.h (revision 884) @@ -2,38 +2,97 @@ #define BNSEPH_H +#include + +#include #include #include -class gpsEph { + +class t_eph { public: - QString prn; - double GPSweek; - double TOW; // [s] - double TOC; // [s] - double TOE; // [s] - double IODE; - double IODC; + virtual ~t_eph() {}; + virtual void position(int GPSweek, double GPSweeks, ColumnVector& xc, + ColumnVector& vv) const = 0; + virtual void read(const QStringList& lines) = 0; + virtual bool isNewerThan(const t_eph* ep) const = 0; + virtual int IOD() const = 0; + QString prn() const {return _prn;} + protected: + QString _prn; +}; - double clock_bias; // [s] - double clock_drift; // [s/s] - double clock_driftrate; // [s/s^2] +class t_ephGlo : public t_eph { + public: + t_ephGlo() { + _xv.ReSize(6); + }; + virtual ~t_ephGlo() {}; + virtual void read(const QStringList& lines); + virtual void position(int GPSweek, double GPSweeks, ColumnVector& xc, + ColumnVector& vv) const; + virtual bool isNewerThan(const t_eph* ep) const; + virtual int IOD() const; + private: + static ColumnVector glo_deriv(double /* tt */, const ColumnVector& xv); + ColumnVector _xv; - double Crs; // [m] - double Delta_n; // [rad/s] - double M0; // [rad] - double Cuc; // [rad] - double e; // - double Cus; // [rad] - double sqrt_A; // [m^0.5] - double Cic; // [rad] - double OMEGA0; // [rad] - double Cis; // [rad] - double i0; // [rad] - double Crc; // [m] - double omega; // [rad] - double OMEGADOT; // [rad/s] - double IDOT; // [rad/s] + double _GPSweek; + double _GPSTOW; + double _E; /* [days] */ + double _tau; /* [s] */ + double _gamma; /* */ + double _x_pos; /* [km] */ + double _x_velocity; /* [km/s] */ + double _x_acceleration; /* [km/s^2] */ + double _y_pos; /* [km] */ + double _y_velocity; /* [km/s] */ + double _y_acceleration; /* [km/s^2] */ + double _z_pos; /* [km] */ + double _z_velocity; /* [km/s] */ + double _z_acceleration; /* [km/s^2] */ + int _flags; /* GLOEPHF_xxx */ + int _frequency_number; /* ICD-GLONASS data position */ +}; - double TGD; // [s] + +class t_ephGPS : public t_eph { + public: + t_ephGPS() {}; + virtual ~t_ephGPS() {}; + virtual void read(const QStringList& lines); + virtual void position(int GPSweek, double GPSweeks, ColumnVector& xc, + ColumnVector& vv) const; + virtual bool isNewerThan(const t_eph* ep) const; + virtual int IOD() const {return int(_IODE);} + + private: + double _GPSweek; + double _TOW; // [s] + double _TOC; // [s] + double _TOE; // [s] + double _IODE; + double _IODC; + + double _clock_bias; // [s] + double _clock_drift; // [s/s] + double _clock_driftrate; // [s/s^2] + + double _Crs; // [m] + double _Delta_n; // [rad/s] + double _M0; // [rad] + double _Cuc; // [rad] + double _e; // + double _Cus; // [rad] + double _sqrt_A; // [m^0.5] + double _Cic; // [rad] + double _OMEGA0; // [rad] + double _Cis; // [rad] + double _i0; // [rad] + double _Crc; // [m] + double _omega; // [rad] + double _OMEGADOT; // [rad/s] + double _IDOT; // [rad/s] + + double _TGD; // [s] }; @@ -46,5 +105,5 @@ signals: - void newEph(gpsEph* eph); + void newEph(t_eph* eph); void newMessage(const QByteArray msg); void error(const QByteArray msg); Index: trunk/BNS/bnsutils.cpp =================================================================== --- trunk/BNS/bnsutils.cpp (revision 883) +++ trunk/BNS/bnsutils.cpp (revision 884) @@ -108,86 +108,4 @@ } -// Satellite Position computed using broadcast ephemeris -//////////////////////////////////////////////////////////////////////////// -void satellitePosition(int GPSweek, double GPSweeks, const gpsEph* ep, - double& X, double& Y, double& Z, double& dt, - double& vX, double& vY, double& vZ) { - - const static double secPerWeek = 7 * 86400.0; - const static double omegaEarth = 7292115.1467e-11; - const static double gmWGS = 398.6005e12; - - X = Y = Z = dt = 0.0; - - double a0 = ep->sqrt_A * ep->sqrt_A; - if (a0 == 0) { - return; - } - - double n0 = sqrt(gmWGS/(a0*a0*a0)); - double tk = GPSweeks - ep->TOE; - if (GPSweek != ep->GPSweek) { - tk += (GPSweek - ep->GPSweek) * secPerWeek; - } - double n = n0 + ep->Delta_n; - double M = ep->M0 + n*tk; - double E = M; - double E_last; - do { - E_last = E; - E = M + ep->e*sin(E); - } while ( fabs(E-E_last)*a0 > 0.001 ); - double v = 2.0*atan( sqrt( (1.0 + ep->e)/(1.0 - ep->e) )*tan( E/2 ) ); - double u0 = v + ep->omega; - double sin2u0 = sin(2*u0); - double cos2u0 = cos(2*u0); - double r = a0*(1 - ep->e*cos(E)) + ep->Crc*cos2u0 + ep->Crs*sin2u0; - double i = ep->i0 + ep->IDOT*tk + ep->Cic*cos2u0 + ep->Cis*sin2u0; - double u = u0 + ep->Cuc*cos2u0 + ep->Cus*sin2u0; - double xp = r*cos(u); - double yp = r*sin(u); - double OM = ep->OMEGA0 + (ep->OMEGADOT - omegaEarth)*tk - - omegaEarth*ep->TOE; - - double sinom = sin(OM); - double cosom = cos(OM); - double sini = sin(i); - double cosi = cos(i); - X = xp*cosom - yp*cosi*sinom; - Y = xp*sinom + yp*cosi*cosom; - Z = yp*sini; - - double tc = GPSweeks - ep->TOC; - if (GPSweek != ep->GPSweek) { - tc += (GPSweek - ep->GPSweek) * secPerWeek; - } - dt = ep->clock_bias + ep->clock_drift*tc + ep->clock_driftrate*tc*tc - - 4.442807633e-10 * ep->e * sqrt(a0) *sin(E); - - // Velocity - // -------- - double tanv2 = tan(v/2); - double dEdM = 1 / (1 - ep->e*cos(E)); - double dotv = sqrt((1.0 + ep->e)/(1.0 - ep->e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2) - * dEdM * n; - double dotu = dotv + (-ep->Cuc*sin2u0 + ep->Cus*cos2u0)*2*dotv; - double dotom = ep->OMEGADOT - omegaEarth; - double doti = ep->IDOT + (-ep->Cic*sin2u0 + ep->Cis*cos2u0)*2*dotv; - double dotr = a0 * ep->e*sin(E) * dEdM * n - + (-ep->Crc*sin2u0 + ep->Crs*cos2u0)*2*dotv; - double dotx = dotr*cos(u) - r*sin(u)*dotu; - double doty = dotr*sin(u) + r*cos(u)*dotu; - - vX = cosom *dotx - cosi*sinom *doty // dX / dr - - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA - + yp*sini*sinom*doti; // dX / di - - vY = sinom *dotx + cosi*cosom *doty - + xp*cosom*dotom - yp*cosi*sinom*dotom - - yp*sini*cosom*doti; - - vZ = sini *doty + yp*cosi *doti; -} - // Transformation xyz --> radial, along track, out-of-plane //////////////////////////////////////////////////////////////////////////// @@ -202,2 +120,23 @@ rsw(3) = DotProduct(xyz, cross) / cross.norm_Frobenius(); } + +// Fourth order Runge-Kutta numerical integrator for ODEs +//////////////////////////////////////////////////////////////////////////// +ColumnVector rungeKutta4( + double xi, // the initial x-value + const ColumnVector& yi, // vector of the initial y-values + double dx, // the step size for the integration + ColumnVector (*der)(double x, const ColumnVector& y) + // A pointer to a function that computes the + // derivative of a function at a point (x,y) + ) { + + ColumnVector k1 = der(xi , yi ) * dx; + ColumnVector k2 = der(xi+dx/2.0, yi+k1/2.0) * dx; + ColumnVector k3 = der(xi+dx/2.0, yi+k2/2.0) * dx; + ColumnVector k4 = der(xi+dx , yi+k3 ) * dx; + + ColumnVector yf = yi + k1/6.0 + k2/3.0 + k3/3.0 + k4/6.0; + + return yf; +} Index: trunk/BNS/bnsutils.h =================================================================== --- trunk/BNS/bnsutils.h (revision 883) +++ trunk/BNS/bnsutils.h (revision 884) @@ -23,9 +23,9 @@ void currentGPSWeeks(int& week, double& sec); -void satellitePosition(int GPSweek, double GPSweeks, const gpsEph* ep, - double& X, double& Y, double& Z, double&, - double& vX, double& vY, double& vZ); - void XYZ_to_RSW(const ColumnVector& rr, const ColumnVector& vv, const ColumnVector& xyz, ColumnVector& rsw); + +ColumnVector rungeKutta4(double xi, const ColumnVector& yi, double dx, + ColumnVector (*der)(double x, const ColumnVector& y)); + #endif Index: trunk/BNS/glonass.cpp =================================================================== --- trunk/BNS/glonass.cpp (revision 883) +++ (revision ) @@ -1,53 +1,0 @@ -/* ------------------------------------------------------------------------- - * BKG NTRIP Server - * ------------------------------------------------------------------------- - * - * Function: glo_deriv - * - * Purpose: Derivative of the state vector of a Galileo satellite - * using its position, velocity and a simplified force model - * - * Author: L. Mervart - * - * Created: 07-Mai-2008 - * - * Changes: - * -/******************************************************************************/ - -#include "glonass.h" - -// Derivative of the state vector -//////////////////////////////////////////////////////////////////////////// -void glo_deriv(double /* tt */, const ColumnVector& yy, - ColumnVector& yp, void* /* pVoid */) { - - // State vector components - // ----------------------- - ColumnVector rr = yy.rows(1,3); - ColumnVector vv = yy.rows(4,6); - - // Acceleration - // ------------ - const static double GM = 398.60044e12; - const static double AE = 6378136.0; - const static double OMEGA = 7292115.e-11; - const static double C20 = -1082.63e-6; - - double rho = rr.norm_Frobenius(); - double t1 = -GM/(rho*rho*rho); - double t2 = 3.0/2.0 * C20 * (GM*AE*AE) / (rho*rho*rho*rho*rho); - double t3 = OMEGA * OMEGA; - double t4 = 2.0 * OMEGA; - double z2 = rr(3) * rr(3); - - ColumnVector aa(3); - aa(1) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2); - aa(2) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1); - aa(3) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3); - - // State vector derivative - // ----------------------- - yp = vv & - aa ; -} Index: trunk/BNS/glonass.h =================================================================== --- trunk/BNS/glonass.h (revision 883) +++ (revision ) @@ -1,9 +1,0 @@ -#ifndef GLONASS_H -#define GLONASS_H - -#include - -void glo_deriv(double tt, const ColumnVector& yy, - ColumnVector& yp, void* pVoid = 0); - -#endif