DSACSS Operational Code: OrbitObserver.cpp Source File

00001 //////////////////////////////////////////////////////////////////////////////////////////////////
00002 /*! \file: orbitObserver.cpp
00003 *  \brief: Mean Orbit Element Observer for DSACSS.
00004 *  \author $Author: jayhawk_hokie $
00005 *  \version $Revision: 1.1 $
00006 *  \date    $Date: 2007/08/31 16:09:30 $
00007 *//////////////////////////////////////////////////////////////////////////////////////////////////
00008 
00009 
00010 #include "OrbitObserver.h"
00011 
00012 OrbitObserver::OrbitObserver( )
00013 {
00014 
00015 }
00016 
00017 OrbitObserver::~OrbitObserver( )
00018 {
00019 
00020 }
00021 
00022 
00023 /*! \brief Direction Cosine Matrix about the 1 axis
00024  *
00025  *  @param _angle the single axis rotation angle
00026  *  @return _DCM 3x3 matrix
00027  */
00028 Matrix OrbitObserver::DirectionCosMatrix1Axis( double _angle )
00029 {
00030         Matrix _DCM(3,3);
00031         _DCM(1,1) = 1;
00032         _DCM(2,2) = cos( _angle );
00033         _DCM(2,3) = sin( _angle );
00034         _DCM(3,2) = -sin( _angle );
00035         _DCM(3,3) = cos( _angle );
00036         return ( _DCM );
00037 }
00038 
00039 /*! \brief Direction Cosine Matrix about the 2 axis
00040  *
00041  *  @param _angle the single axis rotation angle
00042  *  @return _DCM 3x3 matrix
00043  */
00044 Matrix OrbitObserver::DirectionCosMatrix2Axis( double _angle )
00045 {
00046         Matrix _DCM(3,3);
00047         _DCM(1,1) = cos( _angle );
00048         _DCM(1,3) = -sin( _angle );
00049         _DCM(2,2) = 1;
00050         _DCM(3,1) = sin( _angle );
00051         _DCM(3,3) = cos( _angle );
00052         return ( _DCM );
00053 }
00054 
00055 /*! \brief Direction Cosine Matrix about the 3 axis
00056  *
00057  *  @param _angle the single axis rotation angle
00058  *  @return _DCM 3x3 matrix
00059  */
00060 Matrix OrbitObserver::DirectionCosMatrix3Axis( double _angle )
00061 {
00062         Matrix _DCM(3,3);
00063         _DCM(1,1) = cos( _angle );
00064         _DCM(1,2) = sin( _angle );
00065         _DCM(2,1) = -sin( _angle );
00066         _DCM(2,2) = cos( _angle );
00067         _DCM(3,3) = 1;
00068         return ( _DCM );
00069 }
00070 
00071 /*! \brief Convert from WGS84 ( latitude (rad), longitude (rad), altitude (m) ) to ECEF poisiton (m) )
00072  *
00073  *  @param _latitude (rad)
00074  *  @param _longitude (rad)
00075  *  @param _altitude (m)
00076 
00077  */
00078 Vector OrbitObserver::WGS2ECEFPosition( double _latitude, double _longitude, double _altitude )
00079 {
00080         Vector _ECEFVector(3);
00081         double AA = 6378137.0; // semi-major axis of the eliptical Earth (m)
00082         double f = 1/298.257223563; // flattening ratio
00083         //double BB = AA*(1-f); // semi-minor axis of the eliptical Earth (m)
00084 
00085         double eSquare = 2*f - pow( f, 2 );
00086         double NN = AA / ( sqrt( 1- eSquare*pow( sin( _latitude ), 2 ) ) ); // (m)
00087 
00088         // ECEF Position
00089         _ECEFVector(1) = ( NN + _altitude )*cos( _latitude )*cos( _longitude ); // x (m)
00090         _ECEFVector(2) = ( NN + _altitude )*cos( _latitude )*sin( _longitude ); // y (m)
00091         _ECEFVector(3) = ( NN*(1-eSquare) + _altitude )*sin( _latitude ); // z (m)
00092 
00093         return ( _ECEFVector );
00094 }
00095 
00096 /*! \brief Convert from NED velocity (m/s) to ECEF velocity (m/s)
00097  *
00098  * \par Equation:
00099  * \f[ V_{ECEF} = DCM_{321}(\phi, -\phi-\frac{\pi}{2}, 0 )^{-1}V_{NED} \f]
00100  *
00101  *  @param _latitude (rad)
00102  *  @param _longitude (rad)
00103  *  @param _altitude (m)
00104  *  @param _velocityNorth (m/s)
00105  *  @param _velocityEast (m/s)
00106  *  @param _velocityDown (m/s)
00107  *  @return  _ECEFVector (3x1) [ Vx, Vy, Vz ] (m/s)
00108  */
00109 Vector OrbitObserver::NED2ECEFVelocity( double _latitude, double _longitude, double _altitude, double _velocityNorth, double _velocityEast, double _velocityDown )
00110 {
00111         // ECEF Velocity
00112         Vector _ECEFVector(3);
00113         Matrix DCM321(3,3);
00114         DCM321 = DirectionCosMatrix1Axis( 0 ) * DirectionCosMatrix2Axis( -_latitude-PI/2 ) * DirectionCosMatrix3Axis( _longitude );
00115         Vector VNED(3);
00116         VNED(1) = _velocityNorth;
00117         VNED(2) = _velocityEast;
00118         VNED(3) = _velocityDown;
00119         _ECEFVector( _(1,3) ) = DCM321/VNED;
00120 
00121         return ( _ECEFVector );
00122 }
00123 
00124 /*! \brief Convert from WGS84 ( latitude (rad), longitude (rad), altitude (m) ) to ECEF poisiton (m) and velocity (m/s)
00125  *
00126  *  @param _latitude (rad)
00127  *  @param _longitude (rad)
00128  *  @param _altitude (m)
00129  *  @param _velocityNorth (m/s)
00130  *  @param _velocityEast (m/s)
00131  *  @param _velocityDown (m/s)
00132  *  @return  _ECEFVector (6x1) [ x, y, z, Vx, Vy, Vz ] (m & m/s)
00133  */
00134 Vector OrbitObserver::WGS842ECEF( double _latitude, double _longitude, double _altitude, double _velocityNorth, double _velocityEast, double _velocityDown )
00135 {
00136         Vector _ECEFVector(6);
00137         _ECEFVector( _(1,3) ) = WGS2ECEFPosition( _latitude, _longitude, _altitude );
00138         _ECEFVector( _(4,6) ) = NED2ECEFVelocity( _latitude, _longitude, _altitude, _velocityNorth, _velocityEast, _velocityDown );
00139 
00140         return ( _ECEFVector );
00141 }
00142 
00143 
00144 /*! \brief Convert from ECEF position (m) to WGS84 ( latitude (rad), longitude (rad), altitude (m) )
00145  *
00146  * \par Equation: ( www.colorado.edu/geographyy/gcraft/notes/datum/datum_f.html )
00147  * \f[ \phi = atan{ \left( \frac{ z+e^{'2}b\sin^2{\theta} }{ p-e^2 acos^2{\theta} } \right) } \f]
00148  * \f[ \lambda = atan{ \frac{y}{x} } \f]
00149  * \f[ h = \frac{p}{\cos{\phi}} - N(\phi) \f]
00150  *
00151  * where:
00152  * \f$\phi, \lambda, h \f$ are the geodetic latitude, longitude, and height above ellipsoid
00153  *
00154  * \f[ p = \sqrt{x^2+y^2} \f]
00155  * \f[ \theta = atan \frac{ za }{ pb } \f]
00156  * \f[ e^{'2} = \frac{a^2-b^2}{b^2} \f]
00157  * \f[ N = \frac{a}{ \sqrt{ 1-e^2\sin^2{ \phi } } } \f]
00158  * \f[ e^2 = 2f-f^2 \f]
00159  *
00160  *  @param _x (m)
00161  *  @param _y (m)
00162  *  @param _z (m)
00163  *  @return  _WGS84Vector (3x1) [ latitude, longitude, altitude ] (rad, rad, m)
00164  */
00165 Vector OrbitObserver::ECEFPosition2WGS84( double _x, double _y, double _z )
00166 {
00167         Vector _WGS84Vector(3);
00168 
00169         _WGS84Vector(2) = atan2( _y, _x );  // longitude (rad)
00170 
00171         /* Bowring's Method for determining latitude */
00172         double AA = 6378137.0; // semi-major axis of the eliptical Earth (m)
00173         double f = 1/298.257223563; // flattening ratio
00174         //double BB = AA*(1-f); // semi-minor axis of the eliptical Earth (m)
00175         double p = pow( pow( _x, 2 ) + pow( _y, 2 ), 0.5 );
00176         double _beta = atan2( _z, (1-f)*p );
00177 
00178         double eSquare = 2*f - pow( f, 2 );
00179         double R = AA; // equaterial radius (m)
00180         double num = _z + eSquare*(1-f)/(1-eSquare)*R*pow( sin( _beta ), 3 );
00181         double den = p - eSquare*R*pow( cos( _beta ), 3 );
00182         double _latitudeInitial = atan2( num, den );
00183         _WGS84Vector(1) = _latitudeInitial;
00184 
00185         double TOLERANCE = 10e-9;
00186         double difference = 1;
00187         while( difference  < TOLERANCE )
00188         {
00189                 _beta = atan2( (1-f)*sin( _WGS84Vector(1) ), cos( _WGS84Vector(1) ));
00190                 num = _z + eSquare*(1-f)/(1-eSquare)*R*pow( sin( _beta ), 3 );
00191                 den = p - eSquare*R*pow( cos( _beta ), 3 );
00192                 _WGS84Vector(1) = atan2( num, den );
00193 
00194                 difference = fabs( _latitudeInitial - _WGS84Vector(1) );
00195         }
00196 
00197         double NN = AA / ( sqrt( 1- eSquare*pow( sin( _WGS84Vector(1) ), 2 ) ) ); // (m)
00198 
00199         _WGS84Vector(3) = p*cos( _WGS84Vector(1) ) + (_z+eSquare*NN*sin( _WGS84Vector(1) ))*sin( _WGS84Vector(1) ) - NN; // altitude (m)
00200 
00201         return ( _WGS84Vector );
00202 }
00203 
00204 /*! \brief Convert from ECEF velocity (m/s) to NED velocity (m/s)
00205  *
00206  * \par Equation:
00207  * \f[ V_{NED} = DCM_{321}(\phi, \phi+\frac{\pi}{2}, 0 )V_{ECEF} \f]
00208  *
00209  *  @param _latitude (rad)
00210  *  @param _longitude (rad)
00211  *  @param _altitude (m)
00212  *  @param _Vx (m/s)
00213  *  @param _Vy (m/s)
00214  *  @param _Vz (m/s)
00215  *  @return  _NEDVector (3x1) [ Vx, Vy, Vz ] (m/s)
00216  */
00217 Vector OrbitObserver::ECEF2NEDVelocity( double _latitude, double _longitude, double _altitude, double _Vx, double _Vy, double _Vz )
00218 {
00219         // ECEF Velocity
00220         Vector _NEDVector(3);
00221         Matrix DCM321(3,3);
00222         DCM321 = DirectionCosMatrix1Axis( 0 ) * DirectionCosMatrix2Axis( -_latitude-PI/2 ) * DirectionCosMatrix3Axis( _longitude );
00223         Vector VECEF(3);
00224         VECEF(1) = _Vx;
00225         VECEF(2) = _Vy;
00226         VECEF(3) = _Vz;
00227         _NEDVector( _(1,3) ) = DCM321*VECEF;
00228 
00229         return ( _NEDVector );
00230 }
00231 
00232 
00233 /*! \brief Convert from ECEF position (m) and velocity (m/s) to WGS84 ( latitude (rad), longitude (rad), altitude (m) )
00234  *
00235  *  @param _x (m)
00236  *  @param _y (m)
00237  *  @param _z (m)
00238  *  @param _Vx (m/s)
00239  *  @param _Vy (m/s)
00240  *  @param _Vz (m/s)
00241  *  @return  _WGS84Vector (6x1) [ latitude, longitude, altitude, velocity North, velocity East, velocity Down ] (rad, rad, m, m/s, m/s, m/s)
00242  */
00243 Vector OrbitObserver::ECEF2WGS84( double _x, double _y, double _z, double _Vx, double _Vy, double _Vz )
00244 {
00245         Vector _WGS84Vector(6);
00246         _WGS84Vector( _(1,3) ) = ECEFPosition2WGS84( _x, _y, _z );
00247         _WGS84Vector( _(4,6) ) = ECEF2NEDVelocity( _WGS84Vector(1), _WGS84Vector(2), _WGS84Vector(3), _Vx, _Vy, _Vz );
00248 
00249         return ( _WGS84Vector );
00250 }
00251 
00252 
00253 /*! \brief Convert from ECEF position (m) and velocity (m/s) to ECI position (m) and velocity (m/s).
00254  *
00255  *  @parem _ECEFVector [x, y, z, Vx, Vy, Vz] (m & m/s)
00256  *  @return _ECIVector [x, y, z, Vx, Vy, Vz] (m & m/s)
00257  */
00258 Vector OrbitObserver::ECEF2ECI( Vector _ECEFVector, double _jDay )
00259 {
00260         Vector _ECIVector(6);
00261         Matrix R3_dot(3,3); // initialize matrix
00262         /// Determine thetaGMST
00263         double GMST2000 = 1.7447671633061; // Reference value for Jan 1, 2000 (rad)
00264         double angularRateEarth = 0.000072921158553; // angular rate of earth (rad/s)
00265         double thetaGMST = GMST2000 + angularRateEarth*86400*( _jDay + 0.5 ); // thetaGMST (rad)
00266 
00267         _ECIVector( _(1,3) ) = ( ~DirectionCosMatrix3Axis( thetaGMST ) )*_ECEFVector( _(1,3) ); // ECI x,y,z coordinates (m)
00268                 /// Derivative of rotation matrix (R3)
00269                 R3_dot(1,1)=-sin(thetaGMST)*angularRateEarth;
00270                 R3_dot(1,2)=cos(thetaGMST)*angularRateEarth;
00271                 R3_dot(1,3)=0;
00272                 R3_dot(2,1)=-cos(thetaGMST)*angularRateEarth;
00273                 R3_dot(2,2)=-sin(thetaGMST)*angularRateEarth;
00274                 R3_dot(2,3)=0;
00275                 R3_dot(3,1)=0;
00276                 R3_dot(3,2)=0;
00277                 R3_dot(3,3)=0;
00278         _ECIVector( _(4,6) ) = 2*PI/(24*60*60)*(~R3_dot)*_ECEFVector( _(1,3) )+( ~DirectionCosMatrix3Axis( thetaGMST ) )*_ECEFVector( _(4,6) ); // ECI Vx,Vy,Vz (m/s)
00279 
00280         return ( _ECIVector );
00281 }
00282 
00283 
00284 /*! \brief Convert from ECI position (m) and velocity (m/s) to ECEF position (m) and velocity (m/s).
00285  *
00286  *  @parem _ECIVector [x, y, z, Vx, Vy, Vz] (m & m/s)
00287  *  @return _ECEFVector [x, y, z, Vx, Vy, Vz] (m & m/s)
00288  */
00289 Vector OrbitObserver::ECI2ECEF( Vector _ECIVector, double _jDay )
00290 {
00291         Vector _ECEFVector(6);
00292 
00293         Matrix R3_dot(3,3); // initialize matrix
00294         /// Determine thetaGMST
00295         double GMST2000 = 1.7447671633061; // Reference value for Jan 1, 2000 (rad)
00296         double angularRateEarth = 0.000072921158553; // angular rate of earth (rad/s)
00297         //double thetaGMST = GMST2000 + angularRateEarth*86400*( _jDay  ); // thetaGMST (rad)
00298         double thetaGMST = GMST2000 + angularRateEarth*86400*( _jDay + 0.5 ); // thetaGMST (rad)
00299 
00300         _ECEFVector( _(1,3) ) = ( DirectionCosMatrix3Axis( thetaGMST ) )*_ECIVector( _(1,3) ); // ECI x,y,z coordinates (m)
00301                 /// Derivative of rotation matrix (R3)
00302                 R3_dot(1,1)=-sin(thetaGMST)*angularRateEarth;
00303                 R3_dot(1,2)=cos(thetaGMST)*angularRateEarth;
00304                 R3_dot(1,3)=0;
00305                 R3_dot(2,1)=-cos(thetaGMST)*angularRateEarth;
00306                 R3_dot(2,2)=-sin(thetaGMST)*angularRateEarth;
00307                 R3_dot(2,3)=0;
00308                 R3_dot(3,1)=0;
00309                 R3_dot(3,2)=0;
00310                 R3_dot(3,3)=0;
00311         _ECEFVector( _(4,6) ) = (2*PI)/(24*60*60)*(R3_dot)*_ECIVector( _(1,3) )+( DirectionCosMatrix3Axis( thetaGMST ) )*_ECIVector( _(4,6) ); // ECEF Vx,Vy,Vz (m/s)
00312         return ( _ECEFVector );
00313 }
00314 
00315 
00316 /*! \brief Convert Latitude and Longitude in the form of DEG 'N/S' 'E/W' to +/-RAD.
00317  *
00318  *  @param latlon
00319  *  @param sector
00320  *  return value (+/-rad) of conversion.
00321  */
00322 double OrbitObserver::lat_lon( double latlon, char sector )
00323 {
00324         if (sector=='S') latlon = -latlon;      // for latitude
00325         if (sector=='W') latlon = -latlon;      // for longitude
00326         latlon = Deg2Rad( latlon );               // convert to rad
00327         return ( latlon );
00328 }
00329 
00330 /*! \brief Determine Tangent Plane Velocities (North, East, Down) (m/s)
00331  *  @param recPos
00332  *  @return _WGS84Vector
00333  */
00334 void OrbitObserver::TangentPlaneState( AshtechG12_GPS_PhysicalDevice::Position recPos, Vector &_WGS84Vector )
00335 {
00336         float Conknots          = 0.51444444444444444444444; // Conversion Value (knot->m/s)
00337         double heading          = Deg2Rad(recPos.m_groundTrack);        // Heading (deg->rad)
00338         double speed            = (Conknots*recPos.m_groundSpeed);      // Speed (m/s)
00339         _WGS84Vector(1) = lat_lon(recPos.m_latitude, recPos.m_latitudeSector); // Latitude (deg->rad)
00340         _WGS84Vector(2) = lat_lon(recPos.m_longitude, recPos.m_longitudeSector); // Longitude (deg->rad)
00341         _WGS84Vector(3) = recPos.m_altitude; // altitude (m)
00342         _WGS84Vector(4) = (speed*cos(heading)); // North Velocity
00343         _WGS84Vector(5) = (speed*sin(heading)); // East Velocity
00344         _WGS84Vector(6) = -recPos.m_verticalVelocity; // Down Velocity
00345 }
00346 
00347 // Do not change the comments below - they will be added automatically by CVS
00348 /*****************************************************************************
00349 *       $Log: OrbitObserver.cpp,v $
00350 *       Revision 1.1  2007/08/31 16:09:30  jayhawk_hokie
00351 *       Initial Submission.
00352 *
00353 *
00354 *
00355 *
00356 ******************************************************************************/
00357 
00358 
00359 
00360 
00361