phasewindup.cpp

linux的gps应用

#pragma ident "$Id: PhaseWindup.cpp 293 2006-11-10 16:39:56Z rickmach $"

//============================================================================
//
//  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 PhaseWindup.cpp
 * Implement computations of phase windup, solar ephemeris, satellite attitude
 * and eclipse at the satellite.
 */
 
// -----------------------------------------------------------------------------------
// GPSTk includes
#include "Position.hpp"
#include "Matrix.hpp"
#include "geometry.hpp"             // DEG_TO_RAD
#include "icd_200_constants.hpp"    // TWO_PI

using namespace std;
using namespace gpstk;

// -----------------------------------------------------------------------------------
void SolarPosition(DayTime t, double& lat, double& lon, double& R, double& AR);
Matrix<double> SatelliteAttitude(DayTime& tt, Position& SV, double& sf);
double shadowFactor(double Rearth, double Rsun, double dES);
static double GMST(DayTime t);

// -----------------------------------------------------------------------------------
// Given a Position, compute unit (ECEF) vectors in the Up, East and North directions
// at that position. Use geodetic coordinates, i.e. 'up' is perpendicular to the
// geoid. Return the vectors in the form of a 3x3 Matrix<double>, this is in fact the
// rotation matrix that will take an ECEF vector into an 'up-east-north' vector.
Matrix<double> UpEastNorth(Position& P)
{
try {
   Matrix<double> R(3,3);
   P.transformTo(Position::Geodetic);

   double lat = P.getGeodeticLatitude() * DEG_TO_RAD;      // rad N
   double lon = P.getLongitude() * DEG_TO_RAD;             // rad E
   double ca = ::cos(lat);
   double sa = ::sin(lat);
   double co = ::cos(lon);
   double so = ::sin(lon);

   // This is the rotation matrix which will take X=(x,y,z) into (R*X)(up,east,north)
   R(0,0) =  ca*co;  R(0,1) =  ca*so;  R(0,2) = sa;
   R(1,0) =    -so;  R(1,1) =     co;  R(1,2) = 0.;
   R(2,0) = -sa*co;  R(2,1) = -sa*so;  R(2,2) = ca;

   // The rows of R are also the unit vectors, in ECEF, of up,east,north;
   //  R = (U && E && N) = transpose(U || E || N).
   //Matrix<double> U = R.rowCopy(0);
   //Matrix<double> E = R.rowCopy(1);
   //Matrix<double> N = R.rowCopy(2);

   return R;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
catch(exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

// -----------------------------------------------------------------------------------
// Generate a 3x3 rotation Matrix, for direct rotations about one axis
// (for XYZ, axis=123), given the rotation angle in radians;
// @param angle in radians.
// @param axis 1,2,3 as rotation about X,Y,Z.
// @return Rotation matrix (3x3).
// @throw InvalidInput if axis is anything other than 1, 2 or 3.
Matrix<double> SingleAxisRotation(double angle,
                                  int axis)
   throw(Exception)
{
try {
   if(axis < 1 || axis > 3) {
      Exception e(string("Invalid axis (1,2,3 <=> X,Y,Z): ")
            + StringUtils::asString(axis));
      GPSTK_THROW(e);
   }
   Matrix<double> R(3,3,0.0);

   int i1=axis-1;                      // axis = 1 : 0,1,2
   int i2=i1+1; if(i2 == 3) i2=0;      // axis = 2 : 1,2,0
   int i3=i2+1; if(i3 == 3) i3=0;      // axis = 3 : 2,0,1

   R(i1,i1) = 1.0;
   R(i2,i2) = R(i3,i3) = ::cos(angle);
   R(i3,i2) = -(R(i2,i3) = ::sin(angle));

   return R;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
catch(exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

// -----------------------------------------------------------------------------------
// Compute the satellite attitude, given the time and the satellite position SV.
// Return a 3x3 Matrix which contains, as rows, the unit (ECEF) vectors X,Y,Z in the
// body frame of the satellite, namely
//    Z = along the boresight (i.e. towards Earth center),
//    Y = perpendicular to both Z and the satellite-sun direction, and
//    X completing the orthonormal triad. X will generally point toward the sun.
// Also return the shadow factor = fraction of the sun's area visible to satellite.
Matrix<double> SatelliteAttitude(DayTime& tt, Position& SV, double& sf)
{
try {
   int i;
   double d,svrange,lat,lon,DistSun,Radsun,Radearth,dES;
   Position X,Y,Z,T;
   Matrix<double> R(3,3);

   // Z points from satellite to Earth center - along the antenna boresight
   Z = SV;
   Z.transformTo(Position::Cartesian);
   svrange = Z.mag();
   d = -1.0/Z.mag();
   Z = d * Z;                                // reverse and normalize Z

   // T points from satellite to sun
   SolarPosition(tt, lat, lon, DistSun, Radsun);
   Radsun *= DEG_TO_RAD;                     // angular radius of sun at satellite
   Radearth = ::asin(6378137.0/svrange);     // angular radius of earth at sat

   T.setGeocentric(lat,lon,DistSun);         // vector earth to sun
   T.transformTo(Position::Cartesian);
   T = T - SV;                               // sat to sun=(E to sun)-(E to sat)
   d = 1.0/T.mag();
   T = d * T;                                // normalize T

   dES = ::acos(Z.dot(T));                   // apparent angular distance, earth
                                             // to sun, as seen at satellite

   sf = shadowFactor(Radearth, Radsun, dES); // is satellite in eclipse?
   //if(sf > 0.999) { ;    // total eclipse }
   //else if(sf > 0.0) { ; // partial eclipse }
   //else { ;              // no eclipse }

   // Y is perpendicular to Z and T, such that ...
   Y = Z.cross(T);
   d = 1.0/Y.mag();
   Y = d * Y;                                // normalize Y

   // ... X points generally in the direction of the sun
   X = Y.cross(Z);                           // X will be unit vector
   if(X.dot(T) < 0) {                        // need to reverse X, hence Y also
      X = -1.0 * X;
      Y = -1.0 * Y;
   }

   // fill the matrix and return it
   for(i=0; i<3; i++) {
      R(0,i) = X[i];
      R(1,i) = Y[i];
      R(2,i) = Z[i];
   }

   return R;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
catch(exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

// -----------------------------------------------------------------------------------
// Compute the phase windup, in cycles, given the time, the unit vector from receiver
// to transmitter, and the west and north unit vectors at the receiver, all in ECEF.
// YR is the West unit vector, XR is the North unit vector, at the receiver.