geodeticframes.cpp

linux的gps应用

#pragma ident "$Id: GeodeticFrames.cpp 327 2006-11-30 18:37:30Z ehagen $"

//============================================================================
//
//  This file is part of GPSTk, the GPS Toolkit.
//
//  The GPSTk is free software; you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License as published
//  by the Free Software Foundation; either version 2.1 of the License, or
//  any later version.
//
//  The GPSTk is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with GPSTk; if not, write to the Free Software Foundation,
//  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//  
//  Copyright 2004, The University of Texas at Austin
//
//============================================================================

//============================================================================
//
//This software developed by Applied Research Laboratories at the University of
//Texas at Austin, under contract to an agency or agencies within the U.S. 
//Department of Defense. The U.S. Government retains all rights to use,
//duplicate, distribute, disclose, or release this software. 
//
//Pursuant to DoD Directive 523024 
//
// DISTRIBUTION STATEMENT A: This software has been approved for public 
//                           release, distribution is unlimited.
//
//=============================================================================

/**
 * @file GeodeticFrames.cpp
 * Implement geodetic frame computations in the GeodeticFrames class.
 * class gpstk::GeodeticFrames encapsulates frame transformations between the
 * conventional terrestrial frame and the conventional inertial frame, as defined
 * by the reference. It implements models of precession and nutation (IERS 1996) of
 * Earth's axis, as well as the precise rotation of Earth and its 'wobble'
 * as given by the Earth orientation parameters (see class EarthOrientation).
 * Reference: IERS Technical Note 21, IERS Conventions (1996),
 * by Dennis D. McCarthy, U.S. Naval Observatory.
 */

//------------------------------------------------------------------------------------
// GPSTk includes
#include "StringUtils.hpp"
#include "geometry.hpp"             // for DEG_TO_RAD
#include "icd_200_constants.hpp"    // for PI and TWO_PI
#include "GeodeticFrames.hpp"

using namespace std;

namespace gpstk
{
   //---------------------------------------------------------------------------------
   //---------------------------------------------------------------------------------
   // constants
   // epoch for CoordTransTime
   const long GeodeticFrames::JulianEpoch=2451545;

   //---------------------------------------------------------------------------------
   //---------------------------------------------------------------------------------
   // functions used internally

   //---------------------------------------------------------------------------------
   // Compute the 'coordinate transformation time' which is the time since
   // epoch J2000 = January 1 2000 12h UT = 2451545.0JD, divided by 36525 days.
   // This quantity is used throughout the terrestrial / inertial coordinate
   // transformations.
   double GeodeticFrames::CoordTransTime(DayTime t)
      throw()
   {
      double ct = (t.JD()-double(JulianEpoch))/36525.0; // day contribution
      ct += ((t.secOfDay()-43200.0)/86400.0)/36525.0;   // seconds contribution
      return ct;
   }

   //---------------------------------------------------------------------------------
   // Return mean longitude of lunar ascending node, in degrees,
   // given T, the CoordTransTime at the Epoch of interest. (Ref: F5 pg 23)
   double GeodeticFrames::Omega(double T)
      throw()
   {
      double om;
      om = -0.00005939*T;           // seconds of arc
      om = (om + 0.007702)*T;
      om = (om + 7.4722)*T;
      om /= 3600.0;                 // convert to degrees
      om = (om - 1934.136185139)*T; // 1934.136185139 = 6962890.2665/3600.0
      om += 125.04455501;
      //om = fmod(om,360.0);
      return om;
   }

   //---------------------------------------------------------------------------------
   // Return mean longitude of the moon - Omega, in degrees,
   // given T, the CoordTransTime at the Epoch of interest. (Ref: F3 pg 23)
   double GeodeticFrames::F(double T)
      throw()
   {
      double f;
      f = 0.00000417*T;             // seconds of arc
      f = (f - 0.001037)*T;
      f = (f - 12.7512)*T;
      f /= 3600.0;                  // convert to degrees
      f = (f + 483202.01745772)*T;  // 483202.01745772 = 1739527262.8478/3600.0
      f += 93.27209062;
      //f = fmod(f,360.0);
      return 0.0;
   }

   //---------------------------------------------------------------------------------
   // Return mean elongation of the moon from the sun, in degrees,
   // given T, the CoordTransTime at the Epoch of interest. (Ref: F4 pg 23)
   double GeodeticFrames::D(double T)
      throw()
   {
      double d;
      d = -0.00003169*T;            // seconds of arc
      d = (d + 0.006593)*T;
      d = (d - 6.3706)*T;
      d /= 3600.0;                  // convert to degrees
      d = (d + 445267.111446944)*T; // 445267.111446944 = 1602961601.2090 / 3600.0
      d += 297.85019547;
      //d = fmod(d,360.0);
      return d;
   }

