geodeticframes.cpp

1 gps基本算法

		// line 35 of Table 8.1, period = 29.53 days
		arg = d;
		UT1mUT1R += 0.05e-4 * ::sin(arg);
		dlodR -= 0.1e-5 * ::cos(arg);
		domegaR += 0.1e-14 * ::cos(arg);
		// line 36 of Table 8.1, period = 29.80 days
		arg = l-lp;
		UT1mUT1R -= 0.06e-4 * ::sin(arg);
		dlodR += 0.1e-5 * ::cos(arg);
		domegaR -= 0.1e-14 * ::cos(arg);
		// line 37 of Table 8.1, period = 31.66 days
		arg = -l+2*d-o;
		UT1mUT1R += 0.12e-4 * ::sin(arg);
		dlodR -= 0.2e-5 * ::cos(arg);
		domegaR += 0.2e-14 * ::cos(arg);
		// line 38 of Table 8.1, period = 31.81 days
		arg = -l+2*d;
		UT1mUT1R -= 1.82e-4 * ::sin(arg);
		dlodR += 3.6e-5 * ::cos(arg);
		domegaR -= 3.0e-14 * ::cos(arg);
		// line 39 of Table 8.1, period = 31.96 days
		arg = -l+2*d+o;
		UT1mUT1R += 0.13e-4 * ::sin(arg);
		dlodR -= 0.3e-5 * ::cos(arg);
		domegaR += 0.2e-14 * ::cos(arg);
		// line 40 of Table 8.1, period = 32.61 days
		arg = l-2*f+2*d-o;
		UT1mUT1R += 0.02e-4 * ::sin(arg);
		// line 41 of Table 8.1, period = 34.85 days
		arg = -l-lp+2*d;
		UT1mUT1R -= 0.09e-4 * ::sin(arg);
		dlodR += 0.2e-5 * ::cos(arg);
		domegaR -= 0.1e-14 * ::cos(arg);
	
		//End Code implementing Table 8.1 IERS Conventions 1996 UT1R tide series.
		//-----------------------------------------------------------------------
	}

   //---------------------------------------------------------------------------------
   // Compute the Greenwich hour angle of the true vernal equinox, or
   // Greenwich Apparent Sidereal Time (GAST) in radians,
   // given the (UT) time of interest t, and, where T = CoordTransTime(t),
   // o  = Omega(T) = mean longitude of lunar ascending node, in degrees,
   // eps = the obliquity of the ecliptic, in degrees,
   // dpsi = nutation in longitude (counted in the ecliptic),
   //                in seconds of arc.
   //
   // GAST = Greenwich hour angle of the true vernal equinox
   // GAST = GMST + dpsi*cos(eps) + 0.00264" * sin(Omega) +0.000063" * sin(2*Omega)
   //    (these terms account for the accumulated precession and nutation in
   //       right ascension and minimize any discontinuity in UT1)
   //
   // GMST = Greenwich hour angle of the mean vernal equinox
   //      = Greenwich Mean Sideral Time
   //      = GMST0 + r*[UTC + (UT1-UTC)]
   // r    = is the ratio of universal to sidereal time
   //      = 1.002737909350795 + 5.9006E-11*T' - 5.9e-15*T'^2
   // T'   = days'/36525
   // days'= number of days elapsed since the Julian Epoch t0 (J2000)
   //      = +/-(integer+0.5)
   //   and
   // (UT1-UTC) (seconds) is taken from the IERS bulletin 
   //
   // GMST0 = GMST at 0h UT1
   //      = 6h 41min (50.54841+8640184.812866*T'+0.093104*T'^2-6.2E-6*T'^3)sec
   //
   // see pg 21 of the Reference (IERS 1996).
   double GeodeticFrames::gast(DayTime t,
                               double om,
                               double eps,
                               double dpsi,
                               double UT1mUTC)
      throw()
   {
      double G = GMST(t,UT1mUTC);
         // add dpsi, eps and Omega terms
      om *= DEG_TO_RAD;
      eps *= DEG_TO_RAD;
      G += (       dpsi * ::cos(eps)
             + 0.00264  * ::sin(om)
             + 0.000063 * ::sin(2.0*om) ) * DEG_TO_RAD / 3600.0;

      return G;
   }

   //---------------------------------------------------------------------------------
   // Compute the precession matrix, a 3x3 rotation matrix, given T,
   // the coordinate transformation time at the time of interest
   Matrix<double> GeodeticFrames::PrecessionMatrix(double T)
      throw(InvalidRequest)
   {
      try {
            // IAU76 - ref McCarthy - seconds of arc
         double zeta  = T*(2306.2181 + T*(0.30188 + T*0.017998));
         double theta = T*(2004.3109 - T*(0.42665 + T*0.041833));
         double z     = T*(2306.2181 + T*(1.09468 + T*0.018203));

            // convert to degrees
         zeta  /= 3600.0;
         theta /= 3600.0;
         z     /= 3600.0;

         Matrix<double> R1 = rotation(-zeta*DEG_TO_RAD, 3);
         Matrix<double> R2 = rotation(theta*DEG_TO_RAD, 2);
         Matrix<double> R3 = rotation(-z*DEG_TO_RAD, 3);
         Matrix<double> P = R3*R2*R1;

         return P;
      }
      catch(InvalidRequest& ire) {
         GPSTK_RETHROW(ire);
      }
   }

   //---------------------------------------------------------------------------------
   // Compute the nutation matrix, given
   // eps,  the obliquity of the ecliptic, in degrees,
   // dpsi, the nutation in longitude (counted in the ecliptic),
   // in seconds of arc, and
   // deps, the nutation in obliquity, in seconds of arc.
   Matrix<double> GeodeticFrames::NutationMatrix(double eps,
                                                 double dpsi,
                                                 double deps)
      throw(InvalidRequest)
   {
      Matrix<double> N;
      try {
         Matrix<double> R1 = rotation(-eps*DEG_TO_RAD, 1);
         Matrix<double> R2 = rotation(dpsi*DEG_TO_RAD/3600.0, 3);
         Matrix<double> R3 = rotation((eps+deps/3600.0)*DEG_TO_RAD, 1);
         N = R1*R2*R3;
         return N;
      }
      catch(InvalidRequest& ire) {
         GPSTK_RETHROW(ire);
      }
   }

   //---------------------------------------------------------------------------------
   // public functions

   //---------------------------------------------------------------------------------
   // Compute Greenwich Mean Sidereal Time, or the Greenwich hour angle of
   // the mean vernal equinox, given the coordinate time of interest,
   // and UT1-UTC (sec), which comes from the IERS bulletin.
   // @param t DayTime epoch of the rotation.
   // @param UT1mUTC, UT1-UTC in seconds, as found in the IERS bulletin.
   // @param reduced, bool true when UT1mUTC is 'reduced', meaning assumes
   //                 'no tides', as is the case with the NGA EOPs (default=F).
   double GeodeticFrames::GMST(DayTime t,
                               double UT1mUTC,
                               bool reduced)
         throw()
   {
         // days' since epoch = +/-(integer+0.5)
      double days = double(t.JD() - JulianEpoch) - 1.0 + t.secOfDay()/86400.0;
      int d=int(days);
      if(d < 0 && days==double(d)) d++;
      days = d + (days<0.0 ? -0.5 : 0.5);
      double Tp = days/36525.0;

         // Compute GMST in radians
         // First compute GMST0
      double G;
      //G = 24060.0 + 50.54841 + 8640184.812866*Tp;  // seconds (24060s = 6h 41min)
      //G /= 86400.0; // instead, divide the above equation by 86400.0 manually...
      G = 0.27847222 + 0.00058505104167 + 100.0021390378009*Tp;
      G += (0.093104 - 6.2e-6*Tp)*Tp*Tp/86400.0;      // seconds/86400 = circles

         // if reduced, compute tidal terms
      if(reduced) {
         double dlodR,domegaR,UT1mUT1R;
         UT1mUTCTidalCorrections(CoordTransTime(t), UT1mUT1R, dlodR, domegaR);
         UT1mUTC = UT1mUT1R-UT1mUTC;
      }

         // now get GMST
      double r=1.002737909350795 + (5.9006e-11 - 5.9e-15*Tp)*Tp;
      G += r*(UT1mUTC + t.secOfDay() - 13.0)/86400.0;       // circles
      G *= TWO_PI;                                   // radians

      return G;
   }

   //---------------------------------------------------------------------------------
   // Compute Greenwich Apparent Sidereal Time, or the Greenwich hour angle of
   // the true vernal equinox, given the coordinate time of interest,
   // and UT1-UTC (sec), which comes from the IERS bulletin.
   // @param t DayTime epoch of the rotation.
   // @param UT1mUTC, UT1-UTC in seconds, as found in the IERS bulletin.
   double GeodeticFrames::GAST(DayTime t,
                               double UT1mUTC,
                               bool reduced)
      throw()
   {
	   double T = CoordTransTime(t);
	   double o = Omega(T);
	   double eps = Obliquity(T);
      double deps,dpsi;

      NutationAngles(T,deps,dpsi);        // deps is not used...

      // if reduced (NGA), correct for tides
      double UT1mUT1R,dlodR,domegaR;
      if(reduced)
         UT1mUTCTidalCorrections(T, UT1mUT1R, dlodR, domegaR);

	   return gast(t, o, eps, dpsi, reduced ? UT1mUT1R-UT1mUTC : UT1mUTC);
   }

   //---------------------------------------------------------------------------------
   // Generate transformation matrix (3X3 rotation) due to polar motion (xp,yp)
   // xp and yp are in arc seconds, as found in the IERS bulletin
   Matrix<double> GeodeticFrames::PolarMotion(double xp,
                                              double yp)
      throw(InvalidRequest)
   {
      try {
	      xp *= DEG_TO_RAD/3600.0;
	      yp *= DEG_TO_RAD/3600.0;
         Matrix<double> R1,R2;
         R1 = rotation(yp,1);
         R2 = rotation(xp,2);
	      return (R1*R2);
      }
      catch(InvalidRequest& ire) {
         GPSTK_RETHROW(ire);
      }
   }

   //---------------------------------------------------------------------------------
   // Generate precise transformation matrix (3X3 rotation) due to Earth rotation
   // at Greenwich hour angle of the true vernal equinox and which accounts for
   // precession and nutation in right ascension, given the UT time of interest
   // and the UT1-UTC correction (in sec), obtained from the IERS bulletin.
   Matrix<double> GeodeticFrames::PreciseEarthRotation(DayTime t,
                                                       double UT1mUTC,
                                                       bool reduced)
      throw(InvalidRequest)
   {
      try {
         return (rotation(-GAST(t,UT1mUTC,reduced),3));
      }
      catch(InvalidRequest& ire) {
         GPSTK_RETHROW(ire);
      }
   }

   //---------------------------------------------------------------------------------
   // Generate an Earth Nutation Matrix (3X3 rotation) at the given DayTime.
   // @param t DayTime epoch of the rotation.
   Matrix<double> GeodeticFrames::Nutation(DayTime t)
      throw(InvalidRequest)
   {
      try {
	      double T=CoordTransTime(t);
	      double eps=Obliquity(T);								// degrees
         double deps,dpsi;

         NutationAngles(T,deps,dpsi);

	      return NutationMatrix(eps,dpsi,deps);
      }
      catch(InvalidRequest& ire) {
         GPSTK_RETHROW(ire);
      }
   }

   //---------------------------------------------------------------------------------
   // Generate the full transformation matrix (3x3 rotation) relating the ECEF
   // frame to the conventional inertial frame.
   // throw(); Input is the time of interest,
   // the polar motion angles xp and yp (arcseconds), and UT1-UTC (seconds)
   // (xp,yp and UT1-UTC are just as found in the IERS bulletin).
   Matrix<double> GeodeticFrames::ECEFtoInertial(DayTime t,
                                                 double xp,
                                                 double yp,
                                                 double UT1mUTC,
                                                 bool reduced)
      throw(InvalidRequest)
   {
      try {
	      Matrix<double> P,N,W,S;

	      double T=CoordTransTime(t);
         P = PrecessionMatrix(T);

	      double eps=Obliquity(T);								// degrees
         double deps,dpsi;
         NutationAngles(T,deps,dpsi);
         N = NutationMatrix(eps,dpsi,deps);

         // PolarMotion converts xp, yp to radians
	      W = PolarMotion(xp, yp);

         // if reduced (NGA), correct UT1mUTC for tides
         double UT1mUT1R,dlodR,domegaR;
         if(reduced)
            UT1mUTCTidalCorrections(T, UT1mUT1R, dlodR, domegaR);

	      double omega = Omega(T);
	      double g = gast(t, omega, eps, dpsi, reduced ? UT1mUT1R-UT1mUTC : UT1mUTC);
         S = rotation(-g,3);

	      return (P*N*W*S);
      }
      catch(InvalidRequest& ire) {
         GPSTK_RETHROW(ire);
      }
   }

   //---------------------------------------------------------------------------------
   // Given a rotation matrix R (3x3), inverse(R)=transpose(R),
   // find the Euler angles (theta,phi,psi) which produce this rotation,
   // and also determine the magnitude (alpha) and direction (nhat = unit 3-vector)
   // of the net rotation.
   // Throw InvalidRequest if the matrix is not a rotation matrix.
   //
   // Euler angles (this is one convention - there are others):
   //   Let R1 = rotation about z through angle phi
   //       R2 = rotation about x through angle theta ( 0 <= theta <= pi)
   //       R3 = rotation about z through angle psi
   //   Any rotation matrix can be expressed as the product of these rotations:
   //   R = R3*R2*R1. In particular, by definition
   //
   //           [  cos(phi) sin(phi)  0 ]
   //      R1 = [ -sin(phi) cos(phi)  0 ]
   //           [     0        0      1 ]
   //
   //           [ cos(theta) 0 -sin(theta) ]
   //      R2 = [      0     1     0       ]
   //           [ sin(theta) 0  cos(theta) ]
   //
   //           [  cos(psi) sin(psi)  0 ]
   //      R3 = [ -sin(psi) cos(psi)  0 ]
   //           [     0        0      1 ]
   //
   //   and if we define
   //          [ r11 r12 r13 ]
   //      R = [ r21 r22 r23 ]
   //          [ r31 r32 r33 ]
   //
   //   then
   //      r11 =  cos(phi)cos(psi) - cos(theta)sin(phi)sin(psi)
   //      r12 =  sin(phi)cos(psi) + cos(theta)cos(phi)sin(psi)
   /