//! Function to compute proper-time rate w.r.t. coordinate time, minus 1.0, from a speed and scalar potential
double calculateFirstCentralBodyProperTimeRateDifference(
        const Eigen::Vector6d relativeStateVector, const double centralBodyGravitationalParameter,
        const double equivalencePrincipleLpiViolationParameter )
{
    return calculateFirstCentralBodyProperTimeRateDifference(
                relativeStateVector.segment( 3, 3 ).norm( ), centralBodyGravitationalParameter /
                relativeStateVector.segment( 0, 3 ).norm( ), equivalencePrincipleLpiViolationParameter );
}
Пример #2
0
//! Function to get the unit vector colinear with position segment of a translational state.
Eigen::Vector3d getForceDirectionColinearWithPosition(
        const boost::function< void( Eigen::Vector6d& ) > currentStateFunction,
        const double currentTime, const bool putForceInOppositeDirection )
{
    static Eigen::Vector6d currentState;
    currentStateFunction( currentState );
    return ( ( putForceInOppositeDirection == 1 ) ? -1.0 : 1.0 ) * ( currentState.segment( 0, 3 ) ).normalized( );
}
//! Convert dimensionless Cartesian state to dimensional units.
Eigen::VectorXd convertDimensionlessCartesianStateToDimensionalUnits(
        const Eigen::Vector6d &dimensionlessCartesianState,
        const double gravitationalParameterOfPrimaryBody,
        const double gravitationalParameterOfSecondaryBody,
        const double distanceBetweenPrimaries )
{
    // Declare dimensional Cartesian state.
    Eigen::Vector6d dimensionalCartesianState;

    // Convert position to dimensional units.
    dimensionalCartesianState.segment( 0, 3 )
            = dimensionlessCartesianState.segment( 0, 3 ) * distanceBetweenPrimaries;

    // Convert velocity to dimensional units.
    dimensionalCartesianState.segment( 3, 3 )
            = dimensionlessCartesianState.segment( 3, 3 )
            * std::sqrt( ( gravitationalParameterOfPrimaryBody
                           + gravitationalParameterOfSecondaryBody ) / distanceBetweenPrimaries );

    // Return state in dimensional units.
    return dimensionalCartesianState;
}
//! Compute state derivative.
Eigen::Vector6d StateDerivativeCircularRestrictedThreeBodyProblem::computeStateDerivative(
        const double time, const Eigen::Vector6d& cartesianState )
{
    using namespace orbital_element_conversions;

    TUDAT_UNUSED_PARAMETER( time );

    // Compute distance to primary body.
    const double xCoordinateToPrimaryBodySquared =
            ( cartesianState( xCartesianPositionIndex ) + massParameter )
            * ( cartesianState( xCartesianPositionIndex ) + massParameter );

    const double yCoordinateSquared = cartesianState( yCartesianPositionIndex )
            * cartesianState( yCartesianPositionIndex );

    const double zCoordinateSquared = cartesianState( zCartesianPositionIndex )
            * cartesianState( zCartesianPositionIndex );

    const double normDistanceToPrimaryBodyCubed = pow(
                xCoordinateToPrimaryBodySquared + yCoordinateSquared + zCoordinateSquared, 1.5 );

    // Compute distance to secondary body.
    const double xCoordinateSecondaryBodySquared =
            ( cartesianState( xCartesianPositionIndex ) - ( 1.0 - massParameter ) )
            * ( cartesianState( xCartesianPositionIndex ) - ( 1.0 - massParameter ) );

    double normDistanceToSecondaryBodyCubed = pow(
                xCoordinateSecondaryBodySquared + yCoordinateSquared + zCoordinateSquared, 1.5 );

    // Compute derivative of state.
    Eigen::VectorXd stateDerivative( 6 );

    stateDerivative.segment( xCartesianPositionIndex, 3 ) = cartesianState.segment( xCartesianVelocityIndex, 3 );

    stateDerivative( 3 ) = cartesianState( xCartesianPositionIndex )
            - ( ( 1.0 - massParameter ) / normDistanceToPrimaryBodyCubed )
            * ( cartesianState( xCartesianPositionIndex ) + massParameter )
            - ( massParameter / normDistanceToSecondaryBodyCubed )
            * ( cartesianState( xCartesianPositionIndex ) - ( 1.0 - massParameter ) )
            + 2.0 * cartesianState( yCartesianVelocityIndex );
    stateDerivative( 4 ) = cartesianState( yCartesianPositionIndex )
            * ( 1.0 - ( ( 1.0 - massParameter ) / normDistanceToPrimaryBodyCubed )
                - ( massParameter / normDistanceToSecondaryBodyCubed ) )
            - 2.0 * cartesianState( xCartesianVelocityIndex );
    stateDerivative( 5 ) = -cartesianState( zCartesianPositionIndex )
            * ( ( ( 1.0 - massParameter ) / normDistanceToPrimaryBodyCubed )
                + ( massParameter / normDistanceToSecondaryBodyCubed ) );

    // Return computed state derivative.
    return stateDerivative;
}
//! Function for updating common blocks of partial to current state.
void EmpiricalAccelerationPartial::update( const double currentTime )
{
    if( !( currentTime_ == currentTime ) )
    {

        using namespace tudat::basic_mathematics;
        using namespace tudat::linear_algebra;

        // Get current state and associated data.
        Eigen::Vector6d currentState = empiricalAcceleration_->getCurrentState( );
        Eigen::Vector3d angularMomentumVector = Eigen::Vector3d( currentState.segment( 0, 3 ) ).cross(
                    Eigen::Vector3d( currentState.segment( 3, 3 ) ) );
        Eigen::Vector3d crossVector = angularMomentumVector.cross( Eigen::Vector3d( currentState.segment( 0, 3 ) ) );

        // Compute constituent geometric partials.
        Eigen::Matrix3d normPositionWrtPosition = calculatePartialOfNormalizedVector( Eigen::Matrix3d::Identity( ), currentState.segment( 0, 3 ) );
        Eigen::Matrix3d angularMomentumWrtPosition = -getCrossProductMatrix( currentState.segment( 3, 3 ) );
        Eigen::Matrix3d angularMomentumWrtVelocity = getCrossProductMatrix( currentState.segment( 0, 3 ) );
        Eigen::Matrix3d normAngularMomentumWrtPosition = calculatePartialOfNormalizedVector( angularMomentumWrtPosition, angularMomentumVector );
        Eigen::Matrix3d normAngularMomentumWrtVelocity = calculatePartialOfNormalizedVector( angularMomentumWrtVelocity, angularMomentumVector );
        Eigen::Matrix3d crossVectorWrtPosition = getCrossProductMatrix( angularMomentumVector ) -
                angularMomentumWrtVelocity * angularMomentumWrtPosition;
        Eigen::Matrix3d crossVectorWrtVelocity = -getCrossProductMatrix( currentState.segment( 0, 3 ) ) *
                getCrossProductMatrix( currentState.segment( 0, 3 ) );
        Eigen::Matrix3d normCrossVectorWrtPosition = calculatePartialOfNormalizedVector( crossVectorWrtPosition, crossVector );
        Eigen::Matrix3d normCrossVectorWrtVelocity = calculatePartialOfNormalizedVector( crossVectorWrtVelocity, crossVector );

        // Retrieve current empirical acceleration
        Eigen::Vector3d localAcceleration = empiricalAcceleration_->getCurrentLocalAcceleration( );

        // Compute partial derivative contribution of derivative of rotation matrix from RSW to inertial frame
        currentPositionPartial_ = localAcceleration.x( ) * normPositionWrtPosition + localAcceleration.y( ) * normCrossVectorWrtPosition +
                localAcceleration.z( ) * normAngularMomentumWrtPosition;
        currentVelocityPartial_ = localAcceleration.y( ) * normCrossVectorWrtVelocity + localAcceleration.z( ) * normAngularMomentumWrtVelocity;

        // Compute partial derivative contribution of derivative of true anomaly
        Eigen::Matrix< double, 1, 6 > localTrueAnomalyPartial = calculateNumericalPartialOfTrueAnomalyWrtState(
                    empiricalAcceleration_->getCurrentState( ),
                    empiricalAcceleration_->getCurrentGravitationalParameter( ), cartesianStateElementPerturbations );
        Eigen::Matrix< double, 1, 6 > trueAnomalyPartial = localTrueAnomalyPartial;
        currentPositionPartial_ += empiricalAcceleration_->getCurrentToInertialFrame( ) * (
                    empiricalAcceleration_->getCurrentAccelerationComponent( basic_astrodynamics::sine_empirical ) *
                    std::cos( empiricalAcceleration_->getCurrentTrueAnomaly( ) ) -
                    empiricalAcceleration_->getCurrentAccelerationComponent( basic_astrodynamics::cosine_empirical ) *
                    std::sin( empiricalAcceleration_->getCurrentTrueAnomaly( ) )
                    ) * trueAnomalyPartial.block( 0, 0, 1, 3 );
        currentVelocityPartial_ +=
                ( empiricalAcceleration_->getCurrentToInertialFrame( ) * (
                      empiricalAcceleration_->getCurrentAccelerationComponent( basic_astrodynamics::sine_empirical ) *
                      std::cos( empiricalAcceleration_->getCurrentTrueAnomaly( ) ) -
                      empiricalAcceleration_->getCurrentAccelerationComponent( basic_astrodynamics::cosine_empirical ) *
                      std::sin( empiricalAcceleration_->getCurrentTrueAnomaly( ) ) ) ) * trueAnomalyPartial.block( 0, 3, 1, 3 );
        currentTime_ = currentTime;

        // Check output.
        if( currentPositionPartial_ != currentPositionPartial_ )
        {
            throw std::runtime_error( "Error, empirical acceleration position partials are NaN" );
        }
        if( currentVelocityPartial_ != currentVelocityPartial_ )
        {
            throw std::runtime_error( "Error, empirical acceleration velocity partials are NaN" );
        }
    }
}
//! Execute propagation of orbit of TeslaRoadster around the Earth.
int main( )
{
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////            USING STATEMENTS              //////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    using namespace tudat;
    using namespace tudat::simulation_setup;
    using namespace tudat::propagators;
    using namespace tudat::numerical_integrators;
    using namespace tudat::orbital_element_conversions;
    using namespace tudat::basic_mathematics;
    using namespace tudat::gravitation;
    using namespace tudat::numerical_integrators;


    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////     CREATE ENVIRONMENT AND VEHICLE       //////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


    // Load Spice kernels.
    spice_interface::loadStandardSpiceKernels( { input_output::getSpiceKernelPath( ) + "de430.bsp" } );

    // Set simulation time settings.
    const double simulationStartEpoch =
            ( 2458163.500000000 - basic_astrodynamics::JULIAN_DAY_ON_J2000 ) * 86400.0;
    const double simulationEndEpoch = 500.0 * physical_constants::JULIAN_YEAR;

    // Define body settings for simulation.
    std::vector< std::string > bodiesToCreate;
    bodiesToCreate.push_back( "Sun" );
    bodiesToCreate.push_back( "Earth" );
    bodiesToCreate.push_back( "Moon" );
    bodiesToCreate.push_back( "Mars" );
    bodiesToCreate.push_back( "Venus" );
    bodiesToCreate.push_back( "Jupiter" );
    bodiesToCreate.push_back( "Saturn" );
    bodiesToCreate.push_back( "Mercury" );

    // Create body objects.
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings =
            getDefaultBodySettings( bodiesToCreate );
//    bodySettings[ "Sun" ]->ephemerisSettings->resetFrameOrientation( "J2000" );
//    bodySettings[ "Moon" ]->ephemerisSettings->resetFrameOrientation( "J2000" );
//    bodySettings[ "Earth" ]->ephemerisSettings->resetFrameOrientation( "J2000" );

//    for( unsigned int i = 0; i < bodiesToCreate.size( ); i++ )
//    {
//        bodySettings[ bodiesToCreate.at( i ) ]->rotationModelSettings->resetOriginalFrame( "J2000" );
//    }

    bodySettings[ "Mars" ]->ephemerisSettings = std::make_shared< ApproximatePlanetPositionSettings >(
                ephemerides::ApproximatePlanetPositionsBase::mars, false );
    bodySettings[ "Jupiter" ]->ephemerisSettings = std::make_shared< ApproximatePlanetPositionSettings >(
                ephemerides::ApproximatePlanetPositionsBase::mars, false );
    bodySettings[ "Saturn" ]->ephemerisSettings = std::make_shared< ApproximatePlanetPositionSettings >(
                ephemerides::ApproximatePlanetPositionsBase::mars, false );
    bodySettings[ "Venus" ]->ephemerisSettings = std::make_shared< ApproximatePlanetPositionSettings >(
                ephemerides::ApproximatePlanetPositionsBase::mars, false );
    bodySettings[ "Mercury" ]->ephemerisSettings = std::make_shared< ApproximatePlanetPositionSettings >(
                ephemerides::ApproximatePlanetPositionsBase::mars, false );

    NamedBodyMap bodyMap = createBodies( bodySettings );

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////             CREATE VEHICLE            /////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Create spacecraft object.
    bodyMap[ "TeslaRoadster" ] = std::make_shared< simulation_setup::Body >( );

    bodyMap[ "TeslaRoadster" ]->setConstantBodyMass( 1500.0 );

    // Create radiation pressure settings
    double referenceAreaRadiation = 8.0;
    double radiationPressureCoefficient = 1.2;
    std::vector< std::string > occultingBodies;
    std::shared_ptr< RadiationPressureInterfaceSettings > teslaRoadsterRadiationPressureSettings =
            std::make_shared< CannonBallRadiationPressureInterfaceSettings >(
                "Sun", referenceAreaRadiation, radiationPressureCoefficient, occultingBodies );

    // Create and set radiation pressure settings
    bodyMap[ "TeslaRoadster" ]->setRadiationPressureInterface(
                "Sun", createRadiationPressureInterface(
                    teslaRoadsterRadiationPressureSettings, "TeslaRoadster", bodyMap ) );


    // Finalize body creation.
    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////            CREATE ACCELERATIONS          //////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Define propagator settings variables.
    SelectedAccelerationMap accelerationMap;
    std::vector< std::string > bodiesToPropagate;
    std::vector< std::string > centralBodies;

    // Define propagation settings.
    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfAsterix;
    accelerationsOfAsterix[ "Earth" ].push_back( std::make_shared< AccelerationSettings >(
                                                     basic_astrodynamics::central_gravity ) );
    accelerationsOfAsterix[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( 
                                                   basic_astrodynamics::central_gravity ) );
    accelerationsOfAsterix[ "Moon" ].push_back( std::make_shared< AccelerationSettings >(
                                                     basic_astrodynamics::central_gravity ) );
    accelerationsOfAsterix[ "Mars" ].push_back( std::make_shared< AccelerationSettings >(
                                                     basic_astrodynamics::central_gravity ) );
    accelerationsOfAsterix[ "Venus" ].push_back( std::make_shared< AccelerationSettings >(
                                                     basic_astrodynamics::central_gravity ) );
    accelerationsOfAsterix[ "Mercury" ].push_back( std::make_shared< AccelerationSettings >(
                                                     basic_astrodynamics::central_gravity ) );
    accelerationsOfAsterix[ "Saturn" ].push_back( std::make_shared< AccelerationSettings >(
                                                     basic_astrodynamics::central_gravity ) );
    accelerationsOfAsterix[ "Jupiter" ].push_back( std::make_shared< AccelerationSettings >(
                                                     basic_astrodynamics::central_gravity ) );
    accelerationsOfAsterix[ "Sun" ].push_back( std::make_shared< AccelerationSettings >(
                                                     basic_astrodynamics::cannon_ball_radiation_pressure ) );


    accelerationMap[  "TeslaRoadster" ] = accelerationsOfAsterix;
    bodiesToPropagate.push_back( "TeslaRoadster" );
    centralBodies.push_back( "Sun" );

    basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                bodyMap, accelerationMap, bodiesToPropagate, centralBodies );

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////             CREATE PROPAGATION SETTINGS            ////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Set Keplerian elements for TeslaRoadster.
    Eigen::Vector6d teslaRoadsterInitialStateJ2000;
    teslaRoadsterInitialStateJ2000 << -1.484544094935328E+06, -1.530028575781120E+06, -4.100899567817330E+05,
    -2.358201939417945E+00, -2.459124989364402E+00, -6.370785284397021E-01;
    teslaRoadsterInitialStateJ2000 *= 1000.0;
    teslaRoadsterInitialStateJ2000 += spice_interface::getBodyCartesianStateAtEpoch(
                "Earth", "SSB", "J2000", "None", simulationStartEpoch );

    Eigen::Quaterniond toEclipJ2000 = spice_interface::computeRotationQuaternionBetweenFrames(
                                 "J2000", "ECLIPJ2000", 0.0 );

    Eigen::Vector6d teslaRoadsterInitialState;
    teslaRoadsterInitialState.segment( 0, 3 ) = toEclipJ2000 * teslaRoadsterInitialStateJ2000.segment( 0, 3 );
    teslaRoadsterInitialState.segment( 3, 3 ) = toEclipJ2000 * teslaRoadsterInitialStateJ2000.segment( 3, 3 );