   //---------------------------------------------------------------------------------
   // Return mean anomaly of the moon, in degrees,
   // given T, the CoordTransTime at the Epoch of interest. (Ref: F1 pg 23)
   double GeodeticFrames::L(double T)
      throw()
   {
      double l;
      l = -0.00024470*T;            // seconds of arc
      l = (l + 0.051635)*T;
      l = (l + 31.8792)*T;
      l /= 3600.0;                  // convert to degrees
      l = (l + 477198.8675605)*T;   // 477198.8675605 = 1717915923.2178 / 3600.0
      l += 134.96340251;
      //l = fmod(l,360.0);
      return l;
   }

   //---------------------------------------------------------------------------------
   // Return mean anomaly of the sun, in degrees,
   // given T, the CoordTransTime at the Epoch of interest. (Ref: F2 pg 23)
   double GeodeticFrames::Lp(double T)
      throw()
   {
      double lp;
      lp = -0.00001149*T;           // seconds of arc
      lp = (lp + 0.000136)*T;
      lp = (lp - 0.5532)*T;
      lp /= 3600.0;                 // convert to degrees
      lp = (lp + 35999.050291139)*T;// 35999.050291139 = 129596581.0481 / 3600.0
      lp += 357.52910918;
      //lp = fmod(lp,360.0);
      return lp;
   }

   //---------------------------------------------------------------------------------
   // Compute eps, the obliquity of the ecliptic, in degrees,
   // given T, the CoordTransTime at the time of interest. IAU76
   double GeodeticFrames::Obliquity(double T)
      throw()
   {
      double ep;
      // seconds of arc
      ep = T*(-46.8150 + T*(-0.00059 + T*0.001813));
      ep /= 3600.0;                       // convert to degrees
      // degrees
      ep += 23.43929111;                  // = 84381.448/3600.0

      return ep;
   }

