Tudat/Astrodynamics/Propagators/nBodyGaussKeplerStateDerivative.cpp
//! Function to evaluate the Gauss planetary equations for Kepler elements

Function to evaluate the Gauss planetary equations for Kepler elements
Function to evaluate the Gauss planetary equations for Kepler elements
Eigen::Vector6d computeGaussPlanetaryEquationsForKeplerElements(
        const Eigen::Vector6d& currentOsculatingKeplerElements,
        const Eigen::Vector3d& accelerationsInRswFrame,
        const double semiLatusRectum,
        const double distance,
        const double meanMotion,
        const double orbitalAngularMomentum )
{
    double semiMajorAxis = currentOsculatingKeplerElements( 0 );
    double eccentricity = currentOsculatingKeplerElements( 1 );
    double inclination = currentOsculatingKeplerElements( 2 );
    double meanMotionTimesASquared = meanMotion * semiMajorAxis * semiMajorAxis;
    double eccentricityTerm = std::sqrt( 1.0 - eccentricity * eccentricity );
    double sineTrueAnomaly = std::sin( currentOsculatingKeplerElements( 5 ) );
    double cosineTrueAnomaly = std::cos( currentOsculatingKeplerElements( 5 ) );
    double argumentOfLatitude = currentOsculatingKeplerElements( 5 ) + currentOsculatingKeplerElements( 3 );
    Eigen::Vector6d stateDerivative;
    stateDerivative( 0 ) = 2.0 / ( meanMotion * eccentricityTerm ) * (
                eccentricity * sineTrueAnomaly * accelerationsInRswFrame( 0 ) +
                semiLatusRectum / distance * accelerationsInRswFrame( 1 ) );
    stateDerivative( 1 ) = eccentricityTerm / ( meanMotion * semiMajorAxis ) * (
                sineTrueAnomaly * accelerationsInRswFrame( 0 ) +
                ( cosineTrueAnomaly + ( eccentricity + cosineTrueAnomaly ) /
                  (  1.0 + eccentricity * cosineTrueAnomaly ) ) *  accelerationsInRswFrame( 1 ) );
    stateDerivative( 2 ) = distance * std::cos( argumentOfLatitude ) /
            ( meanMotionTimesASquared * eccentricityTerm ) *
            accelerationsInRswFrame( 2 );
    stateDerivative( 3 ) = eccentricityTerm /
            ( meanMotion * semiMajorAxis * eccentricity ) * (
                -cosineTrueAnomaly * accelerationsInRswFrame( 0 ) +
                sineTrueAnomaly * ( 1.0 + distance / semiLatusRectum ) * accelerationsInRswFrame( 1 ) ) -
            distance * std::sin( argumentOfLatitude ) /
            ( orbitalAngularMomentum * std::tan( inclination ) ) *
            accelerationsInRswFrame( 2 );
    stateDerivative( 4 ) = distance * std::sin( argumentOfLatitude ) /
            ( meanMotionTimesASquared * eccentricityTerm * std::sin( inclination ) ) *
            accelerationsInRswFrame( 2 );
    stateDerivative( 5 ) = meanMotion + 1.0 / ( meanMotionTimesASquared * eccentricity ) * (
                ( semiLatusRectum * cosineTrueAnomaly - 2.0 * eccentricity * distance ) * accelerationsInRswFrame( 0 ) -
                ( semiLatusRectum + distance ) * sineTrueAnomaly * accelerationsInRswFrame( 1 ) );
    return stateDerivative;
}
Eigen :: Vector6d computeGaussPlanetaryEquationsForKeplerElements ( const Eigen :: Vector6d & currentOsculatingKeplerElements , const Eigen :: Vector3d & accelerationsInRswFrame , const double semiLatusRectum , const double distance , const double meanMotion , const double orbitalAngularMomentum ) { double semiMajorAxis = currentOsculatingKeplerElements ( 0 ) ; double eccentricity = currentOsculatingKeplerElements ( 1 ) ; double inclination = currentOsculatingKeplerElements ( 2 ) ; double meanMotionTimesASquared = meanMotion * semiMajorAxis * semiMajorAxis ; double eccentricityTerm = std :: sqrt ( 1.0 - eccentricity * eccentricity ) ; double sineTrueAnomaly = std :: sin ( currentOsculatingKeplerElements ( 5 ) ) ; double cosineTrueAnomaly = std :: cos ( currentOsculatingKeplerElements ( 5 ) ) ; double argumentOfLatitude = currentOsculatingKeplerElements ( 5 ) + currentOsculatingKeplerElements ( 3 ) ; Eigen :: Vector6d stateDerivative ; stateDerivative ( 0 ) = 2.0 / ( meanMotion * eccentricityTerm ) * ( eccentricity * sineTrueAnomaly * accelerationsInRswFrame ( 0 ) + semiLatusRectum / distance * accelerationsInRswFrame ( 1 ) ) ; stateDerivative ( 1 ) = eccentricityTerm / ( meanMotion * semiMajorAxis ) * ( sineTrueAnomaly * accelerationsInRswFrame ( 0 ) + ( cosineTrueAnomaly + ( eccentricity + cosineTrueAnomaly ) / ( 1.0 + eccentricity * cosineTrueAnomaly ) ) * accelerationsInRswFrame ( 1 ) ) ; stateDerivative ( 2 ) = distance * std :: cos ( argumentOfLatitude ) / ( meanMotionTimesASquared * eccentricityTerm ) * accelerationsInRswFrame ( 2 ) ; stateDerivative ( 3 ) = eccentricityTerm / ( meanMotion * semiMajorAxis * eccentricity ) * ( - cosineTrueAnomaly * accelerationsInRswFrame ( 0 ) + sineTrueAnomaly * ( 1.0 + distance / semiLatusRectum ) * accelerationsInRswFrame ( 1 ) ) - distance * std :: sin ( argumentOfLatitude ) / ( orbitalAngularMomentum * std :: tan ( inclination ) ) * accelerationsInRswFrame ( 2 ) ; stateDerivative ( 4 ) = distance * std :: sin ( argumentOfLatitude ) / ( meanMotionTimesASquared * eccentricityTerm * std :: sin ( inclination ) ) * accelerationsInRswFrame ( 2 ) ; stateDerivative ( 5 ) = meanMotion + 1.0 / ( meanMotionTimesASquared * eccentricity ) * ( ( semiLatusRectum * cosineTrueAnomaly - 2.0 * eccentricity * distance ) * accelerationsInRswFrame ( 0 ) - ( semiLatusRectum + distance ) * sineTrueAnomaly * accelerationsInRswFrame ( 1 ) ) ; return stateDerivative ; }
Function to evaluate the Gauss planetary equations for Kepler elements
Function to evaluate the Gauss planetary equations for Kepler elements
// Retrieve Kepler elements and precompute reused terms
 // Evaluate Gauss equations.
