MappingMeanOsculatingOrbitElements.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file MappingMeanOsculatingOrbitElements.cpp
*  \brief First order mapping algorithm between mean orbit elments and osculating orbit elements.
*       Referenced from "Anayltic Mechanics of Space Systems" by Schaub and Junkins (p.693).
*  \author $Author: jayhawk_hokie $
*  \version $Revision: 1.2 $
*  \date    $Date: 2007/08/31 16:52:10 $
*//////////////////////////////////////////////////////////////////////////////////////////////////
/*
*
*/
//////////////////////////////////////////////////////////////////////////////////////////////////

#include "MappingMeanOsculatingOrbitElements.h"

namespace O_SESSAME 
{

/*! \brief Default Deconstructor */
OsculatingOrbitalElements::~OsculatingOrbitalElements()
{
}


/*! \brief Return a pointer to a new instance of a OsculatingOrbitELement orbit state representation type.
 *
 *  This is used to request memory for a new instance of a OsculatingOrbitELement. It is necessary
 *      when attempting to get a pointer from the abstract data type OrbitStateRepresentation
 *      and the actual representation type isn't known.
 * @return a pointer to a new allocation of memory for the Keplerian object.
 */
OsculatingOrbitalElements* OsculatingOrbitalElements::NewPointer()
{
    return new OsculatingOrbitalElements();
}


/*! \brief Return a pointer to a copy of the OsculatingOrbitalElements orbit state representation instance.
 *
 *  This is used to request memory for a copy of this instance of Keplerian. It is necessary
 *      when attempting to get a pointer from the abstract data type OrbitStateRepresentation
 *      and the actual representation type isn't known.
 * @return a pointer to a copy of the OsculatingOrbitalElements object.
 */
OsculatingOrbitalElements* OsculatingOrbitalElements::Clone()
{
    return new OsculatingOrbitalElements(*this);
}


/*! \brief Create an initially empty OsculatingOrbitalElements orbit state representation. 
 *
 */
OsculatingOrbitalElements::OsculatingOrbitalElements()  
{
}


/*! \brief Set the osculating oribtal elements with this function. 
 *
 *  @param _OsculatingOrbitalElements
 */
//Keplerian OsculatingOrbitalElements::SetOsculatingOrbitalElements( Keplerian &_OsculatingOrbitalElements )
void OsculatingOrbitalElements::SetOsculatingOrbitalElements( Keplerian& _OsculatingOrbitalElements )
{
        m_OsculatingOrbitalElements = _OsculatingOrbitalElements.KeplerianCopy( );
        
        OsculatingToMean( );
        
        return;
}

/*! \brief Set the osculating oribtal elements with this function. 
 *
 *  @param _ECIVector (6x1) km & km/s
 */
void OsculatingOrbitalElements::SetOsculatingOrbitalElements( CAMdoubleVector _ECIVector )
{
        Keplerian _OsculatingOrbitalElements;
        _OsculatingOrbitalElements.SetPositionVelocity( _ECIVector );
        m_OsculatingOrbitalElements = _OsculatingOrbitalElements.KeplerianCopy( );
        
        OsculatingToMean( );
        
        return;
}

/*! \brief Set the mean oribtal elements with this function. 
 *
 */
void OsculatingOrbitalElements::SetMeanOrbitalElements( Keplerian& _MeanOrbitalElements )
{
        m_MeanOrbitalElements = _MeanOrbitalElements.KeplerianCopy( );

        MeanToOsculating( );
        
        return;
}

/*! \brief Set the mean oribtal elements with this function. 
 *
 */
void OsculatingOrbitalElements::SetMeanOrbitalElements( CAMdoubleVector _ECIVector )
{
        Keplerian _MeanOrbitalElements;
        _MeanOrbitalElements.SetPositionVelocity( _ECIVector );
        m_MeanOrbitalElements = _MeanOrbitalElements.KeplerianCopy( );

        MeanToOsculating( );
        
        return;
}


/*! \brief Get the Mean oribtal elements with this function. 
 *
 */
Keplerian OsculatingOrbitalElements::GetMeanOrbitalElements( )
{
        return( m_MeanOrbitalElements );
}

/*! \brief Get the Osculating oribtal elements with this function. 
 *
 */
Keplerian OsculatingOrbitalElements::GetOsculatingOrbitalElements( )
{
        return( m_OsculatingOrbitalElements );
}


/*! \brief Convert the Osculating oribtal elements to Mean Orbital Elements. 
 *
 */
void OsculatingOrbitalElements::OsculatingToMean( )
{
        m_Gamma2 = -J2 / 2 * pow( ( Re / m_OsculatingOrbitalElements.GetSemimajorAxis() ),2);

        m_MeanOrbitalElements = Mapping( m_OsculatingOrbitalElements );

        return;
}


/*! \brief Convert the Mean oribtal elements to Osculating Orbital Elements. 
 *
 */
void OsculatingOrbitalElements::MeanToOsculating( )
{
        m_Gamma2 = -J2 / 2 * pow( ( Re / m_MeanOrbitalElements.GetSemimajorAxis() ),2);

        m_MeanOrbitalElements = Mapping( m_MeanOrbitalElements );

        return;
}


/*! \brief Convert the Mean oribtal elements to Osculating Orbital Elements. 
 *  This mapping algorithm is from page 693 of Analytical Mechanics of Space
 *  Systems by Schaub and Junkins.
 *
 *  @param _keplerian variable of class Keplerian
 *
 *  @return the mapped keplerian representation
 */
Keplerian OsculatingOrbitalElements::Mapping( Keplerian& _keplerian )
{

        double  a       = _keplerian.GetSemimajorAxis(); 
        double  e       = _keplerian.GetEccentricity();
        double  i       = _keplerian.GetInclination();
        double  Lon     = _keplerian.GetLongAscNode();
        double  Arg     = _keplerian.GetArgPerigee();
        double  tru     = _keplerian.GetTrueAnomaly();  

        double eta      = sqrt( 1-pow( e, 2 ) );

        double gamma2_T = m_Gamma2 / pow( eta, 4 ); // gamma2 transformed

        double E        = _keplerian.GetEccentricAnomaly();
        /* Require E to be positive */
        if( E < 0 )
                E = 2*PI + E; // return the appropriate Eccentric Anomaly which is > 0
        /* This is wrong.
        if(E < 0)
        {
                E = -E;
        }
        */

        double M        = _keplerian.GetMeanAnomaly();

        double a_r      = ( 1+e*cos( tru ) ) / pow( eta, 2 ); // a/r

        /* Semi-major Axis Mapping (km) */
        double _a       = a + a*m_Gamma2*
                        (
                                (
                                3*pow(cos(i),2)-1
                                )
                        *
                                (
                                pow(a_r,3))-1/(pow(eta,3)
                                )
                        + 3*
                                (
                                1-pow(cos(i),2)
                                )
                        *pow(a_r,3)*cos(2*Arg+2*tru)
                        ); 

        /* Intermediate Results for the transformation */
        double de1 = gamma2_T/8*e*pow(eta,2)*
                        (
                        1-11*pow(cos(i),2)-40*pow(cos(i),4)/(1-5*pow(cos(i),2))
                        )
                        *cos(2*Arg);

        double de = de1 + pow(eta,2)/2*
                (
                m_Gamma2*
                        (
                (3*pow(cos(i),2)-1)/pow(eta,6)*
                                (
                e*eta + e/(1+eta) + 3*cos(tru) + 3*e*pow(cos(tru),2) + pow(e,2)*pow(cos(tru),3)
                                ) 
                + 3*
                                (
                1-pow(cos(i),2)
                                )
                /pow(eta,6)*
                                (
                e + 3*cos(tru) + 3*e*pow(cos(tru),2) + pow(e,2)*pow(cos(tru),3)
                                )
                *cos(2*Arg+3*tru)
                        )
                - gamma2_T*
                        (
                1-pow(cos(i),2)
                        )
                *
                        (
                3*cos(2*Arg+tru)+cos(2*Arg+3*tru)
                        )
                );

        double di = -e*de1/(pow(eta,2)*tan(i)) + gamma2_T/2*cos(i)*sqrt(1-pow(cos(i),2))*
                        (
                        3*cos(2*Arg+2*tru) + 3*e*cos(2*Arg+tru) + e*cos(2*Arg+3*tru)
                        );

        // M_T+Arg_T+Lon_T = term
        double term = M + Arg + Lon + gamma2_T/8*pow(eta,3)*
                        (
                        1 - 11*pow(cos(i),2) - 40*pow(cos(i),4)/(1-5*pow(cos(i),2))
                        )
                        -gamma2_T/16*
                        (
                        2 + pow(e,2) - 11*
                                (
                        2+3*pow(e,2)
                                )
                        *pow(cos(i),2) - 40*
                                (
                        2+5*pow(e,2)
                                )*pow(cos(i),4)/
                                (
                        1-5*pow(cos(i),2)
                                )
                        -400*pow(e,2)*pow(cos(i),6)/pow((1-5*pow(cos(i),2)),2)
                        )
                        +gamma2_T/4*
                        (
                        -6*
                                (
                        1-5*pow(cos(i),2)
                                )
                        *
                                (
                        tru - M + e*sin(tru)
                                )
                        +
                                (
                        3 - 5*pow(cos(i),2)
                                )
                        *
                                (
                        3*sin(2*Arg+2*tru) + 3*e*sin(2*Arg+tru) + e*sin(2*Arg+3*tru)
                                )
                        )
                        -gamma2_T/8*pow(e,2)*cos(i)*
                        (
                        11 + 80*pow(cos(i),2)/(1-5*pow(cos(i),2)) + 200*pow(cos(i),4)/pow((1-5*pow(cos(i),2)),2)
                        )
                        -gamma2_T/2*cos(i)*
                        (
                        6*(tru-M+e*sin(tru))-3*sin(2*Arg+2*tru)-3*e*sin(2*Arg+tru)-e*sin(2*Arg+3*tru)
                        );

        double edM = gamma2_T/8*e*pow(eta,3)*
                        (
                        1 - 11*pow(cos(i),2) - 40*pow(cos(i),2)/(1-5*pow(cos(i),2))
                        )
                        - gamma2_T/4*pow(eta,3)*
                        (
                        2*(3*pow(cos(i),2)-1)*
                                (
                        pow(a_r*eta,2)+a_r+1
                                )
                        *sin(tru) + 3*(1-pow(cos(i),2))*
                                (
                                        (
                        -pow(a_r*eta,2)-a_r+1
                                        )
                        *sin(2*Arg+tru) +                 
                                        (
                        pow(a_r*eta,2)+a_r+1/3
                                        )
                        *sin(2*Arg+3*tru)
                                )
                        );

        double dLon = -gamma2_T/8*pow(e,2)*cos(i)*
                        (
                        11 + 80*pow(cos(i),2)/(1-5*pow(cos(i),2)) + 200*pow(cos(i),4)/pow((1-5*pow(cos(i),2)),2)
                        )
                        -gamma2_T/2*cos(i)*
                        (
                        6*(tru-M+e*sin(tru))-3*sin(2*Arg+2*tru)-3*e*sin(2*Arg+tru)-e*sin(2*Arg+3*tru)
                        );

        
        /* Compute the remaining transformed orbit elements */
        
        double d1 = (e+de)*sin(M)+edM*cos(M);
        double d2 = (e+de)*cos(M)-edM*sin(M);
 
        double _M = atan2(d1,d2);

        double _e = sqrt(pow(d1,2)+pow(d2,2));

        double d3 = (sin(i/2)+cos(i/2)*di/2)*sin(Lon)+sin(i/2)*dLon*cos(Lon);
        double d4 = (sin(i/2)+cos(i/2)*di/2)*cos(Lon)-sin(i/2)*dLon*sin(Lon);  

        double _Lon     = atan2(d3,d4);

        double _i       = 2*asin(sqrt(pow(d3,2)+pow(d4,2)));

        double _Arg = term - _M - _Lon;

        
        /* Format Argument of Perigee so that the number is represented between 0 and 2*pi */
        int cycles      = (int)( _Arg / (2*PI) );
        _Arg            = _Arg - cycles*2*PI;

        Vector _OrbitalElements(6);
        _OrbitalElements( VectorIndexBase + 0 ) = _a;
        _OrbitalElements( VectorIndexBase + 1 ) = _e;
        _OrbitalElements( VectorIndexBase + 2 ) = _i;
        _OrbitalElements( VectorIndexBase + 3 ) = _Lon;
        _OrbitalElements( VectorIndexBase + 4 ) = _Arg;
        _OrbitalElements( VectorIndexBase + 5 ) = _M;
        
        Keplerian _MappedKeplerian;
        _MappedKeplerian.SetKeplerianRepresentationMeanAnomaly(  _OrbitalElements ) ;

        return( _MappedKeplerian );
} 


/*
// test code
int main()
{
        cout << "Mean COE Test Section" << endl;
        double          re      = 6378.1; // radius of earth (km)
        double          J2      = 0.00108263; // gravitational perturbation constant
        
        Vector ECI(3);
        Vector VECI(3);
        ECI(1) =  -4255599.88588388; // km
        ECI(2) = -4255599.88588388;
        ECI(3) = 3200001.26841524;
        VECI(1) = 5410.04028290564;
        VECI(2) =  -5410.04028290564; // km/s
        VECI(3) = 0; // km/s

        //ECI(1) =  6878137; // km
        //ECI(2) = 0;
        //ECI(3) = 0;
        //VECI(1) = 0;
        //VECI(2) =  6596.1; // km/s
        //VECI(3) = 3808.3; // km/s
        
        Keplerian test; // initialize test coe
        test.SetPositionVelocity(ECI/1000,VECI/1000); // ( m m/s)

        double  a = test.GetSemimajorAxis(); 
        double  e = test.GetEccentricity();
        double  i = test.GetInclination();
        double  Lon = test.GetLongAscNode();
        double  Arg = test.GetArgPerigee();
        double  tru = test.GetTrueAnomaly();    

        cout << " osc: " << a << " " << e << " " << i << " " << Lon << " " << Arg << " " << tru << endl;

        // 
        // Check conversion from ECI to COE and COE to ECI
        Vector check(6);
        check = COE2ECI(test);
        cout << "Original ECI: " << ECI << VECI << endl;
        cout << "Recal ECI: " << check << endl;

        //
        // Check mean calculation
        Transform meanCOE = osculating2mean(test, re, J2);
        Vector RV(6);
        Vector mycoe(6);
        mycoe(1) = meanCOE.SemimajorAxis;
        mycoe(2) = meanCOE.Eccentricity;
        mycoe(3) = meanCOE.Inclination;
        mycoe(4) = meanCOE.LongAscNode;
        mycoe(5) = meanCOE.ArgPerigee;
        mycoe(6) = test.GetTrueAnomaly();
        RV = COE2ECI(mycoe);
        Keplerian test2; // initialize test coe
        Vector ECI2(3);
        Vector VECI2(3);
        ECI2(1) = RV(1);
        ECI2(2) = RV(2);
        ECI2(3) = RV(3);
        VECI2(1) = RV(4);
        VECI2(2) = RV(5);
        VECI2(3) = RV(6);
        test2.SetPositionVelocity(ECI2,VECI2); // ( km km/s)
        
        Transform oscCOE = mean2osculating(test2, re, J2);

        cout << "a mean " << test2.GetSemimajorAxis() << endl;
        cout << "Values " << meanCOE.SemimajorAxis << " " << meanCOE.Eccentricity << " " << 
        meanCOE.Inclination << " " << meanCOE.LongAscNode << " " << meanCOE.ArgPerigee << " " 
        << " " << meanCOE.MeanAnomaly << endl;

        cout << "Osc Values " << oscCOE.SemimajorAxis << " " << oscCOE.Eccentricity << " " << 
        oscCOE.Inclination << " " << oscCOE.LongAscNode << " " << oscCOE.ArgPerigee << " " 
        << " " << oscCOE.MeanAnomaly << endl;

        return 0;
}
*/

}

---