   //---------------------------------------------------------------------------------
   // Nutation of the obliquity (deps) and of the longitude (dpsi), IERS 1996
   // model (ref pg 26), given
   // @param T,    the coordinate transformation time at the time of interest
   // @param deps, nutation of the obliquity, in arc seconds (output)
   // @param dpsi, nutation of the longitude, in arc seconds (output)
   void GeodeticFrames::NutationAngles(double T,
                                       double& deps,
                                       double& dpsi)
      throw()
   {
		//-----------------------------------------------------------------------
		// Code to implement Table 5.2 of IERS Conventions 1996 series for
		// nutation in longitude dpsi and obliquity deps in arc seconds.
		// (Generated by perl script Table52.pl)
		double arg;
		dpsi = deps = 0.0;
	
		// double T must be defined as time in Julian centuries from epoch J2000.0
		// Also define (all doubles):
		double o = Omega(T); // = mean longitude of lunar ascending node, in degrees,
		double f = F(T)    ; // = mean longitude of the moon - Omega, in degrees,
		double d = D(T)    ; // = mean elongation of the moon from the sun, in degrees,
		double l = L(T)    ; // = mean anomaly of the moon, in degrees, and
		double lp = Lp(T)  ; // = mean anomaly of the sun, in degrees.
	   o *= DEG_TO_RAD;
	   f *= DEG_TO_RAD;
	   d *= DEG_TO_RAD;
	   l *= DEG_TO_RAD;
	   lp *= DEG_TO_RAD;
	
		// line 1 of Table 5.2, period = -6798.38 days
		arg = o;
		dpsi += (-17.206277 - 0.017419*T) * ::sin(arg) + 0.003645 * ::cos(arg);
		deps += (+9.205356 + 0.000886*T) * ::cos(arg) + 0.001553 * ::sin(arg);
		// line 2 of Table 5.2, period = 182.62 days
		arg = 2*f-2*d+2*o;
		dpsi += (-1.317014 - 0.000156*T) * ::sin(arg) - 0.001400 * ::cos(arg);
		deps += (+0.573058 - 0.000306*T) * ::cos(arg) - 0.000464 * ::sin(arg);
		// line 3 of Table 5.2, period = 13.66 days
		arg = 2*f+2*o;
		dpsi += (-0.227720 - 0.000023*T) * ::sin(arg) + 0.000269 * ::cos(arg);
		deps += (+0.097864 - 0.000048*T) * ::cos(arg) + 0.000136 * ::sin(arg);
		// line 4 of Table 5.2, period = -3399.18 days
		arg = 2*o;
		dpsi += (+0.207429 + 0.000021*T) * ::sin(arg) - 0.000071 * ::cos(arg);
		deps += (-0.089747 + 0.000047*T) * ::cos(arg) - 0.000029 * ::sin(arg);
		// line 5 of Table 5.2, period = -365.26 days
		arg = -lp;
		dpsi += (-0.147538 + 0.000364*T) * ::sin(arg) + 0.001121 * ::cos(arg);
		deps += (+0.007388 - 0.000019*T) * ::cos(arg) + 0.000198 * ::sin(arg);
		// line 6 of Table 5.2, period = 121.75 days
		arg = lp+2*f-2*d+2*o;
		dpsi += (-0.051687 + 0.000123*T) * ::sin(arg) - 0.000054 * ::cos(arg);
		deps += (+0.022440 - 0.000068*T) * ::cos(arg) - 0.000018 * ::sin(arg);
		// line 7 of Table 5.2, period = 27.55 days
		arg = l;
		dpsi += (+0.071118 + 0.000007*T) * ::sin(arg) - 0.000094 * ::cos(arg);
		deps -= 0.000687 * ::cos(arg) + 0.000039 * ::sin(arg);
		// line 8 of Table 5.2, period = 13.63 days
		arg = 2*f+o;
		dpsi += (-0.038752 - 0.000037*T) * ::sin(arg) + 0.000034 * ::cos(arg);
		deps += (+0.020076 + 0.000002*T) * ::cos(arg) + 0.000032 * ::sin(arg);
		// line 9 of Table 5.2, period = 9.13 days
		arg = l+2*f+2*o;
		dpsi += (-0.030137 - 0.000004*T) * ::sin(arg) + 0.000077 * ::cos(arg);
		deps += (+0.012896 - 0.000006*T) * ::cos(arg) + 0.000035 * ::sin(arg);
		// line 10 of Table 5.2, period = 365.22 days
		arg = -lp+2*f-2*d+2*o;
		dpsi += (+0.021583 - 0.000049*T) * ::sin(arg) + 0.000006 * ::cos(arg);
		deps += (-0.009591 + 0.000030*T) * ::cos(arg) + 0.000012 * ::sin(arg);
		// line 11 of Table 5.2, period = 177.84 days
		arg = 2*f-2*d+o;
		dpsi += (+0.012820 + 0.000014*T) * ::sin(arg) + 0.000018 * ::cos(arg);
		deps += (-0.006897 - 0.000001*T) * ::cos(arg) + 0.000004 * ::sin(arg);
		// line 12 of Table 5.2, period = 27.09 days
		arg = -l+2*f+2*o;
		dpsi += (+0.012353 + 0.000001*T) * ::sin(arg) + 0.000002 * ::cos(arg);
		deps += (-0.005334 + 0.000003*T) * ::cos(arg);
		// line 13 of Table 5.2, period = 31.81 days
		arg = -l+2*d;
		dpsi += (+0.015699 + 0.000001*T) * ::sin(arg) - 0.000018 * ::cos(arg);
		deps -= 0.000127 * ::cos(arg) + 0.000009 * ::sin(arg);
		// line 14 of Table 5.2, period = 27.67 days
		arg = l+o;
		dpsi += (+0.006314 + 0.000006*T) * ::sin(arg) + 0.000003 * ::cos(arg);
		deps -= 0.003323 * ::cos(arg) - 0.000001 * ::sin(arg);
		// line 15 of Table 5.2, period = -27.44 days
		arg = -l+o;
		dpsi += (-0.005797 - 0.000006*T) * ::sin(arg) - 0.000019 * ::cos(arg);
		deps += 0.003141 * ::cos(arg) - 0.000008 * ::sin(arg);
		// line 16 of Table 5.2, period = 9.56 days
		arg = -l+2*f+2*d+2*o;
		dpsi += (-0.005965 - 0.000001*T) * ::sin(arg) + 0.000014 * ::cos(arg);
		deps += (+0.002554 - 0.000001*T) * ::cos(arg) + 0.000007 * ::sin(arg);
		// line 17 of Table 5.2, period = 9.12 days
		arg = l+2*f+o;
		dpsi += (-0.005163 - 0.000004*T) * ::sin(arg) + 0.000012 * ::cos(arg);
		deps += 0.002635 * ::cos(arg) + 0.000008 * ::sin(arg);
		// line 18 of Table 5.2, period = 1305.48 days
		arg = -2*l+2*f+o;
		dpsi += (+0.004590 + 0.000005*T) * ::sin(arg) + 0.000001 * ::cos(arg);
		deps += (-0.002424 - 0.000001*T) * ::cos(arg) + 0.000001 * ::sin(arg);
		// line 19 of Table 5.2, period = 14.77 days
		arg = 2*d;
		dpsi += (+0.006336 + 0.000001*T) * ::sin(arg) - 0.000015 * ::cos(arg);
		deps -= 0.000125 * ::cos(arg) + 0.000003 * ::sin(arg);
		// line 20 of Table 5.2, period = 7.10 days
		arg = 2*f+2*d+2*o;
		dpsi -= 0.003854 * ::sin(arg) + 0.000015 * ::cos(arg);
		deps += 0.001643 * ::cos(arg) + 0.000006 * ::sin(arg);
		// line 21 of Table 5.2, period = -205.89 days
		arg = -2*l+2*d;
		dpsi -= 0.004774 * ::sin(arg) - 0.000002 * ::cos(arg);
		deps += 0.000048 * ::cos(arg) - 0.000003 * ::sin(arg);
		// line 22 of Table 5.2, period = 6.86 days
		arg = 2*l+2*f+2*o;
		dpsi -= 0.003102 * ::sin(arg) + 0.000012 * ::cos(arg);
		deps += (+0.001323 - 0.000001*T) * ::cos(arg) + 0.000005 * ::sin(arg);
		// line 23 of Table 5.2, period = 23.94 days
		arg = l+2*f-2*d+2*o;
		dpsi += 0.002863 * ::sin(arg);
		deps += (-0.001235 + 0.000001*T) * ::cos(arg);
		// line 24 of Table 5.2, period = 26.98 days
		arg = -l+2*f+o;
		dpsi += (+0.002044 + 0.000002*T) * ::sin(arg) + 0.000001 * ::cos(arg);
		deps -= 0.001076 * ::cos(arg);
		// line 25 of Table 5.2, period = 13.78 days
		arg = 2*l;
		dpsi += 0.002923 * ::sin(arg) - 0.000008 * ::cos(arg);
		deps -= 0.000062 * ::cos(arg) + 0.000001 * ::sin(arg);
		// line 26 of Table 5.2, period = 13.61 days
		arg = 2*f;
		dpsi += 0.002585 * ::sin(arg) - 0.000007 * ::cos(arg);
		deps -= 0.000056 * ::cos(arg) + 0.000001 * ::sin(arg);
		// line 27 of Table 5.2, period = 386.00 days
		arg = lp+o;
		dpsi += (-0.001406 - 0.000003*T) * ::sin(arg) + 0.000008 * ::cos(arg);
		deps += 0.000857 * ::cos(arg) - 0.000004 * ::sin(arg);
		// line 28 of Table 5.2, period = 31.96 days
		arg = -l+2*d+o;
		dpsi += (+0.001517 + 0.000001*T) * ::sin(arg) + 0.000001 * ::cos(arg);
		deps -= 0.000801 * ::cos(arg);
		// line 29 of Table 5.2, period = 91.31 days
		arg = 2*lp+2*f-2*d+2*o;
		dpsi += (-0.001578 + 0.000007*T) * ::sin(arg) - 0.000002 * ::cos(arg);
		deps += (+0.000685 - 0.000004*T) * ::cos(arg) - 0.000001 * ::sin(arg);
		// line 30 of Table 5.2, period = -173.31 days
		arg = -2*f+2*d;
		dpsi += 0.002178 * ::sin(arg) + 0.000001 * ::cos(arg);
		deps -= 0.000015 * ::cos(arg) + 0.000001 * ::sin(arg);
		// line 31 of Table 5.2, period = -31.66 days
		arg = l-2*d+o;
		dpsi += (-0.001286 - 0.000001*T) * ::sin(arg) - 0.000004 * ::cos(arg);
		deps += 0.000694 * ::cos(arg) - 0.000002 * ::sin(arg);
		// line 32 of Table 5.2, period = -346.64 days
		arg = -lp+o;
		dpsi += (-0.001269 + 0.000001*T) * ::sin(arg) + 0.000006 * ::cos(arg);
		deps += (+0.000642 + 0.000001*T) * ::cos(arg) + 0.000002 * ::sin(arg);
		// line 33 of Table 5.2, period = 9.54 days
		arg = -l+2*f+2*d+o;
		dpsi += (-0.001022 - 0.000001*T) * ::sin(arg) + 0.000002 * ::cos(arg);
		deps += 0.000522 * ::cos(arg) + 0.000001 * ::sin(arg);
		// line 34 of Table 5.2, period = -182.63 days
		arg = -2*lp;