Tudat/Astrodynamics/BasicAstrodynamics/unifiedStateModelModifiedRodriguesParameterElementConversions.cpp
//! Convert unified state model elements with modified Rodrigues parameters to Keplerian elements.
Convert unified state model elements with modified Rodrigues parameters to Keplerian elements.
Convert unified state model elements with modified Rodrigues parameters to Keplerian elements .
Eigen::Vector6d convertUnifiedStateModelModifiedRodriguesParametersToKeplerianElements(
        const Eigen::Vector7d& unifiedStateModelElements,
        const double centralBodyGravitationalParameter )
{
    using mathematical_constants::PI;
    Eigen::Vector6d convertedKeplerianElements = Eigen::Vector6d::Zero( );
    const double singularityTolerance = 20.0 * std::numeric_limits< double >::epsilon( );
    Eigen::Vector3d modifiedRodriguesParametersVector = unifiedStateModelElements.segment( sigma1USM6Index, 3 );
    double modifiedRodriguesParametersMagnitude =
            modifiedRodriguesParametersVector.norm( ); 
    bool shadowFlag = int( unifiedStateModelElements( shadowFlagUSM6Index ) ) == 1;
    double modifiedRodriguesParametersMagnitudeSquared =
            modifiedRodriguesParametersMagnitude * modifiedRodriguesParametersMagnitude;
    double onePlusModifiedRodriguesParametersMagnitudeSquaredSquared =
            std::pow( 1.0 + modifiedRodriguesParametersMagnitudeSquared, 2 );
    double oneMinusModifiedRodriguesParametersMagnitudeSquared = shadowFlag ?
                ( modifiedRodriguesParametersMagnitudeSquared - 1.0 ) : 
                ( 1.0 - modifiedRodriguesParametersMagnitudeSquared );
    double oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared =
            std::pow( oneMinusModifiedRodriguesParametersMagnitudeSquared, 2 );
    double denominator = 4.0 * std::pow( modifiedRodriguesParametersVector( 2 ), 2 ) +
            oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared; 
    double rightAscensionOfLatitude =
            std::atan2( 4.0 * modifiedRodriguesParametersVector( 2 ) *
                        ( 1.0 - modifiedRodriguesParametersMagnitudeSquared ) / denominator,
                        ( oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared -
                          4.0 * std::pow( modifiedRodriguesParametersVector( 2 ), 2 ) ) /
                        denominator ); 
    double cosineLambda = std::cos( rightAscensionOfLatitude );
    double sineLambda = std::sin( rightAscensionOfLatitude );
    double auxiliaryParameter1 = unifiedStateModelElements( Rf1HodographUSM6Index ) * cosineLambda +
            unifiedStateModelElements( Rf2HodographUSM6Index ) * sineLambda;
    double auxiliaryParameter2 = unifiedStateModelElements( CHodographUSM6Index ) -
            unifiedStateModelElements( Rf1HodographUSM6Index ) * sineLambda +
            unifiedStateModelElements( Rf2HodographUSM6Index ) * cosineLambda;
    double RHodographElement = std::sqrt( unifiedStateModelElements( Rf1HodographUSM6Index ) *
                                          unifiedStateModelElements( Rf1HodographUSM6Index ) +
                                          unifiedStateModelElements( Rf2HodographUSM6Index ) *
                                          unifiedStateModelElements( Rf2HodographUSM6Index ) );
    convertedKeplerianElements( eccentricityIndex ) =
            RHodographElement / unifiedStateModelElements( CHodographUSM6Index );
    if ( std::fabs( convertedKeplerianElements( eccentricityIndex ) - 1.0 ) < singularityTolerance )
    {
        convertedKeplerianElements( semiLatusRectumIndex ) = centralBodyGravitationalParameter /
                ( unifiedStateModelElements( CHodographUSM6Index ) *
                  unifiedStateModelElements( CHodographUSM6Index ) );
    }
    else
    {
        convertedKeplerianElements( semiMajorAxisIndex ) =
                centralBodyGravitationalParameter /
                ( std::pow( unifiedStateModelElements( CHodographUSM6Index ), 2 ) *
                  ( 1 - std::pow( convertedKeplerianElements( eccentricityIndex ), 2 ) ) );
    }
    double arccosineArgument = ( 4.0 * ( std::pow( modifiedRodriguesParametersVector( 2 ), 2 ) -
                                         std::pow( modifiedRodriguesParametersVector( 0 ), 2 ) -
                                         std::pow( modifiedRodriguesParametersVector( 1 ), 2 ) ) +
                                 oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared ) /
            onePlusModifiedRodriguesParametersMagnitudeSquaredSquared;
    if ( ( ( std::fabs( arccosineArgument ) - 1.0 ) > 0.0 ) ||
         ( std::abs( std::fabs( arccosineArgument ) - 1.0 ) < singularityTolerance ) )
    {
        arccosineArgument = ( arccosineArgument > 0.0 ) ? 1.0 : - 1.0;
    }
    convertedKeplerianElements( inclinationIndex ) = std::acos( arccosineArgument );
    double signDirectionCosineMatrix = shadowFlag ? - 1.0 : 1.0; 
    double sineOmega = 8.0 * modifiedRodriguesParametersVector( 0 ) *
            modifiedRodriguesParametersVector( 2 ) + signDirectionCosineMatrix *
            4.0 * modifiedRodriguesParametersVector( 1 ) *
            oneMinusModifiedRodriguesParametersMagnitudeSquared;
    double cosineOmega = - 8.0 * modifiedRodriguesParametersVector( 1 ) *
            modifiedRodriguesParametersVector( 2 ) + signDirectionCosineMatrix *
            4.0 * modifiedRodriguesParametersVector( 0 ) *
            oneMinusModifiedRodriguesParametersMagnitudeSquared;
    denominator = std::sqrt( cosineOmega * cosineOmega +
                             sineOmega * sineOmega ); 
    if ( std::fabs( convertedKeplerianElements( inclinationIndex ) - PI ) < singularityTolerance )
    {
        std::stringstream errorMessage;
        errorMessage << "Pure-retrograde orbit (inclination = PI).\n"
                     << "Unified state model elements cannot be transformed to Kepler elements." << std::endl;
        throw std::runtime_error( std::runtime_error( errorMessage.str( ) ) );
    }
    else if ( ( std::fabs( convertedKeplerianElements( inclinationIndex ) ) < singularityTolerance ) ||
              ( std::fabs( denominator ) < singularityTolerance ) )
    {
        convertedKeplerianElements( longitudeOfAscendingNodeIndex ) = 0.0;
    }
    else
    {
        convertedKeplerianElements( longitudeOfAscendingNodeIndex ) = std::atan2(
                    sineOmega / denominator, cosineOmega / denominator );
        if ( std::fabs( convertedKeplerianElements( longitudeOfAscendingNodeIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( longitudeOfAscendingNodeIndex ) = 0.0;
        }
        while ( convertedKeplerianElements( longitudeOfAscendingNodeIndex ) < 0.0 )
        {
            convertedKeplerianElements( longitudeOfAscendingNodeIndex ) += 2.0 * PI;
        }
    }
    if ( std::fabs( RHodographElement ) < singularityTolerance ) 
    {
        convertedKeplerianElements( argumentOfPeriapsisIndex ) = 0.0; 
        convertedKeplerianElements( trueAnomalyIndex ) =
                rightAscensionOfLatitude - convertedKeplerianElements( longitudeOfAscendingNodeIndex );
        if ( std::fabs( convertedKeplerianElements( trueAnomalyIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( trueAnomalyIndex ) = 0.0;
        }
        while ( convertedKeplerianElements( trueAnomalyIndex ) < 0.0 )
        {
            convertedKeplerianElements( trueAnomalyIndex ) += 2.0 * PI;
        }
    }
    else
    {
        convertedKeplerianElements( trueAnomalyIndex ) =
                std::atan2( ( auxiliaryParameter1 / RHodographElement ),
                            ( ( auxiliaryParameter2 - unifiedStateModelElements( CHodographUSM6Index ) )
                              / RHodographElement ) );
        if ( std::fabs( convertedKeplerianElements( trueAnomalyIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( trueAnomalyIndex ) = 0.0;
        }
        while ( convertedKeplerianElements( trueAnomalyIndex ) < 0.0 )
        {
            convertedKeplerianElements( trueAnomalyIndex ) += 2.0 * PI;
        }
        convertedKeplerianElements( argumentOfPeriapsisIndex ) =
                rightAscensionOfLatitude -
                convertedKeplerianElements( longitudeOfAscendingNodeIndex ) -
                convertedKeplerianElements( trueAnomalyIndex );
        if ( std::fabs( convertedKeplerianElements( argumentOfPeriapsisIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( argumentOfPeriapsisIndex ) = 0.0;
        }
        while ( convertedKeplerianElements( argumentOfPeriapsisIndex ) < 0.0 )
        {
            convertedKeplerianElements( argumentOfPeriapsisIndex ) += 2.0 * PI;
        }
    }
    return convertedKeplerianElements;
}
Eigen :: Vector6d convertUnifiedStateModelModifiedRodriguesParametersToKeplerianElements ( const Eigen :: Vector7d & unifiedStateModelElements , const double centralBodyGravitationalParameter ) { using mathematical_constants :: PI ; Eigen :: Vector6d convertedKeplerianElements = Eigen :: Vector6d :: Zero ( ) ; const double singularityTolerance = 20.0 * std :: numeric_limits < double > :: epsilon ( ) ; Eigen :: Vector3d modifiedRodriguesParametersVector = unifiedStateModelElements . segment ( sigma1USM6Index , 3 ) ; double modifiedRodriguesParametersMagnitude = modifiedRodriguesParametersVector . norm ( ) ; bool shadowFlag = int ( unifiedStateModelElements ( shadowFlagUSM6Index ) ) == 1 ; double modifiedRodriguesParametersMagnitudeSquared = modifiedRodriguesParametersMagnitude * modifiedRodriguesParametersMagnitude ; double onePlusModifiedRodriguesParametersMagnitudeSquaredSquared = std :: pow ( 1.0 + modifiedRodriguesParametersMagnitudeSquared , 2 ) ; double oneMinusModifiedRodriguesParametersMagnitudeSquared = shadowFlag ? ( modifiedRodriguesParametersMagnitudeSquared - 1.0 ) : ( 1.0 - modifiedRodriguesParametersMagnitudeSquared ) ; double oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared = std :: pow ( oneMinusModifiedRodriguesParametersMagnitudeSquared , 2 ) ; double denominator = 4.0 * std :: pow ( modifiedRodriguesParametersVector ( 2 ) , 2 ) + oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared ; double rightAscensionOfLatitude = std :: atan2 ( 4.0 * modifiedRodriguesParametersVector ( 2 ) * ( 1.0 - modifiedRodriguesParametersMagnitudeSquared ) / denominator , ( oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared - 4.0 * std :: pow ( modifiedRodriguesParametersVector ( 2 ) , 2 ) ) / denominator ) ; double cosineLambda = std :: cos ( rightAscensionOfLatitude ) ; double sineLambda = std :: sin ( rightAscensionOfLatitude ) ; double auxiliaryParameter1 = unifiedStateModelElements ( Rf1HodographUSM6Index ) * cosineLambda + unifiedStateModelElements ( Rf2HodographUSM6Index ) * sineLambda ; double auxiliaryParameter2 = unifiedStateModelElements ( CHodographUSM6Index ) - unifiedStateModelElements ( Rf1HodographUSM6Index ) * sineLambda + unifiedStateModelElements ( Rf2HodographUSM6Index ) * cosineLambda ; double RHodographElement = std :: sqrt ( unifiedStateModelElements ( Rf1HodographUSM6Index ) * unifiedStateModelElements ( Rf1HodographUSM6Index ) + unifiedStateModelElements ( Rf2HodographUSM6Index ) * unifiedStateModelElements ( Rf2HodographUSM6Index ) ) ; convertedKeplerianElements ( eccentricityIndex ) = RHodographElement / unifiedStateModelElements ( CHodographUSM6Index ) ; if ( std :: fabs ( convertedKeplerianElements ( eccentricityIndex ) - 1.0 ) < singularityTolerance ) { convertedKeplerianElements ( semiLatusRectumIndex ) = centralBodyGravitationalParameter / ( unifiedStateModelElements ( CHodographUSM6Index ) * unifiedStateModelElements ( CHodographUSM6Index ) ) ; } else { convertedKeplerianElements ( semiMajorAxisIndex ) = centralBodyGravitationalParameter / ( std :: pow ( unifiedStateModelElements ( CHodographUSM6Index ) , 2 ) * ( 1 - std :: pow ( convertedKeplerianElements ( eccentricityIndex ) , 2 ) ) ) ; } double arccosineArgument = ( 4.0 * ( std :: pow ( modifiedRodriguesParametersVector ( 2 ) , 2 ) - std :: pow ( modifiedRodriguesParametersVector ( 0 ) , 2 ) - std :: pow ( modifiedRodriguesParametersVector ( 1 ) , 2 ) ) + oneMinusModifiedRodriguesParametersMagnitudeSquaredSquared ) / onePlusModifiedRodriguesParametersMagnitudeSquaredSquared ; if ( ( ( std :: fabs ( arccosineArgument ) - 1.0 ) > 0.0 ) || ( std :: abs ( std :: fabs ( arccosineArgument ) - 1.0 ) < singularityTolerance ) ) { arccosineArgument = ( arccosineArgument > 0.0 ) ? 1.0 : - 1.0 ; } convertedKeplerianElements ( inclinationIndex ) = std :: acos ( arccosineArgument ) ; double signDirectionCosineMatrix = shadowFlag ? - 1.0 : 1.0 ; double sineOmega = 8.0 * modifiedRodriguesParametersVector ( 0 ) * modifiedRodriguesParametersVector ( 2 ) + signDirectionCosineMatrix * 4.0 * modifiedRodriguesParametersVector ( 1 ) * oneMinusModifiedRodriguesParametersMagnitudeSquared ; double cosineOmega = - 8.0 * modifiedRodriguesParametersVector ( 1 ) * modifiedRodriguesParametersVector ( 2 ) + signDirectionCosineMatrix * 4.0 * modifiedRodriguesParametersVector ( 0 ) * oneMinusModifiedRodriguesParametersMagnitudeSquared ; denominator = std :: sqrt ( cosineOmega * cosineOmega + sineOmega * sineOmega ) ; if ( std :: fabs ( convertedKeplerianElements ( inclinationIndex ) - PI ) < singularityTolerance ) { std :: stringstream errorMessage ; errorMessage << " \n " << " " << std :: endl ; throw std :: runtime_error ( std :: runtime_error ( errorMessage . str ( ) ) ) ; } else if ( ( std :: fabs ( convertedKeplerianElements ( inclinationIndex ) ) < singularityTolerance ) || ( std :: fabs ( denominator ) < singularityTolerance ) ) { convertedKeplerianElements ( longitudeOfAscendingNodeIndex ) = 0.0 ; } else { convertedKeplerianElements ( longitudeOfAscendingNodeIndex ) = std :: atan2 ( sineOmega / denominator , cosineOmega / denominator ) ; if ( std :: fabs ( convertedKeplerianElements ( longitudeOfAscendingNodeIndex ) ) < singularityTolerance ) { convertedKeplerianElements ( longitudeOfAscendingNodeIndex ) = 0.0 ; } while ( convertedKeplerianElements ( longitudeOfAscendingNodeIndex ) < 0.0 ) { convertedKeplerianElements ( longitudeOfAscendingNodeIndex ) += 2.0 * PI ; } } if ( std :: fabs ( RHodographElement ) < singularityTolerance ) { convertedKeplerianElements ( argumentOfPeriapsisIndex ) = 0.0 ; convertedKeplerianElements ( trueAnomalyIndex ) = rightAscensionOfLatitude - convertedKeplerianElements ( longitudeOfAscendingNodeIndex ) ; if ( std :: fabs ( convertedKeplerianElements ( trueAnomalyIndex ) ) < singularityTolerance ) { convertedKeplerianElements ( trueAnomalyIndex ) = 0.0 ; } while ( convertedKeplerianElements ( trueAnomalyIndex ) < 0.0 ) { convertedKeplerianElements ( trueAnomalyIndex ) += 2.0 * PI ; } } else { convertedKeplerianElements ( trueAnomalyIndex ) = std :: atan2 ( ( auxiliaryParameter1 / RHodographElement ) , ( ( auxiliaryParameter2 - unifiedStateModelElements ( CHodographUSM6Index ) ) / RHodographElement ) ) ; if ( std :: fabs ( convertedKeplerianElements ( trueAnomalyIndex ) ) < singularityTolerance ) { convertedKeplerianElements ( trueAnomalyIndex ) = 0.0 ; } while ( convertedKeplerianElements ( trueAnomalyIndex ) < 0.0 ) { convertedKeplerianElements ( trueAnomalyIndex ) += 2.0 * PI ; } convertedKeplerianElements ( argumentOfPeriapsisIndex ) = rightAscensionOfLatitude - convertedKeplerianElements ( longitudeOfAscendingNodeIndex ) - convertedKeplerianElements ( trueAnomalyIndex ) ; if ( std :: fabs ( convertedKeplerianElements ( argumentOfPeriapsisIndex ) ) < singularityTolerance ) { convertedKeplerianElements ( argumentOfPeriapsisIndex ) = 0.0 ; } while ( convertedKeplerianElements ( argumentOfPeriapsisIndex ) < 0.0 ) { convertedKeplerianElements ( argumentOfPeriapsisIndex ) += 2.0 * PI ; } } return convertedKeplerianElements ; }
Convert unified state model elements with modified Rodrigues parameters to Keplerian elements.
Convert unified state model elements with modified Rodrigues parameters to Keplerian elements .
// Declaring eventual output vector. // Define the tolerance of a singularity // Extract modified Rodrigues parameters // magnitude of modified Rodrigues parameters, also called sigma // Precompute often used variables // Note the for SMRP some variable names do not match their definitions // inverse definition for SMRP // Compute right ascension of latitude (independent on shadowing, since variables have been precomputed) // denominator is never null // also known as lambda // Trigonometric values of right ascension of latitude // Compute auxiliary parameters auxiliaryParameter1 and auxiliaryParameter2 // Compute auxiliary R hodograph parameter // Compute eccentricity // Compute semi-major axis or, in case of a parabolic orbit, the semi-latus rectum. // parabolic orbit -> semi-major axis is not defined. Use semi-latus rectum instead. // Compute inclination // Make sure that the cosine does not exceed 1.0 in magnitude // this acos is always defined correctly because the inclination is always below PI rad // Find sine and cosine of longitude of ascending node separately // sign depends on shadow conditions // overwrite // Compute longitude of ascending node // pure-retrograde orbit -> inclination = PI // Define the error message // Throw exception // pure-prograde orbit -> inclination = 0 -> RAAN is undefined, hence give zero // denominator is null, find a workaround // Compute longitude of ascending node // Round off small values of the right ascension of ascending node to zero // Ensure the longitude of ascending node is positive // Because of the previous if statement, if the longitude of ascending node is smaller than 0, it will // always be smaller than -singularityTolerance // Compute true anomaly and argument of periapsis // circular orbit // by definition // Round off small theta to zero // Ensure the true anomaly is positive // Because of the previous if statement, if the true anomaly is smaller than zero, it will always be smaller than // -singularityTolerance // Round off small theta to zero // Ensure the true anomaly is positive // Because of the previous if statement, if the true anomaly is smaller than zero, it will always // be smaller than -singularityTolerance // Round off small omega to zero // Ensure the argument of periapsis is positive // Because of the previous if statement, if the argument of pericenter is smaller than zero, // it will be smaller than -singularityTolerance // Give back result