\




    // Define list of dependent variables to save.
    std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariablesList;
    dependentVariablesList.push_back(
                std::make_shared< SingleDependentVariableSaveSettings >(
                    relative_position_dependent_variable, "Earth", "Sun" ) );
    dependentVariablesList.push_back(
                std::make_shared< SingleDependentVariableSaveSettings >(
                    relative_position_dependent_variable, "Mars", "Sun" ) );
    dependentVariablesList.push_back(
                std::make_shared< SingleDependentVariableSaveSettings >(
                    relative_position_dependent_variable, "Venus", "Sun" ) );
    dependentVariablesList.push_back(
                std::make_shared< SingleDependentVariableSaveSettings >(
                    relative_distance_dependent_variable, "TeslaRoadster", "Earth" ) );

    // Create object with list of dependent variables
    std::shared_ptr< DependentVariableSaveSettings > dependentVariablesToSave =
            std::make_shared< DependentVariableSaveSettings >( dependentVariablesList );

    std::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings =
            std::make_shared< TranslationalStatePropagatorSettings< double > >
            ( centralBodies, accelerationModelMap, bodiesToPropagate, teslaRoadsterInitialState, simulationEndEpoch, cowell,
              dependentVariablesToSave );

    std::shared_ptr< IntegratorSettings< > > integratorSettings
            = std::make_shared< tudat::numerical_integrators::BulirschStoerIntegratorSettings< > >(
                simulationStartEpoch, 3600.0,
                numerical_integrators::bulirsch_stoer_sequence, 4,
                std::numeric_limits< double >::epsilon( ), std::numeric_limits< double >::infinity( ),
                1.0E-15, 1.0E-12 );

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////             PROPAGATE ORBIT            ////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


    // Create simulation object and propagate dynamics.
    SingleArcDynamicsSimulator< > dynamicsSimulator(
                bodyMap, integratorSettings, propagatorSettings, true, false, false );
    std::map< double, Eigen::VectorXd > integrationResult = dynamicsSimulator.getEquationsOfMotionNumericalSolution( );
    std::map< double, Eigen::VectorXd > dependentVariableResult = dynamicsSimulator.getDependentVariableHistory( );


    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////        PROVIDE OUTPUT TO CONSOLE AND FILES           //////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    std::string outputSubFolder = "SpacexTeslaExample/";

    // Write perturbed satellite propagation history to file.
    input_output::writeDataMapToTextFile( integrationResult,
                                          "spacexTeslaPropagationHistory.dat",
                                          tudat_applications::getOutputPath( ) + outputSubFolder,
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( dependentVariableResult,
                                          "spacexTeslaDependentVariableHistory.dat",
                                          tudat_applications::getOutputPath( ) + outputSubFolder,
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );


    // Final statement.
    // The exit code EXIT_SUCCESS indicates that the program was successfully executed.
    return EXIT_SUCCESS;
}