DSACSS Operational Code: OrbitController.cpp Source File

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