diff --git a/.gitignore b/.gitignore index f08f7250e6..15b5dfd717 100644 --- a/.gitignore +++ b/.gitignore @@ -40,6 +40,7 @@ install_manifest.txt *.exe *.out *.app +*.sh # # Build and doxygen related diff --git a/Tudat/Astrodynamics/Aerodynamics/CMakeLists.txt b/Tudat/Astrodynamics/Aerodynamics/CMakeLists.txt index 4e89b1ce51..3103d2e594 100644 --- a/Tudat/Astrodynamics/Aerodynamics/CMakeLists.txt +++ b/Tudat/Astrodynamics/Aerodynamics/CMakeLists.txt @@ -44,6 +44,7 @@ set(AERODYNAMICS_SOURCES "${SRCROOT}${AERODYNAMICSDIR}/tabulatedAtmosphere.cpp" "${SRCROOT}${AERODYNAMICSDIR}/customAerodynamicCoefficientInterface.cpp" "${SRCROOT}${AERODYNAMICSDIR}/flightConditions.cpp" + "${SRCROOT}${AERODYNAMICSDIR}/trimOrientation.cpp" ) # Set the header files. @@ -63,7 +64,9 @@ set(AERODYNAMICS_HEADERS "${SRCROOT}${AERODYNAMICSDIR}/standardAtmosphere.h" "${SRCROOT}${AERODYNAMICSDIR}/customAerodynamicCoefficientInterface.h" "${SRCROOT}${AERODYNAMICSDIR}/flightConditions.h" - "${SRCROOT}${AERODYNAMICSDIR}//UnitTests/testApolloCapsuleCoefficients.h" + "${SRCROOT}${AERODYNAMICSDIR}/UnitTests/testApolloCapsuleCoefficients.h" + "${SRCROOT}${AERODYNAMICSDIR}/flightConditions.cpp" + "${SRCROOT}${AERODYNAMICSDIR}/aerodynamicGuidance.h" ) if(USE_NRLMSISE00) @@ -82,7 +85,8 @@ setup_tudat_library_target(tudat_aerodynamics "${SRCROOT}{AERODYNAMICSDIR}") # Add unit tests. add_executable(test_AerodynamicMomentAndAerodynamicForce "${SRCROOT}${AERODYNAMICSDIR}/UnitTests/unitTestAerodynamicMomentAndAerodynamicForce.cpp") setup_custom_test_program(test_AerodynamicMomentAndAerodynamicForce "${SRCROOT}${AERODYNAMICSDIR}") -target_link_libraries(test_AerodynamicMomentAndAerodynamicForce tudat_simulation_setup tudat_aerodynamics tudat_reference_frames tudat_ephemerides tudat_basic_astrodynamics tudat_basic_mathematics ${Boost_LIBRARIES}) +target_link_libraries(test_AerodynamicMomentAndAerodynamicForce ${TUDAT_PROPAGATION_LIBRARIES} ${Boost_LIBRARIES}) + add_executable(test_AerodynamicsNamespace "${SRCROOT}${AERODYNAMICSDIR}/UnitTests/unitTestAerodynamicsNamespace.cpp") setup_custom_test_program(test_AerodynamicsNamespace "${SRCROOT}${AERODYNAMICSDIR}") diff --git a/Tudat/Astrodynamics/Aerodynamics/UnitTests/testApolloCapsuleCoefficients.h b/Tudat/Astrodynamics/Aerodynamics/UnitTests/testApolloCapsuleCoefficients.h index 2d7aef9202..e30b823235 100644 --- a/Tudat/Astrodynamics/Aerodynamics/UnitTests/testApolloCapsuleCoefficients.h +++ b/Tudat/Astrodynamics/Aerodynamics/UnitTests/testApolloCapsuleCoefficients.h @@ -12,7 +12,6 @@ namespace unit_tests using basic_mathematics::Vector6d; using mathematical_constants::PI; -using std::vector; using namespace aerodynamics; boost::shared_ptr< HypersonicLocalInclinationAnalysis > getApolloCoefficientInterface( ) @@ -23,9 +22,9 @@ boost::shared_ptr< HypersonicLocalInclinationAnalysis > getApolloCoefficientInte = boost::make_shared< geometric_shapes::Capsule >( 4.694, 1.956, 2.662, -1.0 * 33.0 * PI / 180.0, 0.196 ); - vector< int > numberOfLines; - vector< int > numberOfPoints; - vector< bool > invertOrders; + std::vector< int > numberOfLines; + std::vector< int > numberOfPoints; + std::vector< bool > invertOrders; numberOfLines.resize( 4 ); numberOfPoints.resize( 4 ); invertOrders.resize( 4 ); @@ -47,7 +46,7 @@ boost::shared_ptr< HypersonicLocalInclinationAnalysis > getApolloCoefficientInte Eigen::Vector3d momentReference; momentReference( 0 ) = -0.6624; momentReference( 1 ) = 0.0; - momentReference( 2 ) = -0.1369; + momentReference( 2 ) = 0.1369; std::vector< std::vector< double > > independentVariableDataPoints; independentVariableDataPoints.resize( 3 ); @@ -84,7 +83,7 @@ boost::shared_ptr< HypersonicLocalInclinationAnalysis > getApolloCoefficientInte 3.9116, momentReference ); } -} +} // namespace unit_tests -} +} // namespace tudat #endif // TUDAT_TESTAPOLLOCAPSULECOEFFICIENTS_H diff --git a/Tudat/Astrodynamics/Aerodynamics/UnitTests/unitTestAerodynamicMomentAndAerodynamicForce.cpp b/Tudat/Astrodynamics/Aerodynamics/UnitTests/unitTestAerodynamicMomentAndAerodynamicForce.cpp index f0486fde6c..9e01d83ade 100644 --- a/Tudat/Astrodynamics/Aerodynamics/UnitTests/unitTestAerodynamicMomentAndAerodynamicForce.cpp +++ b/Tudat/Astrodynamics/Aerodynamics/UnitTests/unitTestAerodynamicMomentAndAerodynamicForce.cpp @@ -66,7 +66,14 @@ #include "Tudat/Astrodynamics/Aerodynamics/customAerodynamicCoefficientInterface.h" #include "Tudat/Astrodynamics/Aerodynamics/aerodynamicAcceleration.h" #include "Tudat/Astrodynamics/Aerodynamics/aerodynamicRotationalAcceleration.h" -#include "Tudat/SimulationSetup/createFlightConditions.h" +#include "Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.h" +#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h" +#include +#include +#include +#include +#include +#include namespace tudat { @@ -317,6 +324,425 @@ BOOST_AUTO_TEST_CASE( testAerodynamicMomentAndRotationalAcceleration ) } } +class DummyAngleCalculator +{ +public: + + void update( const double time ) + { + time_ = time; + } + + double getDummyAngleOfAttack( ) + { + return 0.2 - time_ / 1000.0; + } + + double getDummyAngleOfSideslip( ) + { + return 0.6 + 0.5 * time_ / 1000.0; + } + + double getDummyBankAngle( ) + { + return 1.3 + 0.24 * time_ / 1000.0; + } + + double time_; +}; + +void testAerodynamicForceDirection( const bool includeThrustForce, + const bool imposeThrustDirection, + const bool swapCreationOrder ) +{ + using namespace tudat; + using namespace numerical_integrators; + using namespace simulation_setup; + using namespace basic_astrodynamics; + using namespace propagators; + using namespace basic_mathematics; + using namespace basic_astrodynamics; + + //Load spice kernels. + std::string kernelsPath = input_output::getSpiceKernelPath( ); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de-403-masses.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de421.bsp"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "naif0009.tls"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "pck00009.tpc"); + + double thrustMagnitude = 1.0E3; + double specificImpulse = 250.0; + + unsigned int maximumIndex = 8; + if( imposeThrustDirection ) + { + maximumIndex = 4; + } + for( unsigned int i = 4; i < maximumIndex; i++ ) + { + // Create Earth object + std::map< std::string, boost::shared_ptr< BodySettings > > defaultBodySettings = + getDefaultBodySettings( boost::assign::list_of( "Earth" ), -1.0E6, 1.0E6 ); + defaultBodySettings[ "Earth" ]->ephemerisSettings = boost::make_shared< ConstantEphemerisSettings >( + basic_mathematics::Vector6d::Zero( ) ); + NamedBodyMap bodyMap = createBodies( defaultBodySettings ); + + // Create vehicle objects. + double vehicleMass = 5.0E3; + bodyMap[ "Vehicle" ] = boost::make_shared< simulation_setup::Body >( ); + bodyMap[ "Vehicle" ]->setConstantBodyMass( vehicleMass ); + bodyMap[ "Vehicle" ]->setEphemeris( + boost::make_shared< ephemerides::TabulatedCartesianEphemeris< > >( + boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, basic_mathematics::Vector6d > >( ), + "Earth" ) ); + + if( i < 4 && !imposeThrustDirection ) + { + bodyMap[ "Vehicle" ]->setRotationalEphemeris( + boost::make_shared< ephemerides::SpiceRotationalEphemeris >( "ECLIPJ2000", "IAU_MOON" ) ); + } + + bool areCoefficientsInAerodynamicFrame; + if( ( i % 2 ) == 0 ) + { + areCoefficientsInAerodynamicFrame = 1; + } + else + { + areCoefficientsInAerodynamicFrame = 0; + } + + Eigen::Vector3d aerodynamicCoefficients; + if( ( i / 2 ) % 2 == 0 ) + { + aerodynamicCoefficients = Eigen::Vector3d::UnitX( ); + } + else + { + aerodynamicCoefficients = ( Eigen::Vector3d( )<<1.0, -0.1, 0.5 ).finished( ); + + } + + boost::shared_ptr< AerodynamicCoefficientSettings > aerodynamicCoefficientSettings = + boost::make_shared< ConstantAerodynamicCoefficientSettings >( + 2.0, 4.0, 1.5, Eigen::Vector3d::Zero( ), aerodynamicCoefficients, Eigen::Vector3d::Zero( ), + areCoefficientsInAerodynamicFrame, 1 ); + bodyMap[ "Vehicle" ]->setAerodynamicCoefficientInterface( + createAerodynamicCoefficientInterface( aerodynamicCoefficientSettings, "Vehicle" ) ); + Eigen::Vector3d aerodynamicCoefficientsDirection = aerodynamicCoefficients.normalized( ); + + + // Finalize body creation. + setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" ); + + + SelectedAccelerationMap accelerationMap; + std::vector< std::string > bodiesToPropagate; + std::vector< std::string > centralBodies; + + // Define acceleration model settings. + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle; + accelerationsOfVehicle[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + + if( swapCreationOrder ) + { + accelerationsOfVehicle[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( aerodynamic ) ); + } + + Eigen::Vector3d bodyFixedThrustDirection = ( Eigen::Vector3d( ) << 1.4, 3.1, -0.5 ).finished( ).normalized( ); + + if( includeThrustForce ) + { + if( !imposeThrustDirection ) + { + accelerationsOfVehicle[ "Vehicle" ].push_back( + boost::make_shared< ThrustAccelerationSettings >( + boost::make_shared< ThrustDirectionGuidanceSettings >( + thrust_direction_from_existing_body_orientation, "Earth" ), + boost::make_shared< ConstantThrustEngineSettings >( + thrustMagnitude, specificImpulse, bodyFixedThrustDirection ) ) ); + } + else + { + accelerationsOfVehicle[ "Vehicle" ].push_back( + boost::make_shared< ThrustAccelerationSettings >( + boost::make_shared< CustomThrustOrientationSettings >( + boost::bind( spice_interface::computeRotationQuaternionBetweenFrames, + "IAU_Mars", "IAU_Earth", _1 ) ), + boost::make_shared< ConstantThrustEngineSettings >( + thrustMagnitude, specificImpulse, bodyFixedThrustDirection ) ) ); + } + } + + if( !swapCreationOrder ) + { + accelerationsOfVehicle[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( aerodynamic ) ); + } + + + + accelerationMap[ "Vehicle" ] = accelerationsOfVehicle; + + bodiesToPropagate.push_back( "Vehicle" ); + centralBodies.push_back( "Earth" ); + + // Set initial state + basic_mathematics::Vector6d systemInitialState = basic_mathematics::Vector6d::Zero( ); + + systemInitialState( 0 ) = 6.8E6; + systemInitialState( 4 ) = 7.5E3; + + // Create acceleration models and propagation settings. + basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodyMap, accelerationMap, bodiesToPropagate, centralBodies ); + + boost::shared_ptr< DummyAngleCalculator > testAngles = + boost::make_shared< DummyAngleCalculator >( ); + + if( !( i < 4 ) ) + { + boost::shared_ptr< reference_frames::AerodynamicAngleCalculator > angleCalculator = + bodyMap[ "Vehicle" ]->getFlightConditions( )->getAerodynamicAngleCalculator( ); + angleCalculator->setOrientationAngleFunctions( + boost::bind( &DummyAngleCalculator::getDummyAngleOfAttack, testAngles ), + boost::bind( &DummyAngleCalculator::getDummyAngleOfSideslip, testAngles ), + boost::bind( &DummyAngleCalculator::getDummyBankAngle, testAngles ), + boost::bind( &DummyAngleCalculator::update, testAngles, _1 ) ); + } + + + boost::shared_ptr< DependentVariableSaveSettings > dependentVariableSaveSettings; + + std::vector< boost::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables; + + dependentVariables.push_back( + boost::make_shared< SingleAccelerationDependentVariableSaveSettings >( + aerodynamic, "Vehicle", "Earth", 0 ) ); + dependentVariables.push_back( + boost::make_shared< IntermediateAerodynamicRotationVariableSaveSettings >( + "Vehicle", reference_frames::inertial_frame, reference_frames::aerodynamic_frame ) ); + dependentVariables.push_back( + boost::make_shared< IntermediateAerodynamicRotationVariableSaveSettings >( + "Vehicle", reference_frames::inertial_frame, reference_frames::body_frame ) ); + dependentVariables.push_back( + boost::make_shared< IntermediateAerodynamicRotationVariableSaveSettings >( + "Vehicle", reference_frames::inertial_frame, reference_frames::corotating_frame ) ); + dependentVariables.push_back( + boost::make_shared< IntermediateAerodynamicRotationVariableSaveSettings >( + "Vehicle", reference_frames::inertial_frame, reference_frames::trajectory_frame ) ); + dependentVariables.push_back( + boost::make_shared< SingleDependentVariableSaveSettings >( + body_fixed_airspeed_based_velocity_variable, "Vehicle" ) ); + if( includeThrustForce ) + { + dependentVariables.push_back( + boost::make_shared< SingleAccelerationDependentVariableSaveSettings >( + thrust_acceleration, "Vehicle", "Vehicle", 0 ) ); + } + + dependentVariableSaveSettings = boost::make_shared< DependentVariableSaveSettings >( dependentVariables ); + + + boost::shared_ptr< PropagationTimeTerminationSettings > terminationSettings = + boost::make_shared< propagators::PropagationTimeTerminationSettings >( 1000.0 ); + boost::shared_ptr< TranslationalStatePropagatorSettings< double > > translationalPropagatorSettings = + boost::make_shared< TranslationalStatePropagatorSettings< double > > + ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, terminationSettings, + cowell, dependentVariableSaveSettings ); + boost::shared_ptr< IntegratorSettings< > > integratorSettings = + boost::make_shared< IntegratorSettings< > > + ( rungeKutta4, 0.0, 5.0 ); + + // Create simulation object and propagate dynamics. + SingleArcDynamicsSimulator< > dynamicsSimulator( + bodyMap, integratorSettings, translationalPropagatorSettings, true, false, false ); + + // Retrieve numerical solutions for state and dependent variables + std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > > dependentVariableOutput = + dynamicsSimulator.getDependentVariableHistory( ); + + boost::shared_ptr< ephemerides::RotationalEphemeris > rotationalEphemeris = + bodyMap.at( "Vehicle" )->getRotationalEphemeris( ); + + double thrustAcceleration = thrustMagnitude / vehicleMass; + Eigen::Matrix3d matrixDifference; + for( std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > >::const_iterator outputIterator = + dependentVariableOutput.begin( ); outputIterator != dependentVariableOutput.end( ); outputIterator++ ) + { + + + // Retrieve dependent variables from output; + Eigen::Matrix3d rotationToAerodynamicFrame = + getMatrixFromVectorRotationRepresentation( + outputIterator->second.segment( 3, 9 ) ); + Eigen::Matrix3d rotationToBodyFrame = + getMatrixFromVectorRotationRepresentation( + outputIterator->second.segment( 12, 9 ) ); + Eigen::Matrix3d rotationToCorotatingFrame = getMatrixFromVectorRotationRepresentation( + outputIterator->second.segment( 21, 9 ) ); + Eigen::Matrix3d rotationToTrajectoryFrame = getMatrixFromVectorRotationRepresentation( + outputIterator->second.segment( 30, 9 ) ); + Eigen::Vector3d bodyFixedAirspeed = outputIterator->second.segment( 39, 3 ); + + // Velocity vector in aerodynamic and trajectory frames should have component in positivie x-direction only. + Eigen::Vector3d bodyFixedAirspeedInAerodynamicFrame = rotationToAerodynamicFrame * + rotationToCorotatingFrame.transpose( ) * bodyFixedAirspeed; + Eigen::Vector3d bodyFixedAirspeedInTrajectoryFrame = rotationToTrajectoryFrame * + rotationToCorotatingFrame.transpose( ) * bodyFixedAirspeed; + + // Check velocity in aerodynamic frame + BOOST_CHECK_CLOSE_FRACTION( bodyFixedAirspeedInAerodynamicFrame.x( ), + bodyFixedAirspeedInAerodynamicFrame.norm( ), + std::numeric_limits< double >::epsilon( ) ); + + // Check velocity in trajectory frame + BOOST_CHECK_CLOSE_FRACTION( bodyFixedAirspeedInTrajectoryFrame.x( ), + bodyFixedAirspeedInTrajectoryFrame.norm( ), + std::numeric_limits< double >::epsilon( ) ); + + // For drag-only aerodynamics + if( ( i % 4 ) == 0 ) + { + Eigen::Vector3d aerodynamicForceInAerodynamicFrame = rotationToAerodynamicFrame * outputIterator->second.segment( 0, 3 ); + BOOST_CHECK_CLOSE_FRACTION( -aerodynamicForceInAerodynamicFrame.x( ), + aerodynamicForceInAerodynamicFrame.norm( ), + std::numeric_limits< double >::epsilon( ) ); + } + + // For C_{x}-only aerodynamics + else if( ( i % 4 ) == 1 ) + { + Eigen::Vector3d aerodynamicForceInBodyFrame = rotationToBodyFrame * outputIterator->second.segment( 0, 3 ); + BOOST_CHECK_CLOSE_FRACTION( -aerodynamicForceInBodyFrame.x( ), + aerodynamicForceInBodyFrame.norm( ), + std::numeric_limits< double >::epsilon( ) ); + } + + // Check that aerodynamic force is in correct direction (in aerodynamic frame). + else if( ( i % 4 ) == 2 ) + { + Eigen::Vector3d aerodynamicForceDirectionInAerodynamicFrame = + ( rotationToAerodynamicFrame * outputIterator->second.segment( 0, 3 ) ).normalized( ); + for( unsigned int j = 0; j < 3; j++ ) + { + BOOST_CHECK_CLOSE_FRACTION( -aerodynamicForceDirectionInAerodynamicFrame( j ), + aerodynamicCoefficientsDirection( j ), 5.0E-14 ); + } + } + + // Check that aerodynamic force is in correct direction (in body frame). + else if( ( i % 4 ) == 3 ) + { + Eigen::Vector3d aerodynamicForceDirectionInBodyFrame = + ( rotationToBodyFrame * outputIterator->second.segment( 0, 3 ) ).normalized( ); + for( unsigned int j = 0; j < 3; j++ ) + { + BOOST_CHECK_CLOSE_FRACTION( -aerodynamicForceDirectionInBodyFrame( j ), + aerodynamicCoefficientsDirection( j ), 5.0E-14 ); + } + } + + // Check if thrust force is in correct direction. + if( includeThrustForce && !imposeThrustDirection ) + { + Eigen::Vector3d thrustForceInBodyFrame = rotationToBodyFrame * outputIterator->second.segment( 42, 3 ); + + for( unsigned int j = 0; j < 3; j++ ) + { + BOOST_CHECK_SMALL( + std::fabs( thrustForceInBodyFrame( j ) - bodyFixedThrustDirection( j ) * + thrustAcceleration ), 1.0E-14 ); + } + } + else if( includeThrustForce && imposeThrustDirection ) + { + Eigen::Vector3d thrustForceInPropagationFrame = ( outputIterator->second.segment( 42, 3 ) ); + + Eigen::Matrix3d rotationToBodyFrameFromEphemeris = spice_interface::computeRotationQuaternionBetweenFrames( + "IAU_Earth", "IAU_Mars", outputIterator->first ).toRotationMatrix( ); + Eigen::Vector3d imposedThrustForceInPropagationFrame = + thrustAcceleration * ( rotationToBodyFrameFromEphemeris.transpose( ) * bodyFixedThrustDirection ); + + + + for( unsigned int j = 0; j < 3; j++ ) + { + BOOST_CHECK_SMALL( + std::fabs( thrustForceInPropagationFrame( j ) - imposedThrustForceInPropagationFrame( j ) ), + 1.0E-15 ); + } + matrixDifference = rotationToBodyFrameFromEphemeris - rotationToBodyFrame; + + for( unsigned int j = 0; j < 3; j++ ) + { + for( unsigned int k = 0; k < 3; k++ ) + { + BOOST_CHECK_SMALL( std::fabs( matrixDifference( j, k ) ), 5.0E-14 ); + } + } + + } + + if( i < 4 && !imposeThrustDirection ) + { + // Check if imposed and indirectly obtained rotation matrices are equal. + Eigen::Matrix3d rotationToBodyFrameFromEphemeris = rotationalEphemeris->getRotationToTargetFrame( + outputIterator->first, basic_astrodynamics::JULIAN_DAY_ON_J2000 ).toRotationMatrix( ); + matrixDifference = rotationToBodyFrameFromEphemeris - rotationToBodyFrame; + for( unsigned int j = 0; j < 3; j++ ) + { + for( unsigned int k = 0; k < 3; k++ ) + { + BOOST_CHECK_SMALL( std::fabs( matrixDifference( j, k ) ), 1.0E-13 ); + } + } + } + else if( !( i < 4 ) ) + { + testAngles->update( outputIterator->first ); + + Eigen::Matrix3d aerodynamicToBodyFrame = rotationToBodyFrame * rotationToAerodynamicFrame.inverse( ); + Eigen::Matrix3d aerodynamicToTrajectoryFrame = rotationToTrajectoryFrame * rotationToAerodynamicFrame.inverse( ); + + Eigen::Matrix3d manualAerodynamicToBodyFrame = + reference_frames::getAirspeedBasedAerodynamicToBodyFrameTransformationMatrix( + testAngles->getDummyAngleOfAttack( ), testAngles->getDummyAngleOfSideslip( ) ); + Eigen::Matrix3d manualAerodynamicToTrajectoryFrame = + reference_frames::getAerodynamicToTrajectoryFrameTransformationMatrix( + testAngles->getDummyBankAngle( ) ); + + matrixDifference = aerodynamicToBodyFrame - manualAerodynamicToBodyFrame; + + for( unsigned int j = 0; j < 3; j++ ) + { + for( unsigned int k = 0; k < 3; k++ ) + { + BOOST_CHECK_SMALL( std::fabs( matrixDifference( j, k ) ), 1.0E-14 ); + } + } + + matrixDifference = aerodynamicToTrajectoryFrame - manualAerodynamicToTrajectoryFrame; + for( unsigned int j = 0; j < 3; j++ ) + { + for( unsigned int k = 0; k < 3; k++ ) + { + BOOST_CHECK_SMALL( std::fabs( matrixDifference( j, k ) ), 1.0E-14 ); + } + } + } + } + } +} + +BOOST_AUTO_TEST_CASE( testAerodynamicForceDirectionInPropagation ) +{ + //testAerodynamicForceDirection( 0, 0, 0 ); + testAerodynamicForceDirection( 1, 0, 0 ); + //testAerodynamicForceDirection( 1, 1, 0 ); + //testAerodynamicForceDirection( 1, 0, 1 ); + //testAerodynamicForceDirection( 1, 1, 1 ); +} + BOOST_AUTO_TEST_SUITE_END( ) } diff --git a/Tudat/Astrodynamics/Aerodynamics/UnitTests/unitTestCoefficientGenerator.cpp b/Tudat/Astrodynamics/Aerodynamics/UnitTests/unitTestCoefficientGenerator.cpp index df4baf26a4..7dc826e048 100644 --- a/Tudat/Astrodynamics/Aerodynamics/UnitTests/unitTestCoefficientGenerator.cpp +++ b/Tudat/Astrodynamics/Aerodynamics/UnitTests/unitTestCoefficientGenerator.cpp @@ -67,7 +67,6 @@ namespace unit_tests using basic_mathematics::Vector6d; using mathematical_constants::PI; -using std::vector; using namespace aerodynamics; @@ -92,9 +91,9 @@ BOOST_AUTO_TEST_CASE( testAerodynamicCoefficientGenerator ) = boost::make_shared< geometric_shapes::SphereSegment >( 1.0 ); // Set vehicle in analysis with 10,000 panels. - vector< int > numberOfLines( 1, 31 ); - vector< int > numberOfPoints( 1, 31 ); - vector< bool > invertOrder( 1, false ); + std::vector< int > numberOfLines( 1, 31 ); + std::vector< int > numberOfPoints( 1, 31 ); + std::vector< bool > invertOrder( 1, false ); // Create analysis object. std::vector< std::vector< double > > independentVariableDataPoints; @@ -283,9 +282,9 @@ boost::shared_ptr< HypersonicLocalInclinationAnalysis > getApolloCoefficientInte = boost::make_shared< geometric_shapes::Capsule >( 4.694, 1.956, 2.662, -1.0 * 33.0 * PI / 180.0, 0.196 ); - vector< int > numberOfLines( 4 ); - vector< int > numberOfPoints( 4 ); - vector< bool > invertOrders( 4 ); + std::vector< int > numberOfLines( 4 ); + std::vector< int > numberOfPoints( 4 ); + std::vector< bool > invertOrders( 4 ); // Set number of analysis points. numberOfLines[ 0 ] = 31; diff --git a/Tudat/Astrodynamics/Aerodynamics/UnitTests/unitTestNRLMSISE00Atmosphere.cpp b/Tudat/Astrodynamics/Aerodynamics/UnitTests/unitTestNRLMSISE00Atmosphere.cpp index 222c73248b..d9c792bc20 100644 --- a/Tudat/Astrodynamics/Aerodynamics/UnitTests/unitTestNRLMSISE00Atmosphere.cpp +++ b/Tudat/Astrodynamics/Aerodynamics/UnitTests/unitTestNRLMSISE00Atmosphere.cpp @@ -1069,7 +1069,7 @@ BOOST_AUTO_TEST_CASE( test_nrlmise_InputFunction_no_adjustment ) // NRLMSISE00Input gen_data(2030, 172, 29000.0, 16.0, 150.0, 150.0, 4.0); // DD-MM-YY = 21-06-2030 , Hrs-Min-Sec = 8-3-20 - double julianDate = tudat::basic_astrodynamics::convertCalendarDateToJulianDay(2030,6,21,8,3,20) ; + double julianDate = tudat::basic_astrodynamics::convertCalendarDateToJulianDay< double >( 2030, 6, 21, 8, 3, 20.0 ); double time = tudat::basic_astrodynamics::convertJulianDayToSecondsSinceEpoch( julianDate , tudat::basic_astrodynamics::JULIAN_DAY_ON_J2000) ; @@ -1114,7 +1114,7 @@ BOOST_AUTO_TEST_CASE( test_nrlmise_InputFunction_with_adjustment ) // NRLMSISE00Input gen_data(2030, 172, 29000.0, 16.0, 150.0, 150.0, 4.0); // DD-MM-YY = 21-06-2030 , Hrs-Min-Sec = 8-3-20 - double julianDate = tudat::basic_astrodynamics::convertCalendarDateToJulianDay(2030,6,21,8,3,20) ; + double julianDate = tudat::basic_astrodynamics::convertCalendarDateToJulianDay< double >( 2030, 6, 21, 8, 3, 20.0 ); double time = tudat::basic_astrodynamics::convertJulianDayToSecondsSinceEpoch( julianDate , tudat::basic_astrodynamics::JULIAN_DAY_ON_J2000) ; diff --git a/Tudat/Astrodynamics/Aerodynamics/aerodynamicCoefficientGenerator.h b/Tudat/Astrodynamics/Aerodynamics/aerodynamicCoefficientGenerator.h index d40a62137e..dcc91dc26d 100644 --- a/Tudat/Astrodynamics/Aerodynamics/aerodynamicCoefficientGenerator.h +++ b/Tudat/Astrodynamics/Aerodynamics/aerodynamicCoefficientGenerator.h @@ -206,9 +206,11 @@ class AerodynamicCoefficientGenerator: public AerodynamicCoefficientInterface // Check if the correct number of aerodynamic coefficients is provided. if( independentVariables.size( ) != numberOfIndependentVariables_ ) { - throw std::runtime_error( - "Error in AerodynamicCoefficientGenerator, number of " - "input variables is inconsistent " ); + std::string errorMessage = + "Error in AerodynamicCoefficientGenerator, number of input variables is inconsistent " + + boost::lexical_cast< std::string >( independentVariables.size( ) ) + ", " + + boost::lexical_cast< std::string >( numberOfIndependentVariables_ ); + throw std::runtime_error( errorMessage ); } // Update current coefficients. diff --git a/Tudat/Astrodynamics/Aerodynamics/aerodynamicGuidance.h b/Tudat/Astrodynamics/Aerodynamics/aerodynamicGuidance.h new file mode 100644 index 0000000000..6b0679f272 --- /dev/null +++ b/Tudat/Astrodynamics/Aerodynamics/aerodynamicGuidance.h @@ -0,0 +1,82 @@ +#ifndef TUDAT_ENTRYGUIDANCE_H +#define TUDAT_ENTRYGUIDANCE_H + +#include +#include + +#include "Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.h" + +namespace tudat +{ + +namespace aerodynamics +{ + +//! Base class in which the aerodynamic angles (angle of attack, sideslip and bank angle) are computed. A derived class +//! implementing a specific guidance law is to be implemented by the user. +class AerodynamicGuidance +{ +public: + + //! Constructor. + AerodynamicGuidance( ): + currentAngleOfAttack_( TUDAT_NAN ), currentAngleOfSideslip_( TUDAT_NAN ), currentBankAngle_( TUDAT_NAN ){ } + + //! Destructor + virtual ~AerodynamicGuidance( ){ } + + //! Pure virtual function to update the guidance to the current time and state + /*! + * Pure virtual function to update the guidance to the current time and state + * \param currentTime Time to which the guidance object is to be updated. + */ + virtual void updateGuidance( const double currentTime ) = 0; + + //! Function to retrieve the current angle of attack, as set by last call to updateGuidance function + /*! + * Function to retrieve the current angle of attack, as set by last call to updateGuidance function + * \return Current angle of attack, as set by last call to updateGuidance function + */ + double getCurrentAngleOfAttack( ) + { + return currentAngleOfAttack_; + } + + //! Function to retrieve the current angle of sideslip, as set by last call to updateGuidance function + /*! + * Function to retrieve the current angle of sideslip, as set by last call to updateGuidance function + * \return Current angle of sideslip, as set by last call to updateGuidance function + */ + double getCurrentAngleOfSideslip( ) + { + return currentAngleOfSideslip_; + } + + //! Function to retrieve the current bank angle, as set by last call to updateGuidance function + /*! + * Function to retrieve the current bank angle, as set by last call to updateGuidance function + * \return Current bank angle, as set by last call to updateGuidance function + */ + double getCurrentBankAngle( ) + { + return currentBankAngle_; + } + +protected: + + //! Current angle of attack, as set by last call to updateGuidance function + double currentAngleOfAttack_; + + //! Current angle of sideslip, as set by last call to updateGuidance function + double currentAngleOfSideslip_; + + //! Current bank angle, as set by last call to updateGuidance function + double currentBankAngle_; + +}; + +} // namespace aerodynamics + +} // namespace tudat + +#endif // TUDAT_ENTRYGUIDANCE_H diff --git a/Tudat/Astrodynamics/Aerodynamics/aerodynamics.cpp b/Tudat/Astrodynamics/Aerodynamics/aerodynamics.cpp index 14577e6a3d..1aea0af7e1 100644 --- a/Tudat/Astrodynamics/Aerodynamics/aerodynamics.cpp +++ b/Tudat/Astrodynamics/Aerodynamics/aerodynamics.cpp @@ -507,6 +507,12 @@ double computeSpeedOfSound( const double temperature, const double ratioOfSpecif return std::sqrt( temperature * ratioOfSpecificHeats * specificGasConstant ); } +//! Compute Mach number +double computeMachNumber( const double speed, const double speedOfSound ) +{ + return speed / speedOfSound; +} + //! Function to compute the mean free path of a particle. double computeMeanFreePath( const double weightedAverageCollisionDiameter, const double averageNumberDensity ) { diff --git a/Tudat/Astrodynamics/Aerodynamics/aerodynamics.h b/Tudat/Astrodynamics/Aerodynamics/aerodynamics.h index 65830305a1..a4c29a7c84 100644 --- a/Tudat/Astrodynamics/Aerodynamics/aerodynamics.h +++ b/Tudat/Astrodynamics/Aerodynamics/aerodynamics.h @@ -353,6 +353,15 @@ double computeShockDeflectionAngle( double shockAngle, double machNumber, double computeSpeedOfSound( const double temperature, const double ratioOfSpecificHeats, const double specificGasConstant ); +//! Compute Mach number +/*! + * Compute Mach number + * \param speed Airspeed of object for which Mach number is to be computed. + * \param speedOfSound Speed of sound for atmosphere position at which Mach number is to be computed. + * \return Mach number + */ +double computeMachNumber( const double speed, const double speedOfSound ); + //! Function to compute the mean free path of a particle. /*! * Function to compute the mean free path of a particle from e.g. (Chapman, S. & Cowling, T. The mathematical theory of diff --git a/Tudat/Astrodynamics/Aerodynamics/flightConditions.cpp b/Tudat/Astrodynamics/Aerodynamics/flightConditions.cpp index 473da57ecd..5438071bf4 100644 --- a/Tudat/Astrodynamics/Aerodynamics/flightConditions.cpp +++ b/Tudat/Astrodynamics/Aerodynamics/flightConditions.cpp @@ -8,8 +8,11 @@ * http://tudat.tudelft.nl/LICENSE. */ +#include #include +#include +#include "Tudat/Astrodynamics/Aerodynamics/aerodynamics.h" #include "Tudat/Astrodynamics/Aerodynamics/flightConditions.h" #include "Tudat/Astrodynamics/Aerodynamics/standardAtmosphere.h" #include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h" @@ -20,27 +23,25 @@ namespace tudat namespace aerodynamics { +//! Constructor, sets objects and functions from which relevant environment and state variables +//! are retrieved. FlightConditions::FlightConditions( const boost::shared_ptr< aerodynamics::AtmosphereModel > atmosphereModel, const boost::function< double( const Eigen::Vector3d ) > altitudeFunction, - const boost::function< basic_mathematics::Vector6d( ) > stateOfVehicle, - const boost::function< basic_mathematics::Vector6d( ) > stateOfCentralBody, - const boost::function< basic_mathematics::Vector6d( const basic_mathematics::Vector6d& ) > - transformationToCentralBodyFrame, const boost::shared_ptr< AerodynamicCoefficientInterface > aerodynamicCoefficientInterface, const boost::shared_ptr< reference_frames::AerodynamicAngleCalculator > aerodynamicAngleCalculator ): atmosphereModel_( atmosphereModel ), altitudeFunction_( altitudeFunction ), - stateOfVehicle_( stateOfVehicle ), - stateOfCentralBody_( stateOfCentralBody ), - transformationToCentralBodyFrame_( transformationToCentralBodyFrame ), aerodynamicCoefficientInterface_( aerodynamicCoefficientInterface ), aerodynamicAngleCalculator_( aerodynamicAngleCalculator ),currentAltitude_( TUDAT_NAN ), currentLatitude_( TUDAT_NAN ), currentLongitude_( TUDAT_NAN ), currentTime_( TUDAT_NAN ) { updateLatitudeAndLongitude_ = 0; + bodyCenteredPseudoBodyFixedStateFunction_ = boost::bind( + &reference_frames::AerodynamicAngleCalculator::getCurrentBodyFixedState, aerodynamicAngleCalculator_ ); + if( boost::dynamic_pointer_cast< aerodynamics::StandardAtmosphere >( atmosphereModel_ ) == NULL ) { @@ -73,43 +74,62 @@ void FlightConditions::setAerodynamicCoefficientsIndependentVariableFunction( } } - //! Function to update all flight conditions. void FlightConditions::updateConditions( const double currentTime ) { - currentTime_ = currentTime; + if( !( currentTime == currentTime_ ) ) + { + currentTime_ = currentTime; - // Calculate state of vehicle in global frame and corotating frame. - currentBodyCenteredState_ = stateOfVehicle_( ) - stateOfCentralBody_( ); - currentBodyCenteredPseudoBodyFixedState_ = transformationToCentralBodyFrame_( - currentBodyCenteredState_ ); + // Update aerodynamic angles (but not angles w.r.t. body-fixed frame). + if( aerodynamicAngleCalculator_!= NULL ) + { + aerodynamicAngleCalculator_->update( currentTime, false ); + } - // Calculate altitute and airspeed of vehicle. - currentAltitude_ = - altitudeFunction_( currentBodyCenteredPseudoBodyFixedState_.segment( 0, 3 ) ); - currentAirspeed_ = currentBodyCenteredPseudoBodyFixedState_.segment( 3, 3 ).norm( ); + // Calculate state of vehicle in global frame and corotating frame. + currentBodyCenteredPseudoBodyFixedState_ = bodyCenteredPseudoBodyFixedStateFunction_( ); - // Update aerodynamic/geometric angles. - if( aerodynamicAngleCalculator_!= NULL ) - { - aerodynamicAngleCalculator_->update( ); - } + // Calculate altitute and airspeed of vehicle. + currentAltitude_ = + altitudeFunction_( currentBodyCenteredPseudoBodyFixedState_.segment( 0, 3 ) ); + currentAirspeed_ = currentBodyCenteredPseudoBodyFixedState_.segment( 3, 3 ).norm( ); - // Update latitude and longitude (if required) - if( updateLatitudeAndLongitude_ ) - { - currentLatitude_ = aerodynamicAngleCalculator_->getAerodynamicAngle( - reference_frames::latitude_angle ); - currentLongitude_ = aerodynamicAngleCalculator_->getAerodynamicAngle( - reference_frames::longitude_angle ); + + + // Update latitude and longitude (if required) + if( updateLatitudeAndLongitude_ ) + { + currentLatitude_ = aerodynamicAngleCalculator_->getAerodynamicAngle( + reference_frames::latitude_angle ); + currentLongitude_ = aerodynamicAngleCalculator_->getAerodynamicAngle( + reference_frames::longitude_angle ); + } + + // Update density + currentDensity_ = atmosphereModel_->getDensity( currentAltitude_, currentLongitude_, + currentLatitude_, currentTime_ ); + + updateAerodynamicCoefficientInput( ); + + // Update angles from aerodynamic to body-fixed frame (if relevant). + if( aerodynamicAngleCalculator_!= NULL ) + { + aerodynamicAngleCalculator_->update( currentTime, true ); + updateAerodynamicCoefficientInput( ); + } + + // Update aerodynamic coefficients. + aerodynamicCoefficientInterface_->updateCurrentCoefficients( + aerodynamicCoefficientIndependentVariables_ ); } +} - // Update density - currentDensity_ = atmosphereModel_->getDensity( currentAltitude_, currentLongitude_, - currentLatitude_, currentTime_ ); +void FlightConditions::updateAerodynamicCoefficientInput( ) +{ + aerodynamicCoefficientIndependentVariables_.clear( ); // Calculate independent variables for aerodynamic coefficients. - std::vector< double > aerodynamicCoefficientIndependentVariables; for( unsigned int i = 0; i < aerodynamicCoefficientInterface_-> getNumberOfIndependentVariables( ); i++ ) { @@ -117,9 +137,10 @@ void FlightConditions::updateConditions( const double currentTime ) { //Calculate Mach number if needed. case mach_number_dependent: - aerodynamicCoefficientIndependentVariables.push_back( - currentAirspeed_ / atmosphereModel_->getSpeedOfSound( - currentAltitude_, currentLongitude_, currentLatitude_, currentTime_ ) ); + aerodynamicCoefficientIndependentVariables_.push_back( + aerodynamics::computeMachNumber( + currentAirspeed_, atmosphereModel_->getSpeedOfSound( + currentAltitude_, currentLongitude_, currentLatitude_, currentTime_ ) ) ); break; //Get angle of attack if needed. case angle_of_attack_dependent: @@ -128,7 +149,7 @@ void FlightConditions::updateConditions( const double currentTime ) { throw std::runtime_error( "Error, aerodynamic angle calculator is null, but require angle of attack" ); } - aerodynamicCoefficientIndependentVariables.push_back( + aerodynamicCoefficientIndependentVariables_.push_back( aerodynamicAngleCalculator_->getAerodynamicAngle( reference_frames::angle_of_attack ) ); break; @@ -138,11 +159,10 @@ void FlightConditions::updateConditions( const double currentTime ) { throw std::runtime_error( "Error, aerodynamic angle calculator is null, but require angle of sideslip" ); } - aerodynamicCoefficientIndependentVariables.push_back( + aerodynamicCoefficientIndependentVariables_.push_back( aerodynamicAngleCalculator_->getAerodynamicAngle( reference_frames::angle_of_sideslip ) ); break; - // Check if value is custom-defined. default: if( customCoefficientDependencies_.count( aerodynamicCoefficientInterface_->getIndependentVariableName( i ) ) == 0 ) @@ -153,20 +173,33 @@ void FlightConditions::updateConditions( const double currentTime ) } else { - aerodynamicCoefficientIndependentVariables.push_back( + aerodynamicCoefficientIndependentVariables_.push_back( customCoefficientDependencies_.at( aerodynamicCoefficientInterface_->getIndependentVariableName( i ) )( ) ); } } } - - // Update aerodynamic coefficients. - aerodynamicCoefficientInterface_->updateCurrentCoefficients( - aerodynamicCoefficientIndependentVariables ); - - } +//! Function to set the angle of attack to trimmed conditions. +boost::shared_ptr< TrimOrientationCalculator > setTrimmedConditions( + const boost::shared_ptr< FlightConditions > flightConditions ) +{ + // Create trim object. + boost::shared_ptr< TrimOrientationCalculator > trimOrientation = + boost::make_shared< TrimOrientationCalculator >( + flightConditions->getAerodynamicCoefficientInterface( ) ); + + // Create angle-of-attack function from trim object. + boost::function< std::vector< double >( ) > untrimmedIndependentVariablesFunction = + boost::bind( &FlightConditions::getAerodynamicCoefficientIndependentVariables, + flightConditions ); + flightConditions->getAerodynamicAngleCalculator( )->setOrientationAngleFunctions( + boost::bind( &TrimOrientationCalculator::findTrimAngleOfAttackFromFunction, trimOrientation, + untrimmedIndependentVariablesFunction ) ); + + return trimOrientation; +} } // namespace aerodynamics diff --git a/Tudat/Astrodynamics/Aerodynamics/flightConditions.h b/Tudat/Astrodynamics/Aerodynamics/flightConditions.h index e65859cb7f..46ef0a7f4d 100644 --- a/Tudat/Astrodynamics/Aerodynamics/flightConditions.h +++ b/Tudat/Astrodynamics/Aerodynamics/flightConditions.h @@ -14,7 +14,9 @@ #include #include +#include +#include "Tudat/Astrodynamics/Aerodynamics/trimOrientation.h" #include "Tudat/Astrodynamics/Aerodynamics/aerodynamicCoefficientInterface.h" #include "Tudat/Astrodynamics/Aerodynamics/atmosphereModel.h" #include "Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.h" @@ -47,24 +49,13 @@ class FlightConditions * \param atmosphereModel Atmosphere model of atmosphere through which vehicle is flying * \param altitudeFunction Function returning the altitude of the vehicle as a function of * its body-fixed position. - * \param stateOfVehicle Function returning the current state of the vehicle - * (in the global frame) - * \param stateOfCentralBody Function returning the current state of the central body - * (in the global frame) - * \param transformationToCentralBodyFrame Function transforming the inertial body-centered to - * the body-centered, body-fixed (co-rotating) frame. - * \param aerodynamicCoefficientInterface Object from which the aerodynamic coefficients - * are obtained. + * \param aerodynamicCoefficientInterface Class from which the aerodynamic (force and moment) + * coefficients are retrieved * \param aerodynamicAngleCalculator Object from which the aerodynamic/trajectory angles * of the vehicle are calculated. */ FlightConditions( const boost::shared_ptr< aerodynamics::AtmosphereModel > atmosphereModel, const boost::function< double( const Eigen::Vector3d ) > altitudeFunction, - const boost::function< basic_mathematics::Vector6d( ) > stateOfVehicle, - const boost::function< basic_mathematics::Vector6d( ) > stateOfCentralBody, - const boost::function< basic_mathematics::Vector6d( - const basic_mathematics::Vector6d& ) > - transformationToCentralBodyFrame, const boost::shared_ptr< AerodynamicCoefficientInterface > aerodynamicCoefficientInterface, const boost::shared_ptr< reference_frames::AerodynamicAngleCalculator > @@ -110,6 +101,17 @@ class FlightConditions return currentAirspeed_; } + //! Function to return speed of sound + /*! + * Function to return speed of from quantities computed by previous call of updateConditions. + * \return Current speed of sound. + */ + double getCurrentSpeedOfSound( ) + { + return atmosphereModel_->getSpeedOfSound( + currentAltitude_, currentLongitude_, currentLatitude_, currentTime_ ); + } + //! Function to return the current time of the FlightConditions /*! * Function to return the current time of the FlightConditions. @@ -139,6 +141,8 @@ class FlightConditions aerodynamicAngleCalculator ) { aerodynamicAngleCalculator_ = aerodynamicAngleCalculator; + bodyCenteredPseudoBodyFixedStateFunction_ = boost::bind( + &reference_frames::AerodynamicAngleCalculator::getCurrentBodyFixedState, aerodynamicAngleCalculator_ ); } //! Function to set custom dependency of aerodynamic coefficients @@ -161,9 +165,19 @@ class FlightConditions basic_mathematics::Vector6d getCurrentBodyCenteredBodyFixedState( ) { return currentBodyCenteredPseudoBodyFixedState_; + } + //! Function to return current central body-fixed velocity of vehicle. + /*! + * Function to return central body-fixed velocity of vehicle. + * \return Current central body-fixed velocity of vehicle. + */ + Eigen::Vector3d getCurrentAirspeedBasedVelocity( ) + { + return currentBodyCenteredPseudoBodyFixedState_.segment( 3, 3 ); } + //! Function to return aerodynamic angle calculator object /*! * Function to return aerodynamic angle calculator object @@ -175,9 +189,43 @@ class FlightConditions return aerodynamicAngleCalculator_; } + //! Function to return object from which the aerodynamic coefficients are obtained. + /*! + * Function to return object from which the aerodynamic coefficients are obtained. + * \return Object from which the aerodynamic coefficients are obtained. + */ + boost::shared_ptr< AerodynamicCoefficientInterface > getAerodynamicCoefficientInterface( ) + { + return aerodynamicCoefficientInterface_; + } + + //! Function to return list of independent variables of the aerodynamic coefficient interface + /*! + * Function to return list of independent variables of the aerodynamic coefficient interface + * \return List of independent variables of the aerodynamic coefficient interface + */ + std::vector< double > getAerodynamicCoefficientIndependentVariables( ) + { + return aerodynamicCoefficientIndependentVariables_; + } + + //! Function to reset the current time of the flight conditions. + /*! + * Function to reset the current time of the flight conditions. This function is typically sused to set the current time + * to NaN, indicating the need to recompute all quantities for the next time computation. + * \param currentTime + */ + void resetCurrentTime( const double currentTime = TUDAT_NAN ) + { + currentTime_ = currentTime; + aerodynamicAngleCalculator_->resetCurrentTime( currentTime_ ); + } private: + //! Function to update the independent variables of the aerodynamic coefficient interface + void updateAerodynamicCoefficientInput( ); + //! Name of central body (i.e. body with the atmosphere) std::string centralBody_; @@ -187,23 +235,13 @@ class FlightConditions //! Function returning the altitude of the vehicle as a function of its body-fixed position. const boost::function< double( const Eigen::Vector3d ) > altitudeFunction_; - //! Function returning the current state of the vehicle (in the global frame) - boost::function< basic_mathematics::Vector6d( ) > stateOfVehicle_; + //! Function to return the current state of the vehicle in a body-fixed frame. + boost::function< basic_mathematics::Vector6d( ) > bodyCenteredPseudoBodyFixedStateFunction_; - //! Function returning the current state of the central body (in the global frame) - boost::function< basic_mathematics::Vector6d( ) > stateOfCentralBody_; - - //! Function transforming the inertial body-centered to the body-centered, body-fixed - //! co-rotating) frame. - boost::function< basic_mathematics::Vector6d( const basic_mathematics::Vector6d& ) > - transformationToCentralBodyFrame_; //! Object from which the aerodynamic coefficients are obtained. boost::shared_ptr< AerodynamicCoefficientInterface > aerodynamicCoefficientInterface_; - //! List of custom functions for aerodynamic coefficient dependencies. - std::map< AerodynamicCoefficientsIndependentVariables, boost::function< double( ) > > customCoefficientDependencies_; - //! Object from which the aerodynamic/trajectory angles of the vehicle are calculated. boost::shared_ptr< reference_frames::AerodynamicAngleCalculator > aerodynamicAngleCalculator_; @@ -231,11 +269,19 @@ class FlightConditions //! Current time of propagation. double currentTime_; + //! List of custom functions for aerodynamic coefficient dependencies. + std::map< AerodynamicCoefficientsIndependentVariables, boost::function< double( ) > > customCoefficientDependencies_; + //! Boolean setting whether latitude and longitude are to be updated by updateConditions(). bool updateLatitudeAndLongitude_; + + //! Current list of independent variables of the aerodynamic coefficient interface + std::vector< double > aerodynamicCoefficientIndependentVariables_; + }; } // namespace aerodynamics } // namespace tudat + #endif // TUDAT_FLIGHTCONDITIONS_H diff --git a/Tudat/Astrodynamics/Aerodynamics/hypersonicLocalInclinationAnalysis.cpp b/Tudat/Astrodynamics/Aerodynamics/hypersonicLocalInclinationAnalysis.cpp index cc2d321f9e..b2096dae22 100644 --- a/Tudat/Astrodynamics/Aerodynamics/hypersonicLocalInclinationAnalysis.cpp +++ b/Tudat/Astrodynamics/Aerodynamics/hypersonicLocalInclinationAnalysis.cpp @@ -67,8 +67,6 @@ namespace tudat namespace aerodynamics { -using std::string; -using std::endl; using basic_mathematics::Vector6d; using mathematical_constants::PI; @@ -663,8 +661,9 @@ void HypersonicLocalInclinationAnalysis::updateExpansionPressures( const double else { - std::cerr << "Error, expansion local inclination method number "<< method << - " not recognized" << std::endl; + std::string errorMessage = "Error, expansion local inclination method number " + + boost::lexical_cast< std::string >( method ) + " not recognized"; + throw std::runtime_error( errorMessage ); } } diff --git a/Tudat/Astrodynamics/Aerodynamics/hypersonicLocalInclinationAnalysis.h b/Tudat/Astrodynamics/Aerodynamics/hypersonicLocalInclinationAnalysis.h index d6d62aa22e..faeb504d3e 100644 --- a/Tudat/Astrodynamics/Aerodynamics/hypersonicLocalInclinationAnalysis.h +++ b/Tudat/Astrodynamics/Aerodynamics/hypersonicLocalInclinationAnalysis.h @@ -52,7 +52,6 @@ #ifndef TUDAT_HYPERSONIC_LOCAL_INCLINATION_ANALYSIS_H #define TUDAT_HYPERSONIC_LOCAL_INCLINATION_ANALYSIS_H -#include #include #include #include diff --git a/Tudat/Astrodynamics/Aerodynamics/nrlmsise00Atmosphere.cpp b/Tudat/Astrodynamics/Aerodynamics/nrlmsise00Atmosphere.cpp index bc53337ce4..11bb0eb70c 100644 --- a/Tudat/Astrodynamics/Aerodynamics/nrlmsise00Atmosphere.cpp +++ b/Tudat/Astrodynamics/Aerodynamics/nrlmsise00Atmosphere.cpp @@ -1,36 +1,13 @@ -/* Copyright (c) 2010-2015, Delft University of Technology - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, are - * permitted provided that the following conditions are met: - * - Redistributions of source code must retain the above copyright notice, this list of - * conditions and the following disclaimer. - * - Redistributions in binary form must reproduce the above copyright notice, this list of - * conditions and the following disclaimer in the documentation and/or other materials - * provided with the distribution. - * - Neither the name of the Delft University of Technology nor the names of its contributors - * may be used to endorse or promote products derived from this software without specific - * prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS - * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE - * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING - * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED - * OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Changelog - * - * References - * - * Notes +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. */ -#include #include "Tudat/Astrodynamics/Aerodynamics/nrlmsise00Atmosphere.h" #include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h" diff --git a/Tudat/Astrodynamics/Aerodynamics/nrlmsise00Atmosphere.h b/Tudat/Astrodynamics/Aerodynamics/nrlmsise00Atmosphere.h index f20d22a336..b89c67db98 100644 --- a/Tudat/Astrodynamics/Aerodynamics/nrlmsise00Atmosphere.h +++ b/Tudat/Astrodynamics/Aerodynamics/nrlmsise00Atmosphere.h @@ -1,26 +1,11 @@ -/*! Copyright (c) 2010-2012 Delft University of Technology. - * - * This software is protected by national and international copyright. - * Any unauthorized use, reproduction or modification is unlawful and - * will be prosecuted. Commercial and non-private application of the - * software in any form is strictly prohibited unless otherwise granted - * by the authors. - * - * The code is provided without any warranty; without even the implied - * warranty of merchantibility or fitness for a particular purpose. - * - * Changelog - * YYMMDD Author Comment - * 110224 F.M. Engelen File created. - * 110324 J. Melman Added overloaded get functions. - * 110427 F.M. Engelen Changed input parameter to altitude, longitude and latitude. - * 110629 F.M. Engelen Added predefined function. - * 110705 F.M. Engelen Changed to passing by reference. - * 110810 J. Leloux Corrected doxygen documentation. - * 151104 J. Geul Complete rewrite - * - * References +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. */ #ifndef TUDAT_NRLMSISE00_ATMOSPHERE_H diff --git a/Tudat/Astrodynamics/Aerodynamics/nrlmsise00InputFunctions.cpp b/Tudat/Astrodynamics/Aerodynamics/nrlmsise00InputFunctions.cpp index 11358736eb..79def45abb 100644 --- a/Tudat/Astrodynamics/Aerodynamics/nrlmsise00InputFunctions.cpp +++ b/Tudat/Astrodynamics/Aerodynamics/nrlmsise00InputFunctions.cpp @@ -1,35 +1,14 @@ -/* Copyright (c) 2010-2015, Delft University of Technology - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, are - * permitted provided that the following conditions are met: - * - Redistributions of source code must retain the above copyright notice, this list of - * conditions and the following disclaimer. - * - Redistributions in binary form must reproduce the above copyright notice, this list of - * conditions and the following disclaimer in the documentation and/or other materials - * provided with the distribution. - * - Neither the name of the Delft University of Technology nor the names of its contributors - * may be used to endorse or promote products derived from this software without specific - * prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS - * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE - * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING - * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED - * OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Changelog - * - * References - * - * Notes +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. */ -#include + +#include #include "Tudat/Astrodynamics/Aerodynamics/nrlmsise00Atmosphere.h" #include "Tudat/Astrodynamics/Aerodynamics/nrlmsise00InputFunctions.h" @@ -74,7 +53,9 @@ NRLMSISE00Input nrlmsiseInputFunction( const double altitude, const double longi // Check if solar activity is found for current day. if( solarActivityMap.count( julianDay ) == 0 ) { - std::cerr << "Solar activity data could not be found for this date.." << std::endl; + std::string errorMessage = "Solar activity data could not be found for this julian date: " + + boost::lexical_cast< std::string >( julianDay ) + " in nrlmsiseInputFunction"; + throw std::runtime_error( errorMessage ); } SolarActivityDataPtr solarActivity = solarActivityMap.at( julianDay ); diff --git a/Tudat/Astrodynamics/Aerodynamics/nrlmsise00InputFunctions.h b/Tudat/Astrodynamics/Aerodynamics/nrlmsise00InputFunctions.h index 1e031ce02b..d271659bb4 100644 --- a/Tudat/Astrodynamics/Aerodynamics/nrlmsise00InputFunctions.h +++ b/Tudat/Astrodynamics/Aerodynamics/nrlmsise00InputFunctions.h @@ -1,26 +1,11 @@ -/*! Copyright (c) 2010-2012 Delft University of Technology. - * - * This software is protected by national and international copyright. - * Any unauthorized use, reproduction or modification is unlawful and - * will be prosecuted. Commercial and non-private application of the - * software in any form is strictly prohibited unless otherwise granted - * by the authors. - * - * The code is provided without any warranty; without even the implied - * warranty of merchantibility or fitness for a particular purpose. - * - * Changelog - * YYMMDD Author Comment - * 110224 F.M. Engelen File created. - * 110324 J. Melman Added overloaded get functions. - * 110427 F.M. Engelen Changed input parameter to altitude, longitude and latitude. - * 110629 F.M. Engelen Added predefined function. - * 110705 F.M. Engelen Changed to passing by reference. - * 110810 J. Leloux Corrected doxygen documentation. - * 151104 J. Geul Complete rewrite - * - * References +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. */ #ifndef TUDAT_NRLMSISE00_INPUT_FUNCTIONS_H diff --git a/Tudat/Astrodynamics/Aerodynamics/tabulatedAtmosphere.cpp b/Tudat/Astrodynamics/Aerodynamics/tabulatedAtmosphere.cpp index d5dbd4803d..9632b05fa6 100644 --- a/Tudat/Astrodynamics/Aerodynamics/tabulatedAtmosphere.cpp +++ b/Tudat/Astrodynamics/Aerodynamics/tabulatedAtmosphere.cpp @@ -60,8 +60,8 @@ void TabulatedAtmosphere::initialize( const std::string& atmosphereTableFile ) if ( containerOfAtmosphereTableFileData.rows( ) < 1 || containerOfAtmosphereTableFileData.cols( ) < 1 ) { - std::cerr << "The atmosphere table file is empty." << std::endl; - std::cerr << atmosphereTableFile_ << std::endl; + std::string errorMessage = "The atmosphere table file " + atmosphereTableFile_ + " is empty"; + throw std::runtime_error( errorMessage ); } // Initialize vectors. diff --git a/Tudat/Astrodynamics/Aerodynamics/trimOrientation.cpp b/Tudat/Astrodynamics/Aerodynamics/trimOrientation.cpp new file mode 100644 index 0000000000..ce7558b250 --- /dev/null +++ b/Tudat/Astrodynamics/Aerodynamics/trimOrientation.cpp @@ -0,0 +1,99 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include +#include +#include +#include + +namespace tudat +{ + +namespace aerodynamics +{ + +//! Constructor +TrimOrientationCalculator::TrimOrientationCalculator( + const boost::shared_ptr< AerodynamicCoefficientInterface > coefficientInterface, + const boost::shared_ptr< root_finders::RootFinderCore< double > > rootFinder ): + coefficientInterface_( coefficientInterface ), rootFinder_( rootFinder ) +{ + // Find index of angle of attack in aerodynamic coefficient interface (throw error if not found) + std::vector< AerodynamicCoefficientsIndependentVariables > independentVariables = + coefficientInterface->getIndependentVariableNames( ); + std::vector< AerodynamicCoefficientsIndependentVariables >::iterator variableIterator = + std::find( independentVariables.begin( ), independentVariables.end( ), angle_of_attack_dependent ); + + if( variableIterator == independentVariables.end( ) ) + { + throw std::runtime_error( "Error when getting trim angle of attack, no angle of attack dependency is found" ); + } + variableIndex_ = std::distance( independentVariables.begin( ), variableIterator ); + + // If no root finder provided, use default value. + if ( !rootFinder_.get( ) ) + { + rootFinder_ = boost::make_shared< root_finders::SecantRootFinderCore< double > >( + boost::bind( + &root_finders::termination_conditions::RootAbsoluteToleranceTerminationCondition< double >:: + checkTerminationCondition, + boost::make_shared< root_finders::termination_conditions::RootAbsoluteToleranceTerminationCondition + < double > >( 1.0E-15, 1000 ), _1, _2, _3, _4, _5 ), 0.5 ); + } +} + +//! Function to find the trimmed angle of attack for a given set of independent variables +double TrimOrientationCalculator::findTrimAngleOfAttack( + const std::vector< double > untrimmedIndependentVariables ) +{ + // Determine function for which the root is to be determined. + boost::function< double( const double ) > coefficientFunction = + boost::bind( &TrimOrientationCalculator::getPerturbedMomentCoefficient, + this, _1, untrimmedIndependentVariables ); + + double trimmedAngleOfAttack = TUDAT_NAN; + + // Find root of pitch moment function + try + { + trimmedAngleOfAttack = rootFinder_->execute( + boost::make_shared< basic_mathematics::FunctionProxy< double, double > >( coefficientFunction ), + untrimmedIndependentVariables.at( variableIndex_ ) ); + } + // Throw error if not converged + catch( std::runtime_error ) + { + throw std::runtime_error( "Error when getting trim angle of attack, root finder did not converge." ); + + } + + return trimmedAngleOfAttack; +} + + +//! Function to get the moment coefficient for a given angle of attack +double TrimOrientationCalculator::getPerturbedMomentCoefficient( + const double perturbedAngleOfAttack, + const std::vector< double >& unperturbedConditions ) +{ + // Update coefficients to perturbed independent variables + std::vector< double > perturbedConditions = unperturbedConditions; + perturbedConditions[ variableIndex_ ] = perturbedAngleOfAttack; + coefficientInterface_->updateCurrentCoefficients( perturbedConditions ); + + // Get pitch moment coefficient + return coefficientInterface_->getCurrentMomentCoefficients( )( 1 ); +} + +} + +} + + diff --git a/Tudat/Astrodynamics/Aerodynamics/trimOrientation.h b/Tudat/Astrodynamics/Aerodynamics/trimOrientation.h new file mode 100644 index 0000000000..1700e50c8f --- /dev/null +++ b/Tudat/Astrodynamics/Aerodynamics/trimOrientation.h @@ -0,0 +1,112 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + * + * NOTE: The code in this file, and the associated cpp file, is tested in the unitTestDependentVariableOutput.cpp file. + */ + +#ifndef TUDAT_TRIMORIENTATION_H +#define TUDAT_TRIMORIENTATION_H + +#include +#include + +#include + +#include +#include + +namespace tudat +{ + +namespace aerodynamics +{ + + + +//! Class to determine the trimmed angle-of-attack for a given set of aerodynamic coefficients. +/*! + * Class to determine the trimmed angle-of-attack for a given set of aerodynamic coefficients. The coefficient interface + * provided as input must be dependent on the angle of attack for this class to function. + */ +class TrimOrientationCalculator +{ +public: + + //! Constructor + /*! + * Constructor + * \param coefficientInterface Object containing used to retrieve aerodynamic coefficients as function of independent + * variables. + * \param rootFinder Object to iteratively find the root of the equations C_m(alpha)=0, i.e. to determine the + * angle of attack for which the pitch moment is zero. + */ + TrimOrientationCalculator( + const boost::shared_ptr< AerodynamicCoefficientInterface > coefficientInterface, + const boost::shared_ptr< root_finders::RootFinderCore< double > > rootFinder = NULL ); + + //! Function to find the trimmed angle of attack for a given set of independent variables + /*! + * Function to find the trimmed angle of attack for a given set of independent variables. This function iteratively + * changes the angle of attack in the untrimmedIndependentVariables vector (keeping any other variables the same) and + * returns the value of the trimmed angle of attack. Note that this function will typically have some small numerical + * error in the result, as a result of the error tolerances in the root finder. + * \param untrimmedIndependentVariables Untrimmed list of independent variables (in order required as input for + * coefficientInterface_ + * \return Trimmed angle of attack. + */ + double findTrimAngleOfAttack( + const std::vector< double > untrimmedIndependentVariables ); + + //! Function to find the trimmed angle of attack for a given set of independent variables + /*! + * Function to find the trimmed angle of attack for a given set of independent variables. This function iteratively + * changes the angle of attack in the untrimmedIndependentVariables vector (keeping any other variables the same) and + * returns the value of the trimmed angle of attack. Note that this function will typically have some small numerical + * error in the result, as a result of the error tolerances in the root finder. + * \param untrimmedIndependentVariablesFunction Function returning untrimmed list of independent variables + * (in order required as input for coefficientInterface_ + * \return Trimmed angle of attack. + */ + double findTrimAngleOfAttackFromFunction( + const boost::function< std::vector< double >( ) > untrimmedIndependentVariablesFunction ) + { + return findTrimAngleOfAttack( untrimmedIndependentVariablesFunction( ) ); + } + +private: + + //! Function to get the moment coefficient for a given angle of attack + /*! + * Function to get the moment coefficient for a given perturbed angle of attack, keeping all other independent variables + * constant. This function is used as input to the root finder to determine the trim point. + * \param perturbedAngleOfAttack Angle of attack to use + * \param unperturbedConditions Untrimmed list of independent variables (in order required as input for + * coefficientInterface_ + * \return Moment coefficient at given independent variables. + */ + double getPerturbedMomentCoefficient( + const double perturbedAngleOfAttack, + const std::vector< double >& unperturbedConditions ); + + //! Object containing used to retrieve aerodynamic coefficients as function of independent variables. + boost::shared_ptr< AerodynamicCoefficientInterface > coefficientInterface_; + + //! Object to iteratively find the root of the equations C_m(alpha)=0, i.e. to determine the + //! angle of attack for which the pitch moment is zero. + boost::shared_ptr< root_finders::RootFinderCore< double > > rootFinder_; + + //! Index in independent variable list of coefficientInterface_ corresponding to the angle of attack. + int variableIndex_; +}; + +} // namespace aerodynamics + +} // namespace tudat + +#endif // TUDAT_TRIMORIENTATION_H diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/CMakeLists.txt b/Tudat/Astrodynamics/BasicAstrodynamics/CMakeLists.txt index 8d90a1806e..a16bdef74c 100755 --- a/Tudat/Astrodynamics/BasicAstrodynamics/CMakeLists.txt +++ b/Tudat/Astrodynamics/BasicAstrodynamics/CMakeLists.txt @@ -77,8 +77,10 @@ set(BASICASTRODYNAMICS_HEADERS "${SRCROOT}${BASICASTRODYNAMICSDIR}/bodyShapeModel.h" "${SRCROOT}${BASICASTRODYNAMICSDIR}/oblateSpheroidBodyShapeModel.h" "${SRCROOT}${BASICASTRODYNAMICSDIR}/sphericalBodyShapeModel.h" + "${SRCROOT}${BASICASTRODYNAMICSDIR}/massRateModel.h" "${SRCROOT}${BASICASTRODYNAMICSDIR}/sphericalStateConversions.h" "${SRCROOT}${BASICASTRODYNAMICSDIR}/unifiedStateModelElementConversions.h" + "${SRCROOT}${BASICASTRODYNAMICSDIR}/stateRepresentationConversions.h" ) # Add static libraries. @@ -112,7 +114,7 @@ setup_custom_test_program(test_UnitConversions "${SRCROOT}${BASICASTRODYNAMICSDI target_link_libraries(test_UnitConversions tudat_basic_astrodynamics tudat_basic_mathematics ${Boost_LIBRARIES}) add_executable(test_MeanToEccentricAnomalyConversion "${SRCROOT}${BASICASTRODYNAMICSDIR}/UnitTests/unitTestConvertMeanAnomalyToEccentricAnomaly.cpp") -setup_custom_test_program(test_MeanToEccentricAnomalyConversion "${SRCROOT}${BASICASTRODYNAMICSDIR}") +setup_custom_test_program(test_MeanToEccentricAnomalyConversion "${SRCRthrustFunctionsOOT}${BASICASTRODYNAMICSDIR}") target_link_libraries(test_MeanToEccentricAnomalyConversion tudat_basic_astrodynamics tudat_basic_mathematics tudat_root_finders ${Boost_LIBRARIES}) add_executable(test_MeanToHyperbolicEccentricAnomalyConversion "${SRCROOT}${BASICASTRODYNAMICSDIR}/UnitTests/unitTestConvertMeanAnomalyToHyperbolicEccentricAnomaly.cpp") diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/UnitTests/testAccelerationModels.h b/Tudat/Astrodynamics/BasicAstrodynamics/UnitTests/testAccelerationModels.h index 8d3ea545e1..c367763540 100644 --- a/Tudat/Astrodynamics/BasicAstrodynamics/UnitTests/testAccelerationModels.h +++ b/Tudat/Astrodynamics/BasicAstrodynamics/UnitTests/testAccelerationModels.h @@ -266,7 +266,7 @@ class AnotherDerivedAccelerationModel TimeDataType time; }; -} // namespace tudat } // namespace unit_tests +} // namespace tudat #endif // TUDAT_TEST_ACCELERATION_MODELS_H diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/UnitTests/unitTestTimeConversions.cpp b/Tudat/Astrodynamics/BasicAstrodynamics/UnitTests/unitTestTimeConversions.cpp index 03b0d06fa1..d9c8960423 100644 --- a/Tudat/Astrodynamics/BasicAstrodynamics/UnitTests/unitTestTimeConversions.cpp +++ b/Tudat/Astrodynamics/BasicAstrodynamics/UnitTests/unitTestTimeConversions.cpp @@ -51,6 +51,7 @@ namespace unit_tests { using namespace basic_astrodynamics; +using namespace physical_constants; //! Test the functionality of the time conversion functions. BOOST_AUTO_TEST_SUITE( test_Time_Conversions ) @@ -98,29 +99,92 @@ BOOST_AUTO_TEST_CASE( testJulianDayToSecondsConversions ) BOOST_CHECK_CLOSE_FRACTION( computedSecondsSinceEpoch, expectedSecondsSinceEpoch, std::numeric_limits< double >::epsilon( ) ); } + + // Test conversion from Julian day to seconds since epoch at 0 MJD with long doubles. + { + // Set reference epoch and Julian day for tests. + const long double referenceEpoch = JULIAN_DAY_AT_0_MJD_LONG; + const long double julianDay = JULIAN_DAY_AT_0_MJD_LONG + 1.0e6L / 86400.0L; + + // Set expected seconds since epoch result. + const long double expectedSecondsSinceEpoch = 1.0e6L; + + // Compute seconds since epoch given Julian day and reference epoch. + const long double computedSecondsSinceEpoch = convertJulianDayToSecondsSinceEpoch< long double >( + julianDay, referenceEpoch ); + + // Test that computed result matches expected result. + // Test is run at reduced tolerance, because the final digits of the seconds were lost + // when converting to Julian day. + BOOST_CHECK_CLOSE_FRACTION( computedSecondsSinceEpoch, expectedSecondsSinceEpoch, + 1.0e-14 ); + } + + // Test conversion from Julian day to seconds since J2000 epoch with long doubles + { + // Set reference epoch and Julian day for tests. + const long double referenceEpoch = JULIAN_DAY_ON_J2000_LONG; + const long double julianDay = JULIAN_DAY_ON_J2000_LONG + 0.5L; + + // Set expected seconds since epoch result. + long double expectedSecondsSinceEpoch + = physical_constants::JULIAN_DAY_LONG / 2.0L; + + // Compute seconds since epoch given Julian day and reference epoch. + const long double computedSecondsSinceEpoch = convertJulianDayToSecondsSinceEpoch< long double >( + julianDay, referenceEpoch ); + + // Test that computed result matches expected result. + // Test is run at reduced tolerance, because the final digits of the seconds were lost + // when converting to Julian day. + BOOST_CHECK_CLOSE_FRACTION( computedSecondsSinceEpoch, expectedSecondsSinceEpoch, + std::numeric_limits< long double >::epsilon( ) ); + } } //! Unit test for seconds to Julian day conversion function. BOOST_AUTO_TEST_CASE( testSecondsSinceEpochToJulianDayConversions ) { - // Test conversion from seconds since epoch to Julian day. + { + // Test conversion from seconds since epoch to Julian day. + + // Set reference epoch and seconds since epoch for tests. + const double referenceEpoch = JULIAN_DAY_AT_0_MJD; + const double secondsSinceEpoch = 1.0e6; - // Set reference epoch and seconds since epoch for tests. - const double referenceEpoch = JULIAN_DAY_AT_0_MJD; - const double secondsSinceEpoch = 1.0e6; + // Set expected Julian day result. + const double expectedJulianDay = JULIAN_DAY_AT_0_MJD + 1.0e6 / 86400.0; - // Set expected Julian day result. - const double expectedJulianDay = JULIAN_DAY_AT_0_MJD + 1.0e6 / 86400.0; + // Compute Julian day with seconds since reference epoch specified. + const double computedJulianDay = convertSecondsSinceEpochToJulianDay( + secondsSinceEpoch, referenceEpoch ); - // Compute Julian day with seconds since reference epoch specified. - const double computedJulianDay = convertSecondsSinceEpochToJulianDay( - secondsSinceEpoch, referenceEpoch ); + // Test that computed result matches expected result. + BOOST_CHECK_CLOSE_FRACTION( computedJulianDay, expectedJulianDay, + std::numeric_limits< double >::epsilon( ) ); + } - // Test that computed result matches expected result. - BOOST_CHECK_CLOSE_FRACTION( computedJulianDay, expectedJulianDay, - std::numeric_limits< double >::epsilon( ) ); + { + // Test conversion from seconds since epoch to Julian day with long doubles. + + // Set reference epoch and seconds since epoch for tests. + const long double referenceEpoch = JULIAN_DAY_AT_0_MJD_LONG; + const long double secondsSinceEpoch = 1.0e6L; + + // Set expected Julian day result. + const long double expectedJulianDay = JULIAN_DAY_AT_0_MJD_LONG + 1.0e6L / 86400.0L; + + // Compute Julian day with seconds since reference epoch specified. + const long double computedJulianDay = convertSecondsSinceEpochToJulianDay< long double >( + secondsSinceEpoch, referenceEpoch ); + + // Test that computed result matches expected result. + BOOST_CHECK_CLOSE_FRACTION( computedJulianDay, expectedJulianDay, + std::numeric_limits< long double >::epsilon( ) ); + } } + //! Unit test for calendar date to Julian day conversion function. BOOST_AUTO_TEST_CASE( testConversionCalendarDateToJulianDay ) { @@ -134,20 +198,23 @@ BOOST_AUTO_TEST_CASE( testConversionCalendarDateToJulianDay ) // Test that computed result matches expected result. BOOST_CHECK_CLOSE_FRACTION( computedJulianDay, expectedJulianDay, - std::numeric_limits< double >::epsilon( ) ); + std::numeric_limits< double >::epsilon( ) ); } //Compute the Julian day of the calendar date: November 17th, 1858, At 0h0m0s. { + //Use the function to compute the Julian day. - const double computedJulianDay = convertCalendarDateToJulianDay( 1858, 11, 17, 0, 0, 0 ); + const double computedJulianDay = convertCalendarDateToJulianDay( + 1858, 11, 17, 0, 0, 0.0 ); //Known Julian day at this calendar date const double expectedJulianDay = basic_astrodynamics::JULIAN_DAY_AT_0_MJD; // Test that computed result matches expected result. BOOST_CHECK_CLOSE_FRACTION( computedJulianDay, expectedJulianDay, - std::numeric_limits< double >::epsilon( ) ); + std::numeric_limits< double >::epsilon( ) ); + } //Test conversion wrapper against boost result. @@ -173,7 +240,8 @@ BOOST_AUTO_TEST_CASE( testConversionCalendarDateToJulianDay ) const double second = 21.36474854836359; BOOST_CHECK_CLOSE_FRACTION( boost::gregorian::date( year, month, day ).julian_day( ) - 0.5 + - static_cast< double >( hour ) / 24.0 + static_cast< double >( minute ) / ( 24.0 * 60.0 ) + + static_cast< double >( hour ) / 24.0 + + static_cast< double >( minute ) / ( 24.0 * 60.0 ) + second / ( 24.0 * 60.0 * 60.0 ), convertCalendarDateToJulianDay( year, month, day, hour, minute, second ), std::numeric_limits< double >::epsilon( ) ); @@ -181,6 +249,315 @@ BOOST_AUTO_TEST_CASE( testConversionCalendarDateToJulianDay ) } } +BOOST_AUTO_TEST_CASE( testTimeConversions ) +{ + const double testModifiedJulianDay = 54583.87; + const double testJulianDay = testModifiedJulianDay + JULIAN_DAY_AT_0_MJD; + { + // Test conversions to/from Modified Julian day + + BOOST_CHECK_CLOSE_FRACTION( testJulianDay, convertModifiedJulianDayToJulianDay( testModifiedJulianDay ), + 2.0 * std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( testModifiedJulianDay, convertJulianDayToModifiedJulianDay( testJulianDay ), + 10.0 * std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( convertJulianDayToModifiedJulianDay( + convertModifiedJulianDayToJulianDay( testModifiedJulianDay ) ), + testModifiedJulianDay, 10.0 * std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( convertModifiedJulianDayToJulianDay( + convertJulianDayToModifiedJulianDay( testJulianDay ) ), + testJulianDay, 2.0 * std::numeric_limits< double >::epsilon( ) ); + + // Test conversions to/from seconds since epoch + double secondsSinceModifedJulianDayZero = testModifiedJulianDay * JULIAN_DAY; + BOOST_CHECK_CLOSE_FRACTION( secondsSinceModifedJulianDayZero, convertJulianDayToSecondsSinceEpoch( + testJulianDay, JULIAN_DAY_AT_0_MJD ), + 10.0 * std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( testJulianDay, convertSecondsSinceEpochToJulianDay( + secondsSinceModifedJulianDayZero, JULIAN_DAY_AT_0_MJD ), + 2.0 * std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( testModifiedJulianDay / JULIAN_YEAR_IN_DAYS, + convertSecondsSinceEpochToJulianYearsSinceEpoch( secondsSinceModifedJulianDayZero ), + 2.0 * std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( testModifiedJulianDay / ( JULIAN_YEAR_IN_DAYS * 100.0 ), + convertSecondsSinceEpochToJulianCenturiesSinceEpoch( secondsSinceModifedJulianDayZero ), + 2.0 * std::numeric_limits< double >::epsilon( ) ); + } + + { + // Test conversion from Julian day to calendar date + int testYear = 2008; + int testMonth = 4; + int testDay = 27; + boost::gregorian::date testCalendarDate( testYear, testMonth, testDay ); + double testFractionOfDay = 0.37; + + // Check direct conversion from julian day to calendar date + boost::gregorian::date calendarDate = convertJulianDayToCalendarDate( testJulianDay ); + BOOST_CHECK_EQUAL( testYear, calendarDate.year( ) ); + BOOST_CHECK_EQUAL( testMonth, calendarDate.month( ) ); + BOOST_CHECK_EQUAL( testDay, calendarDate.day( ) ); + + // Check indirect conversion from julian day to calendar date + BOOST_CHECK_EQUAL( getDaysInMonth( 1, testYear ) + + getDaysInMonth( 2, testYear ) + + getDaysInMonth( 3, testYear ) + + testDay, testCalendarDate.day_of_year( ) ); + + calendarDate = convertYearAndDaysInYearToDate( + testYear, ( getDaysInMonth( 1, testYear ) + + getDaysInMonth( 2, testYear ) + + getDaysInMonth( 3, testYear ) + + testDay ) - 1 ); // Subtract 1 to go to day 0 for first day of year. + BOOST_CHECK_EQUAL( testYear, calendarDate.year( ) ); + BOOST_CHECK_EQUAL( testMonth, calendarDate.month( ) ); + BOOST_CHECK_EQUAL( testDay, calendarDate.day( ) ); + + // Check if noon reference of Julian day is handled properly + calendarDate = convertJulianDayToCalendarDate( testJulianDay + 0.5 ); + BOOST_CHECK_EQUAL( testYear, calendarDate.year( ) ); + BOOST_CHECK_EQUAL( testMonth, calendarDate.month( ) ); + BOOST_CHECK_EQUAL( testDay + 1, calendarDate.day( ) ); + + BOOST_CHECK_CLOSE_FRACTION( calculateJulianDaySinceEpoch( + testCalendarDate, 0.5 + testFractionOfDay, JULIAN_DAY_AT_0_MJD ), + testModifiedJulianDay, + 50.0 * std::numeric_limits< double >::epsilon( ) ); + } + + // Test relativistic time scale conversions (TCG, TT, TDB, TCB) for double precision. + { + double secondsSinceJ2000Synchronization = getTimeOfTaiSynchronizationSinceJ2000< double >( ); + + // Test whether TCG and TT are both 0 at synchronization time. + double testTcg = convertTtToTcg( secondsSinceJ2000Synchronization ); + double testTt = convertTcgToTt( secondsSinceJ2000Synchronization ); + + BOOST_CHECK_CLOSE_FRACTION( testTcg, secondsSinceJ2000Synchronization, + 2.0 * std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( testTt, secondsSinceJ2000Synchronization, + 2.0 * std::numeric_limits< double >::epsilon( ) ); + + // Test whether TDB and TCB are both at defined relative offset at given time. + double testTcb = convertTdbToTcb( secondsSinceJ2000Synchronization ); + double testTdb = convertTcbToTdb( secondsSinceJ2000Synchronization ); + + BOOST_CHECK_CLOSE_FRACTION( testTdb, secondsSinceJ2000Synchronization + TDB_SECONDS_OFFSET_AT_SYNCHRONIZATION, + 2.0 * std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( testTcb, secondsSinceJ2000Synchronization - TDB_SECONDS_OFFSET_AT_SYNCHRONIZATION, + 2.0 * std::numeric_limits< double >::epsilon( ) ); + + // Test back and forth TCG<->TT, and expected value of TCG at TT=0.. + double testTime = 0.0; + + testTcg = convertTtToTcg( testTime); + testTt = convertTcgToTt( testTcg ); + + double expectedTcg = -secondsSinceJ2000Synchronization * LG_TIME_RATE_TERM / + ( 1.0 - physical_constants::LG_TIME_RATE_TERM_LONG ); + BOOST_CHECK_CLOSE_FRACTION( testTcg, expectedTcg, 2.0 * std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_SMALL( testTt, std::numeric_limits< double >::epsilon( ) ); + + // Test back and forth TDB<->TCB, and expected value of TDB at TCB=0. + testTdb = convertTcbToTdb( testTime ); + testTcb = convertTdbToTcb( testTdb ); + + double expectedTdb = secondsSinceJ2000Synchronization * LB_TIME_RATE_TERM + TDB_SECONDS_OFFSET_AT_SYNCHRONIZATION; + + BOOST_CHECK_CLOSE_FRACTION( testTdb, expectedTdb, 2.0 * std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_SMALL( testTcb, std::numeric_limits< double >::epsilon( ) ); + + // Test back and forth TCG<->TT. + double secondsSinceModifedJulianDayZero = testModifiedJulianDay * JULIAN_DAY; + testTime = secondsSinceModifedJulianDayZero; + + testTcg = convertTtToTcg< double >( testTime ); + testTt = convertTcgToTt< double >( testTcg ); + + BOOST_CHECK_CLOSE_FRACTION( testTt, testTime, 2.0 * std::numeric_limits< double >::epsilon( ) ); + + // Test back and forth TDB<->TCB. + testTdb = convertTcbToTdb< long double >( testTime ); + testTcb = convertTdbToTcb< long double >( testTdb ); + + BOOST_CHECK_CLOSE_FRACTION( testTcb, testTime, 2.0 * std::numeric_limits< double >::epsilon( ) ); + + secondsSinceModifedJulianDayZero = 1.0E12; + + // Test rate difference between TCG and TT + double testTt1 = 0.0; + double testTt2 = secondsSinceModifedJulianDayZero; + double testTcg1 = convertTtToTcg< double >( testTt1 ); + double testTcg2 = convertTtToTcg< double >( testTt2 ); + double computedLg = 1.0 - ( testTt2 - testTt1 ) / ( testTcg2 - testTcg1 ); + BOOST_CHECK_SMALL( computedLg - physical_constants::LG_TIME_RATE_TERM, + std::numeric_limits< double >::epsilon( ) ); + + testTcg1 = 0.0; + testTcg2 = secondsSinceModifedJulianDayZero; + testTt1 = convertTcgToTt< double >( testTcg1 ); + testTt2 = convertTcgToTt< double >( testTcg2 ); + computedLg = 1.0 - ( testTt2 - testTt1 ) / ( testTcg2 - testTcg1 ); + BOOST_CHECK_SMALL( computedLg - physical_constants::LG_TIME_RATE_TERM, + std::numeric_limits< double >::epsilon( ) ); + + + // Test rate difference between TDB and TCB + double testTcb1 = 0.0; + double testTcb2 = secondsSinceModifedJulianDayZero; + double testTdb1 = convertTcbToTdb< double >( testTcb1 ); + double testTdb2 = convertTcbToTdb< double >( testTcb2 ); + double computedLb = 1.0 - ( testTdb2 - testTdb1 ) / ( testTcb2 - testTcb1 ); + BOOST_CHECK_SMALL( computedLb - physical_constants::LB_TIME_RATE_TERM, + std::numeric_limits< double >::epsilon( ) ); + + testTdb1 = 0.0; + testTdb2 = secondsSinceModifedJulianDayZero; + testTcb1 = convertTdbToTcb< double >( 0.0 ); + testTcb2 = convertTdbToTcb< double >( secondsSinceModifedJulianDayZero ); + computedLb = 1.0 - ( testTdb2 - testTdb1 ) / ( testTcb2 - testTcb1 ); + BOOST_CHECK_SMALL( computedLb - physical_constants::LB_TIME_RATE_TERM, + std::numeric_limits< double >::epsilon( ) ); + } + + // Compare TT/TCG conversions against Sofa cookbook (only limited precision). + { + double expectedTT = physical_constants::JULIAN_DAY * + convertCalendarDateToJulianDaysSinceEpoch( 2006, 1, 15, 12, 25, 42.68400, JULIAN_DAY_ON_J2000 ); + double expectedTCG = physical_constants::JULIAN_DAY * + convertCalendarDateToJulianDaysSinceEpoch( 2006, 1, 15, 12, 25, 43.32269, JULIAN_DAY_ON_J2000 ); + + double calculatedTT = convertTcgToTt( expectedTCG ); + double calculatedTCG = convertTtToTcg( expectedTT ); + + BOOST_CHECK_SMALL( calculatedTCG - expectedTCG, 5.0E-5 ); + BOOST_CHECK_SMALL( calculatedTT - expectedTT, 5.0E-5 ); + + } +} + +// Test relativistic time scale conversions (TCG, TT, TDB, TCB) for long double precision. +BOOST_AUTO_TEST_CASE( testTimeConversionsLong ) +{ + // Define test dates (arbitrary). + const long double testModifiedJulianDay = static_cast< long double >( 54583.87 ); + const long double testJulianDay = testModifiedJulianDay + JULIAN_DAY_AT_0_MJD_LONG; + + // Test whether back and forth conversion between JD and MJD provides correct resulats. + BOOST_CHECK_CLOSE_FRACTION( testJulianDay, convertModifiedJulianDayToJulianDay< long double >( testModifiedJulianDay ), + 2.0 * std::numeric_limits< long double >::epsilon( )); + BOOST_CHECK_CLOSE_FRACTION( testModifiedJulianDay, convertJulianDayToModifiedJulianDay< long double >( testJulianDay ), + 2.0 * std::numeric_limits< long double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( convertJulianDayToModifiedJulianDay< long double >( + convertModifiedJulianDayToJulianDay< long double >( testModifiedJulianDay ) ), + testModifiedJulianDay, 2.0 * std::numeric_limits< long double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( convertModifiedJulianDayToJulianDay( + convertJulianDayToModifiedJulianDay( testJulianDay ) ), + testJulianDay, 2.0 * std::numeric_limits< long double >::epsilon( ) ); + + // Test conversion to seconds since Epoch for JD and MJD + long double secondsSinceModifedJulianDayZero = testModifiedJulianDay * JULIAN_DAY_LONG; + + BOOST_CHECK_CLOSE_FRACTION( secondsSinceModifedJulianDayZero, convertJulianDayToSecondsSinceEpoch< long double >( + testJulianDay, JULIAN_DAY_AT_0_MJD_LONG ), + 2.0 * std::numeric_limits< long double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( testJulianDay, convertSecondsSinceEpochToJulianDay< long double >( + secondsSinceModifedJulianDayZero, JULIAN_DAY_AT_0_MJD_LONG ), + 2.0 * std::numeric_limits< long double >::epsilon( ) ); + + // Test whether TCG and TT are both 0 at synchronization time. + long double secondsSinceJ2000Synchronization = getTimeOfTaiSynchronizationSinceJ2000< long double >( ); + + long double testTcg = convertTtToTcg< long double >( secondsSinceJ2000Synchronization ); + long double testTt = convertTcgToTt< long double >( secondsSinceJ2000Synchronization ); + + BOOST_CHECK_CLOSE_FRACTION( testTcg, secondsSinceJ2000Synchronization, + 2.0 * std::numeric_limits< long double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( testTt, secondsSinceJ2000Synchronization, + 2.0 * std::numeric_limits< long double >::epsilon( ) ); + + // Test whether TDB and TCB are both at defined relative offset at given time. + long double testTcb = convertTdbToTcb< long double >( secondsSinceJ2000Synchronization ); + long double testTdb = convertTcbToTdb< long double >( secondsSinceJ2000Synchronization ); + + BOOST_CHECK_CLOSE_FRACTION( testTdb, secondsSinceJ2000Synchronization + TDB_SECONDS_OFFSET_AT_SYNCHRONIZATION, + 2.0 * std::numeric_limits< long double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( testTcb, secondsSinceJ2000Synchronization - TDB_SECONDS_OFFSET_AT_SYNCHRONIZATION, + 2.0 * std::numeric_limits< long double >::epsilon( ) ); + + // Test back and forth TCG<->TT at t=0, and expected value of TCG at TT=0.. + long double testTime = 0.0L; + + testTcg = convertTtToTcg< long double >( testTime); + testTt = convertTcgToTt< long double >( testTcg ); + + long double expectedTcg = -secondsSinceJ2000Synchronization * LG_TIME_RATE_TERM_LONG / + ( 1.0L - LG_TIME_RATE_TERM_LONG); + BOOST_CHECK_CLOSE_FRACTION( testTcg, expectedTcg, 2.0 * std::numeric_limits< long double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( testTt, testTime, 2.0 * std::numeric_limits< long double >::epsilon( ) ); + + // Test back and forth TDB<->TCB at t=0, and expected value of TDB at TCB=0. + testTdb = convertTcbToTdb< long double >( testTime ); + testTcb = convertTdbToTcb< long double >( testTdb ); + + long double expectedTdb = secondsSinceJ2000Synchronization * LB_TIME_RATE_TERM_LONG + + TDB_SECONDS_OFFSET_AT_SYNCHRONIZATION_LONG; + + BOOST_CHECK_CLOSE_FRACTION( testTdb, expectedTdb, 2.0 * std::numeric_limits< long double >::epsilon( ) ); + BOOST_CHECK_SMALL( testTcb, 2.0 * std::numeric_limits< long double >::epsilon( ) ); + + // Test back and forth TCG<->TT. + testTime = secondsSinceModifedJulianDayZero; + + testTcg = convertTtToTcg< long double >( testTime ); + testTt = convertTcgToTt< long double >( testTcg ); + + BOOST_CHECK_CLOSE_FRACTION( testTt, testTime, 2.0 * std::numeric_limits< long double >::epsilon( ) ); + + // Test back and forth TDB<->TCB. + testTdb = convertTcbToTdb< long double >( testTime ); + testTcb = convertTdbToTcb< long double >( testTdb ); + + BOOST_CHECK_CLOSE_FRACTION( testTcb, testTime, 2.0 * std::numeric_limits< long double >::epsilon( ) ); + + secondsSinceModifedJulianDayZero = 1.0E12; + + // Test rate difference between TCG and TT + long double testTt1 = 0.0; + long double testTt2 = secondsSinceModifedJulianDayZero; + long double testTcg1 = convertTtToTcg< long double >( testTt1 ); + long double testTcg2 = convertTtToTcg< long double >( testTt2 ); + long double computedLg = 1.0L - ( testTt2 - testTt1 ) / ( testTcg2 - testTcg1 ); + BOOST_CHECK_SMALL( computedLg - physical_constants::LG_TIME_RATE_TERM_LONG, + std::numeric_limits< long double >::epsilon( ) ); + + testTcg1 = 0.0; + testTcg2 = secondsSinceModifedJulianDayZero; + testTt1 = convertTcgToTt< long double >( testTcg1 ); + testTt2 = convertTcgToTt< long double >( testTcg2 ); + computedLg = 1.0L - ( testTt2 - testTt1 ) / ( testTcg2 - testTcg1 ); + BOOST_CHECK_SMALL( computedLg - physical_constants::LG_TIME_RATE_TERM_LONG, + std::numeric_limits< long double >::epsilon( ) ); + + + // Test rate difference between TDB and TCB + long double testTcb1 = 0.0L; + long double testTcb2 = secondsSinceModifedJulianDayZero; + long double testTdb1 = convertTcbToTdb< long double >( testTcb1 ); + long double testTdb2 = convertTcbToTdb< long double >( testTcb2 ); + long double computedLb = 1.0L - ( testTdb2 - testTdb1 ) / ( testTcb2 - testTcb1 ); + BOOST_CHECK_SMALL( computedLb - physical_constants::LB_TIME_RATE_TERM_LONG, + std::numeric_limits< long double >::epsilon( ) ); + + testTdb1 = 0.0; + testTdb2 = secondsSinceModifedJulianDayZero; + testTcb1 = convertTdbToTcb< long double >( 0.0 ); + testTcb2 = convertTdbToTcb< long double >( secondsSinceModifedJulianDayZero ); + computedLb = 1.0L - ( testTdb2 - testTdb1 ) / ( testTcb2 - testTcb1 ); + BOOST_CHECK_SMALL( computedLb - physical_constants::LB_TIME_RATE_TERM_LONG, + std::numeric_limits< long double >::epsilon( ) ); + +} + BOOST_AUTO_TEST_SUITE_END( ) } // namespace unit_tests diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h b/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h index d5a109f71a..441b7db6fa 100644 --- a/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h +++ b/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h @@ -45,8 +45,9 @@ #ifndef TUDAT_ACCELERATION_MODEL_H #define TUDAT_ACCELERATION_MODEL_H -#include #include +#include +#include #include @@ -72,6 +73,11 @@ class AccelerationModel { public: + + //! Constructor. + AccelerationModel( ): + currentTime_( TUDAT_NAN ){ } + //! Virtual destructor. /*! * Virtual destructor, necessary to ensure that derived class destructors get called correctly. @@ -103,6 +109,11 @@ class AccelerationModel */ virtual void updateMembers( const double currentTime = TUDAT_NAN ) = 0; + //! Function to reset the current time + /*! + * Function to reset the current time of the acceleration model. + * \param currentTime Current time (default NaN). + */ virtual void resetTime( const double currentTime = TUDAT_NAN ) { currentTime_ = currentTime; @@ -110,6 +121,7 @@ class AccelerationModel protected: + //! Previous time to which acceleration model was updated. double currentTime_; protected: @@ -137,14 +149,16 @@ typedef boost::shared_ptr< AccelerationModel2d > AccelerationModel2dPointer; * \tparam AccelerationDataType Data type used to represent accelerations * (default=Eigen::Vector3d). * \param accelerationModel Acceleration model that is to be evaluated. + * \param currentTime Time at which acceleration model is to be updated. * \return Acceleration that is obtained following the member update. */ template < typename AccelerationDataType > AccelerationDataType updateAndGetAcceleration( - boost::shared_ptr< AccelerationModel< AccelerationDataType > > accelerationModel ) + const boost::shared_ptr< AccelerationModel< AccelerationDataType > > accelerationModel, + const double currentTime = TUDAT_NAN ) { // Update members. - accelerationModel->updateMembers( ); + accelerationModel->updateMembers( currentTime ); // Evaluate and return acceleration. return accelerationModel->getAcceleration( ); @@ -152,14 +166,14 @@ AccelerationDataType updateAndGetAcceleration( //! Typedef defining a list of accelerations acting on a single body, key is the name of each //! body exerting a acceletation, value is a list of accelerations exerted by that body. -typedef std::map< std::string, std::vector< +typedef std::unordered_map< std::string, std::vector< boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > > > SingleBodyAccelerationMap; //! Typedef defining a list of accelerations acting on a set of bodies, key is the name of each //! body undergoing a acceletation, value is SingleBodyAccelerationMap, defining all accelerations //! acting on it. -typedef std::map< std::string, SingleBodyAccelerationMap > AccelerationMap; +typedef std::unordered_map< std::string, SingleBodyAccelerationMap > AccelerationMap; } // namespace basic_astrodynamics } // namespace tudat diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.cpp b/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.cpp index d7d8649a22..fe71a3617c 100644 --- a/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.cpp +++ b/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.cpp @@ -40,6 +40,48 @@ namespace tudat namespace basic_astrodynamics { +//! Function to get a string representing a 'named identification' of an acceleration type +std::string getAccelerationModelName( const AvailableAcceleration accelerationType ) +{ + std::string accelerationName; + switch( accelerationType ) + { + case central_gravity: + accelerationName = "central gravity "; + break; + case aerodynamic: + accelerationName = "aerodynamic "; + break; + case cannon_ball_radiation_pressure: + accelerationName = "cannonball radiation pressure "; + break; + case spherical_harmonic_gravity: + accelerationName = "spherical harmonic gravity "; + break; + case mutual_spherical_harmonic_gravity: + accelerationName = "mutual spherical harmonic gravity "; + break; + case third_body_central_gravity: + accelerationName = "third-body central gravity "; + break; + case third_body_spherical_harmonic_gravity: + accelerationName = "third-body spherical harmonic gravity "; + break; + case third_body_mutual_spherical_harmonic_gravity: + accelerationName = "third-body mutual spherical harmonic gravity "; + break; + case thrust_acceleration: + accelerationName = "thrust "; + break; + default: + std::string errorMessage = "Error, acceleration type " + + boost::lexical_cast< std::string >( accelerationType ) + + "not found when retrieving acceleration name "; + throw std::runtime_error( errorMessage ); + } + return accelerationName; +} + //! Function to identify the derived class type of an acceleration model. AvailableAcceleration getAccelerationModelType( const boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > @@ -68,16 +110,25 @@ AvailableAcceleration getAccelerationModelType( { accelerationType = third_body_central_gravity; } - else if( boost::dynamic_pointer_cast< SphericalHarmonicsGravitationalAccelerationModelXd >( + else if( boost::dynamic_pointer_cast< SphericalHarmonicsGravitationalAccelerationModel >( accelerationModel ) != NULL ) { accelerationType = spherical_harmonic_gravity; } + else if( boost::dynamic_pointer_cast< MutualSphericalHarmonicsGravitationalAccelerationModel >( accelerationModel ) != NULL ) + { + accelerationType = mutual_spherical_harmonic_gravity; + } else if( boost::dynamic_pointer_cast< AerodynamicAcceleration >( accelerationModel ) != NULL ) { accelerationType = aerodynamic; } + else if( boost::dynamic_pointer_cast< propulsion::ThrustAcceleration >( + accelerationModel ) != NULL ) + { + accelerationType = thrust_acceleration; + } else { throw std::runtime_error( @@ -89,6 +140,47 @@ AvailableAcceleration getAccelerationModelType( } +//! Function to identify the type of a mass rate model. +AvailableMassRateModels getMassRateModelType( + const boost::shared_ptr< MassRateModel > massRateModel ) +{ + // Nominal type is undefined + AvailableMassRateModels massRateType = undefined_mass_rate_model; + + // Check for each mass rate mdoel type implemented as AvailableMassRateModels. + if( boost::dynamic_pointer_cast< basic_astrodynamics::CustomMassRateModel >( + massRateModel ) != NULL ) + { + massRateType = custom_mass_rate_model; + } + else if( boost::dynamic_pointer_cast< propulsion::FromThrustMassRateModel >( + massRateModel ) != NULL ) + { + massRateType = from_thrust_mass_rate_model; + } + else + { + throw std::runtime_error( + "Error, mass rate model not identified when getting mass rate model type." ); + } + return massRateType; +} + +//! Function to get all acceleration models of a given type from a list of models +std::vector< boost::shared_ptr< AccelerationModel3d > > getAccelerationModelsOfType( + const std::vector< boost::shared_ptr< AccelerationModel3d > >& fullList, + const AvailableAcceleration modelType ) +{ + std::vector< boost::shared_ptr< AccelerationModel3d > > accelerationList; + for( unsigned int i = 0; i < fullList.size( ); i++ ) + { + if( getAccelerationModelType( fullList.at( i ) ) == modelType ) + { + accelerationList.push_back( fullList.at( i ) ); + } + } + return accelerationList; +} } // namespace basic_astrodynamics diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h b/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h index 83b6520f65..a4d6ce19e5 100644 --- a/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h +++ b/Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h @@ -38,8 +38,12 @@ #include "Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.h" #include "Tudat/Astrodynamics/Gravitation/centralGravityModel.h" #include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h" +#include "Tudat/Astrodynamics/Gravitation/mutualSphericalHarmonicGravityModel.h" #include "Tudat/Astrodynamics/Gravitation/thirdBodyPerturbation.h" #include "Tudat/Astrodynamics/Aerodynamics/aerodynamicAcceleration.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/massRateModel.h" +#include "Tudat/Astrodynamics/Propulsion/thrustAccelerationModel.h" +#include "Tudat/Astrodynamics/Propulsion/massRateFromThrust.h" namespace tudat @@ -61,8 +65,31 @@ enum AvailableAcceleration aerodynamic, cannon_ball_radiation_pressure, spherical_harmonic_gravity, + mutual_spherical_harmonic_gravity, third_body_central_gravity, - third_body_spherical_harmonic_gravity + third_body_spherical_harmonic_gravity, + third_body_mutual_spherical_harmonic_gravity, + thrust_acceleration +}; + +//! Function to get a string representing a 'named identification' of an acceleration type +/*! + * Function to get a string representing a 'named identification' of an acceleration type + * \param accelerationType Type of acceleration model. + * \return String with acceleration id. + */ +std::string getAccelerationModelName( const AvailableAcceleration accelerationType ); + +//! List of model types for body mass rates. +/*! +* List of model types for body mass rates available in simulations. Mass rate models not defined by this +* given enum cannot be used for automatic mass rate model setup. +*/ +enum AvailableMassRateModels +{ + undefined_mass_rate_model, + custom_mass_rate_model, + from_thrust_mass_rate_model }; //! Function to identify the derived class type of an acceleration model. @@ -76,6 +103,26 @@ AvailableAcceleration getAccelerationModelType( const boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > accelerationModel ); +//! Function to identify the type of a mass rate model. +/*! + * Function to identify the type of a mass rate model. The type must be defined + * in the AvailableMassRateModels enum to be recognized by this function. + * \param massRateModel Mass rate model of which the type is to be identified. + * \return Type of the massRateModel, as identified by AvailableMassRateModels enum. + */ +AvailableMassRateModels getMassRateModelType( + const boost::shared_ptr< MassRateModel > massRateModel ); + +//! Function to get all acceleration models of a given type from a list of models +/*! + * Function to get all acceleration models of a given type from a list of models + * \param fullList List of acceleration models + * \param modelType Type for which all models are to be retrieved + * \return Subset of fullList for which the acceleration model type is modelType + */ +std::vector< boost::shared_ptr< AccelerationModel3d > > getAccelerationModelsOfType( + const std::vector< boost::shared_ptr< AccelerationModel3d > >& fullList, + const AvailableAcceleration modelType ); } // namespace basic_astrodynamics diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/geodeticCoordinateConversions.cpp b/Tudat/Astrodynamics/BasicAstrodynamics/geodeticCoordinateConversions.cpp index 01d917fda3..4d00900bae 100644 --- a/Tudat/Astrodynamics/BasicAstrodynamics/geodeticCoordinateConversions.cpp +++ b/Tudat/Astrodynamics/BasicAstrodynamics/geodeticCoordinateConversions.cpp @@ -34,6 +34,7 @@ * */ +#include #include #include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h" diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/keplerPropagator.h b/Tudat/Astrodynamics/BasicAstrodynamics/keplerPropagator.h index a68f5ed84b..3c0ab9656b 100644 --- a/Tudat/Astrodynamics/BasicAstrodynamics/keplerPropagator.h +++ b/Tudat/Astrodynamics/BasicAstrodynamics/keplerPropagator.h @@ -147,7 +147,8 @@ Eigen::Matrix< ScalarType, 6, 1 > propagateKeplerOrbit( const ScalarType finalEccentricAnomaly = convertMeanAnomalyToEccentricAnomaly< ScalarType >( initialStateInKeplerianElements( eccentricityIndex ), - initialMeanAnomaly + meanAnomalyChange ); + initialMeanAnomaly + meanAnomalyChange, true, + TUDAT_NAN, aRootFinder ); // Compute true anomaly for computed eccentric anomaly. finalStateInKeplerianElements( trueAnomalyIndex ) = diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/massRateModel.h b/Tudat/Astrodynamics/BasicAstrodynamics/massRateModel.h new file mode 100644 index 0000000000..3784b4645f --- /dev/null +++ b/Tudat/Astrodynamics/BasicAstrodynamics/massRateModel.h @@ -0,0 +1,135 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_MASSRATEMODEL_H +#define TUDAT_MASSRATEMODEL_H + +#include +#include +#include +#include + +#include + +#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h" + +namespace tudat +{ +namespace basic_astrodynamics +{ + +//! Base class for determining the rate of change of a body's mass, to be used in numerical integration. +/*! + * Base class for determining the rate of change of a body's mass, to be used in numerical integration. Specific types + * of mass rate models are to be implemented in derived classes + */ +class MassRateModel +{ +public: + + //! Constructor + MassRateModel( ): + currentTime_( TUDAT_NAN ), currentMassRate_( TUDAT_NAN ){ } + + //! Destructor + virtual ~MassRateModel( ) { } + + //! Function to retrieve current mass rate + /*! + * Function to retrieve current mass rate, as set by last call to updateMembers (implemented in derived class) + * \return Current mass rate + */ + virtual double getMassRate( ) + { + return currentMassRate_; + } + + //! Update member variables used by the mass rate model, and internally compute mass rate. + /*! + * Updates member variables used by the mass rate model, and internally compute mass rate. In the case of mass rate + * models containing varying parameters, function-pointers returning such a parameter (for instance + * the Cartesian state of a body) will be set as a member variable. + * This function evaluates such function-pointers and updates member variables to the 'current' + * values of these parameters. Only these current values, not the function-pointers are then + * used for the actual computation function. + * \param currentTime Time at which acceleration model is to be updated. + */ + virtual void updateMembers( const double currentTime = TUDAT_NAN ) = 0; + + //! Function to reset the current time + /*! + * Function to reset the current time of the acceleration model. + * \param currentTime Current time (default NaN). + */ + virtual void resetTime( const double currentTime = TUDAT_NAN ) + { + currentTime_ = currentTime; + } + +protected: + + //! Previous time to which mass rate model was updated. + double currentTime_; + + //! Current mass rate, as set by last call to updateMembers (implemented in derived class) + double currentMassRate_; + +private: +}; + +//! Derived class for determining the rate of change of a body's mass, user-defined by a function pointer. +/*! + * Derived class for determining the rate of change of a body's mass, user-defined by a function pointer. + * This class can be used for any kind of mass rate model for which the user can define the dependency as a function + * of time. + */ +class CustomMassRateModel: public MassRateModel +{ +public: + + //! Constructor. + /*! + * Constructor + * \param massRateFunction Function returning mass rate as a function of time. + */ + CustomMassRateModel( + const boost::function< double( const double ) > massRateFunction ): + massRateFunction_( massRateFunction ){ } + + //! Destructor. + ~CustomMassRateModel( ){ } + + //! Update member variables used by the mass rate model and compute the mass rate + /*! + * Update member variables used by the mass rate model and compute the mass rate + * \param currentTime Time at which acceleration model is to be updated. + */ + void updateMembers( const double currentTime = TUDAT_NAN ) + { + // Check if update is needed. + if( !( currentTime_ == currentTime ) ) + { + currentMassRate_ = massRateFunction_( currentTime ); + } + } + +private: + + //! Function returning mass rate as a function of time. + boost::function< double( const double ) > massRateFunction_; + +}; + + +} // namespace basic_astrodynamics + +} // namespace tudat + +#endif // TUDAT_MASSRATEMODEL_H diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h b/Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h index ee4e73d4a0..5b58eea9e9 100644 --- a/Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h +++ b/Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h @@ -68,6 +68,11 @@ namespace tudat namespace physical_constants { + +//! Standard gravitational acceleration at sea-level. +const static double SEA_LEVEL_GRAVITATIONAL_ACCELERATION = 9.80665; + + //! Julian day in seconds (NASA, 2012). const static double JULIAN_DAY = 86400.0; diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/sphericalStateConversions.cpp b/Tudat/Astrodynamics/BasicAstrodynamics/sphericalStateConversions.cpp index 24e960900f..2c6ac97959 100644 --- a/Tudat/Astrodynamics/BasicAstrodynamics/sphericalStateConversions.cpp +++ b/Tudat/Astrodynamics/BasicAstrodynamics/sphericalStateConversions.cpp @@ -8,7 +8,7 @@ * http://tudat.tudelft.nl/LICENSE. */ -#include +#include #include #include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h" @@ -42,7 +42,7 @@ basic_mathematics::Vector6d convertCartesianToSphericalOrbitalState( basic_mathematics::Vector6d sphericalOrbitalState; // Compute and set spherical position - Eigen::Vector3d sphericalPosition = coordinate_conversions::convertCartesianToSpherical( + Eigen::Vector3d sphericalPosition = coordinate_conversions::convertCartesianToSpherical< double >( bodyFixedCartesianState.segment( 0, 3 ) ); sphericalOrbitalState( radiusIndex ) = sphericalPosition( 0 ); sphericalOrbitalState( latitudeIndex ) = mathematical_constants::PI / 2.0 - sphericalPosition( 1 ); diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/sphericalStateConversions.h b/Tudat/Astrodynamics/BasicAstrodynamics/sphericalStateConversions.h index f218abfbb2..04b3244131 100644 --- a/Tudat/Astrodynamics/BasicAstrodynamics/sphericalStateConversions.h +++ b/Tudat/Astrodynamics/BasicAstrodynamics/sphericalStateConversions.h @@ -79,9 +79,9 @@ basic_mathematics::Vector6d convertCartesianToSphericalOrbitalState( basic_mathematics::Vector6d convertSphericalOrbitalToCartesianState( const basic_mathematics::Vector6d& sphericalOrbitalState ); -} // namespace tudat - } // namespace orbital_element_conversions +} // namespace tudat + #endif // TUDAT_SPHERICALSTATECONVERSIONS_H diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/stateRepresentationConversions.h b/Tudat/Astrodynamics/BasicAstrodynamics/stateRepresentationConversions.h new file mode 100644 index 0000000000..3e23b78778 --- /dev/null +++ b/Tudat/Astrodynamics/BasicAstrodynamics/stateRepresentationConversions.h @@ -0,0 +1,11 @@ +#ifndef TUDAT_STATEREPRESENTATIONCONVERSIONS_H +#define TUDAT_STATEREPRESENTATIONCONVERSIONS_H + +#include "Tudat/Astrodynamics/BasicAstrodynamics/convertMeanToEccentricAnomalies.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/geodeticCoordinateConversions.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/modifiedEquinoctialElementConversions.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/unifiedStateModelElementConversions.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h" + +#endif // TUDAT_STATEREPRESENTATIONCONVERSIONS_H diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/timeConversions.cpp b/Tudat/Astrodynamics/BasicAstrodynamics/timeConversions.cpp index 3a7d483c51..c44ff7ad36 100644 --- a/Tudat/Astrodynamics/BasicAstrodynamics/timeConversions.cpp +++ b/Tudat/Astrodynamics/BasicAstrodynamics/timeConversions.cpp @@ -35,7 +35,6 @@ #include #include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h" - #include "Tudat/Astrodynamics/BasicAstrodynamics/timeConversions.h" namespace tudat @@ -43,40 +42,243 @@ namespace tudat namespace basic_astrodynamics { -//! Compute number of seconds since a reference Julian day. -double convertJulianDayToSecondsSinceEpoch( const double julianDay, - const double epochSinceJulianDayZero ) +//! Function to get the Julian day on J2000, in double precision. +template< > +double getJulianDayOnJ2000< double >( ) +{ + return JULIAN_DAY_ON_J2000; +} + +//! Function to get the Julian day on J2000, in long double precision. +template< > +long double getJulianDayOnJ2000< long double >( ) { - return ( julianDay - epochSinceJulianDayZero ) * physical_constants::JULIAN_DAY; + return JULIAN_DAY_ON_J2000_LONG; } -//! Compute Julian day from seconds since reference Julian day epoch. -double convertSecondsSinceEpochToJulianDay( const double secondsSinceEpoch, - const double epochSinceJulianDayZero ) +//! Function to get the Julian day on zero modified Julian day, in double precision. +template< > +double getJulianDayOnMjd0< double >( ) { - return ( secondsSinceEpoch / physical_constants::JULIAN_DAY + epochSinceJulianDayZero ); + return JULIAN_DAY_AT_0_MJD; } -//! Compute the Julian day from the calendar date and time. -double convertCalendarDateToJulianDay( const int calendarYear, - const int calendarMonth, - const int calendarDay, - const int calendarHour, - const int calendarMinutes, - const double calendarSeconds ) +//! Function to get the Julian day on zero modified Julian day, in long double precision. +template< > +long double getJulianDayOnMjd0< long double >( ) { - // Calculate julian day of calendar date. - double julianDay = boost::gregorian::date( calendarYear, calendarMonth, calendarDay ).julian_day( ); + return JULIAN_DAY_AT_0_MJD_LONG; +} - //Compute day fraction - const double dayFraction = static_cast< double >( calendarHour ) / 24.0 + - static_cast< double >( calendarMinutes ) / ( 24.0 * 60.0 ) + - calendarSeconds / ( 24.0 * 3600.0 ); +//! Function to get the synchronization Julian day of TT, TCG, and TCB, in double precision. +template< > +double getTimeOfTaiSynchronizationJulianDay< double >( ) +{ + return TAI_JULIAN_DAY_AT_TIME_SYNCHRONIZATION; +} - // Compute Julian day by adding day fraction and subtracting 0.5 to reference to midnight - // instead of noon.. - return julianDay + dayFraction - 0.5; +//! Function to get the synchronization Julian day of TT, TCG, and TCB, in long double precision. +template< > +long double getTimeOfTaiSynchronizationJulianDay< long double >( ) +{ + return TAI_JULIAN_DAY_AT_TIME_SYNCHRONIZATION_LONG; } +//! Function to get the difference between TDB and (TT, TCB and TCB) at TAI_JULIAN_DAY_AT_TIME_SYNCHRONIZATION, +//! in double precision. +template< > +double getTdbSecondsOffsetAtSynchronization< double >( ) +{ + return TDB_SECONDS_OFFSET_AT_SYNCHRONIZATION; +} + +//! Function to get the difference between TDB and (TT, TCB and TCB) at TAI_JULIAN_DAY_AT_TIME_SYNCHRONIZATION, +//! in long double precision. +template< > +long double getTdbSecondsOffsetAtSynchronization< long double >( ) +{ + return TDB_SECONDS_OFFSET_AT_SYNCHRONIZATION_LONG; +} + +//! Function to get the offset of TT from TAI (constant by definition), in double precision. +template< > +double getTTMinusTai< double >( ) +{ + return TT_MINUS_TAI; +} + +//! Function to get the offset of TT from TAI (constant by definition), in long double precision. +template< > +long double getTTMinusTai< long double >( ) +{ + return TT_MINUS_TAI_LONG; +} + +//! Function to convert julian day to gregorian calendar date. +boost::gregorian::date convertJulianDayToCalendarDate( const double julianDay ) +{ + // Declare temporary variables. + int L, M, N, P, Q; + + // Declare date variables. + int day, month, year; + + // Execute algorithm. + double shiftedJulianDay = julianDay + 0.5; + if( shiftedJulianDay > 2299160 ) // after Oct 4, 1582 + { + L = shiftedJulianDay + 68569; + M = (4 * L) / 146097; + L = L - ((146097 * M + 3) / 4); + N = (4000 * (L + 1)) / 1461001; + L = L - ((1461 * N) / 4) + 31; + P = (80 * L) / 2447; + day = int(L - (2447 * P) / 80); + L = P / 11; + month = int(P + 2 - 12 * L); + year = int(100 * (M - 49) + N + L); + } + else + { + P = shiftedJulianDay + 1402; + Q = (P - 1) / 1461; + L = P - 1461 * Q; + M = (L - 1) / 365 - L / 1461; + N = L - 365 * M + 30; + P = (80 * N) / 2447; + day = int(N - (2447 * P) / 80); + N = P / 11; + month = int( P + 2 - 12 * N ); + year = int( 4 * Q + M + N - 4716 ); + if(year <= 0) + { + --year; + } + } + // catch century/non-400 non-leap years + if( year > 1599 && !( year % 100 ) + && ( year % 400 ) && month == 2 && day == 29 ) + { + month = 3; + day = 1; + } + + // Create and return date object + + return boost::gregorian::date( year, month, static_cast< double >( day ) ); +} + + +//! Function to determine whether the given year is a leap year (i.e. has 366 days) +bool isLeapYear( const int year ) +{ + bool isLeapYear = 0; + if( ( year % 4 == 0 ) && !( ( year % 100 == 0 ) && !( year % 400 == 0 ) ) ) + { + isLeapYear = 1; + } + return isLeapYear; +} + +//! Function that returns number of days in given month number +int getDaysInMonth( const int month, + const int year ) +{ + // Declare number of days per month + static const int daysPerMonth[ 12 ] = + { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; + int numberOfDays = 0; + + // Check input consistency + if( month < 1 || month > 12 ) + { + std::cerr<<"Month number "< 12 ) + { + std::cerr<<"Error when converting year and days in year to date, month number has exceeded 12"< +TimeType getJulianDayOnJ2000( ); + +//! Julian day at Modified Julain Date 0, i.e. Nov 17, 1858, 00:00. const static double JULIAN_DAY_AT_0_MJD = 2400000.5; +//! Julian day at Modified Julain Date 0, i.e. Nov 17, 1858, 00:00, in long double precision. +const static long double JULIAN_DAY_AT_0_MJD_LONG = 2400000.5L; + +//! Function to get the Julian day on zero modified Julian day. +/*! + * Function to get the Julian day on on zero modified Julian day, in the requested time representation type + * \return Julian day on zero modified Julian day. + */ +template< typename TimeType > +TimeType getJulianDayOnMjd0( ); + +//! Julian day at which TT, TCG, and TCB all show exact same time (1977 January 1, 00:00:32.184) +const static double TAI_JULIAN_DAY_AT_TIME_SYNCHRONIZATION = 2443144.5003725; + +//! Julian day at which TT, TCG, and TCB all show exact same time (1977 January 1, 00:00:32.184), in long double precision. +const static long double TAI_JULIAN_DAY_AT_TIME_SYNCHRONIZATION_LONG = 2443144.5003725L; + +//! Function to get the synchronization Julian day of TT, TCG, and TCB. +/*! + * Function to get the synchronization Julian day of TT, TCG, and TCB, in the requested time representation type + * \return Synchronization Julian day of TT, TCG, and TCB. + */ +template< typename TimeType > +TimeType getTimeOfTaiSynchronizationJulianDay( ); + +//! Function to get the synchronization time of TT, TCG, and TCB, in seconds since J2000. +/*! + * Function to get the synchronization time of TT, TCG, and TCB, in seconds since J2000 (constant by definition). + * \return Synchronization time of TT, TCG, and TCB, in seconds since J2000. + */ +template< typename TimeType > +TimeType getTimeOfTaiSynchronizationSinceJ2000( ) +{ + return ( getTimeOfTaiSynchronizationJulianDay< TimeType >( ) - getJulianDayOnJ2000< TimeType >( ) ) * + physical_constants::getJulianDay< TimeType >( ); +} + +//! Difference between TDB and (TT, TCB and TCB) at TAI_JULIAN_DAY_AT_TIME_SYNCHRONIZATION +const static double TDB_SECONDS_OFFSET_AT_SYNCHRONIZATION = -6.55E-5; + +//! Difference between TDB and (TT, TCB and TCB) at TAI_JULIAN_DAY_AT_TIME_SYNCHRONIZATION, in the requested time type +const static double TDB_SECONDS_OFFSET_AT_SYNCHRONIZATION_LONG = -6.55E-5L; + +//! Function to get the difference between TDB and (TT, TCB and TCB) at TAI_JULIAN_DAY_AT_TIME_SYNCHRONIZATION +/*! + * Function to get the difference between TDB and (TT, TCB and TCB) at TAI_JULIAN_DAY_AT_TIME_SYNCHRONIZATION, + * in the requested time representation type + * \return Difference between TDB and (TT, TCB and TCB) at TAI_JULIAN_DAY_AT_TIME_SYNCHRONIZATION (constant by definition). + */ +template< typename TimeType > +TimeType getTdbSecondsOffsetAtSynchronization( ); + +//! Offset of TT from TAI (constant by definition). +const static double TT_MINUS_TAI = 32.184; + +//! Offset of TT from TAI (constant by definition), in long double precision. +const static double TT_MINUS_TAI_LONG = 32.184L; + +//! Function to get the offset of TT from TAI (constant by definition) +/*! + * Function to get the offset of TT from TAI (constant by definition), in the requested time representation type + * \return Offset of TT from TAI (constant by definition). + */ +template< typename TimeType > +TimeType getTTMinusTai( ); + //! Compute number of seconds since a reference Julian day. /*! * Computes the number of seconds since a reference Julian day from @@ -60,8 +137,13 @@ const static double JULIAN_DAY_AT_0_MJD = 2400000.5; * \param epochSinceJulianDayZero Epoch in Julian day. * \return Number of seconds since epoch. */ -double convertJulianDayToSecondsSinceEpoch( const double julianDay, - const double epochSinceJulianDayZero ); +template< typename TimeScalarType = double > +TimeScalarType convertJulianDayToSecondsSinceEpoch( + const TimeScalarType julianDay, + const TimeScalarType epochSinceJulianDayZero = getJulianDayOnJ2000< TimeScalarType >( ) ) +{ + return ( julianDay - epochSinceJulianDayZero ) * physical_constants::getJulianDay< TimeScalarType >( ); +} //! Compute Julian day from seconds since reference Julian day epoch. /*! @@ -70,9 +152,14 @@ double convertJulianDayToSecondsSinceEpoch( const double julianDay, * \param epochSinceJulianDayZero Epoch in Julian day. * \return Number of Julian days since epoch. */ -double convertSecondsSinceEpochToJulianDay( - const double secondsSinceEpoch, - const double epochSinceJulianDayZero = JULIAN_DAY_ON_J2000 ); +template< typename TimeScalarType = double > +TimeScalarType convertSecondsSinceEpochToJulianDay( + const TimeScalarType secondsSinceEpoch, + const TimeScalarType epochSinceJulianDayZero = getJulianDayOnJ2000< TimeScalarType >( ) ) +{ + return ( secondsSinceEpoch / physical_constants::getJulianDay< TimeScalarType >( ) + epochSinceJulianDayZero ); +} + //! Compute Julian day from given date and time /*! @@ -85,26 +172,283 @@ double convertSecondsSinceEpochToJulianDay( * \param calendarHour Hour of the time of this day in hours. * \param calendarMinutes Minutes of the time of this day in minutes. * \param calendarSeconds Seconds of the time of this day in seconds. + * \param referenceJulianDay Reference Julian day (i.e. t=0) for input time. + * \return Seconds since referenceJulianDay for input date/time. */ -double convertCalendarDateToJulianDay( const int calendarYear, - const int calendarMonth, - const int calendarDay, - const int calendarHour, - const int calendarMinutes, - const double calendarSeconds ); +template< typename TimeScalarType = double > +TimeScalarType convertCalendarDateToJulianDaysSinceEpoch( const int calendarYear, + const int calendarMonth, + const int calendarDay, + const int calendarHour, + const int calendarMinutes, + const TimeScalarType calendarSeconds, + const TimeScalarType referenceJulianDay ) +{ + // Calculate julian day of calendar date. + TimeScalarType julianDay = + static_cast< TimeScalarType >( boost::gregorian::date( calendarYear, calendarMonth, calendarDay ). + julian_day( ) ) - referenceJulianDay; + + //Compute day fraction + const TimeScalarType dayFraction = + static_cast< TimeScalarType >( calendarHour ) / + mathematical_constants::getFloatingInteger< TimeScalarType >( 24 ) + + static_cast< TimeScalarType >( calendarMinutes ) / + mathematical_constants::getFloatingInteger< TimeScalarType >( 24 * 60 ) + + calendarSeconds / mathematical_constants::getFloatingInteger< TimeScalarType >( 24 * 3600 ); + + // Compute Julian day by adding day fraction and subtracting 0.5 to reference to midnight instead of noon.. + return julianDay + dayFraction - mathematical_constants::getFloatingFraction< TimeScalarType >( 1, 2 ); +} + +//! Compute Julian day from given date and time +/*! + * Computes the Julian day from given year, month, day, hour, minutes, seconds as used in everyday + * life. The function uses the internal calcualtions of the boost::date_time::gregorian class. + * + * \param calendarYear Year of the standard calendar in years. + * \param calendarMonth Month of the standard calendar in months. + * \param calendarDay Day of the standard calendar in days. + * \param calendarHour Hour of the time of this day in hours. + * \param calendarMinutes Minutes of the time of this day in minutes. + * \param calendarSeconds Seconds of the time of this day in seconds. + */ +template< typename TimeScalarType = double > +TimeScalarType convertCalendarDateToJulianDay( const int calendarYear, + const int calendarMonth, + const int calendarDay, + const int calendarHour, + const int calendarMinutes, + const TimeScalarType calendarSeconds ) +{ + return convertCalendarDateToJulianDaysSinceEpoch< TimeScalarType > + ( calendarYear, calendarMonth, calendarDay, calendarHour, calendarMinutes, + calendarSeconds, mathematical_constants::getFloatingInteger< TimeScalarType >( 0.0 ) ); +} + +//! Function to convert julian day to modified julian day. +/*! + * Function to convert julian day to modified julian day. + * \param julianDay Julian day to convert + * \return Modified julian day as obtained from julian day. + */ +template< typename TimeScalarType > +TimeScalarType convertJulianDayToModifiedJulianDay( const TimeScalarType julianDay ) +{ + return julianDay - getJulianDayOnMjd0< TimeScalarType >( ); +} + +//! Function to convert modified julian dat to julian day. +/*! + * Function to convert modified julian dat to julian day. + * \param modifiedJulianDay Modified julian day to convert + * \return Julian day as obtained from modified julian day. + */ +template< typename TimeScalarType > +TimeScalarType convertModifiedJulianDayToJulianDay( const TimeScalarType modifiedJulianDay ) +{ + return modifiedJulianDay + getJulianDayOnMjd0< TimeScalarType >( ); +} -//! Dummy time conversion function (time remains constant) +//! Function to convert the number of seconds since some reference julian day to a julian day since that epoch. /*! - * Dummy time conversion function (time remains constant) - * \param inputTime Time given as input. - * \return Same as inputTime. + * Function to convert the number of seconds since some reference julian day to a julian year since that epoch. + * \param secondsSinceEpoch Seconds since some reference epoch to convert to julian year. + * \return Number of julian years since epoch + */ +template< typename TimeScalarType = double > +TimeScalarType convertSecondsSinceEpochToJulianYearsSinceEpoch( const TimeScalarType secondsSinceEpoch ) +{ + return secondsSinceEpoch / ( physical_constants::getJulianDay< TimeScalarType >( ) * + physical_constants::getJulianYearInDays< TimeScalarType >( ) ); +} + +//! Function to convert the number of seconds since some reference julian day to a julian century since that epoch. +/*! + * Function to convert the number of seconds since some reference julian day to a julian century since that epoch. + * \param secondsSinceEpoch Seconds since some reference epoch to convert to julian year. + * \return Number of julian centuries since epoch + */ +template< typename TimeScalarType = double > +TimeScalarType convertSecondsSinceEpochToJulianCenturiesSinceEpoch( const TimeScalarType secondsSinceEpoch ) +{ + return convertSecondsSinceEpochToJulianYearsSinceEpoch( secondsSinceEpoch ) / 100.0; +} + +//! Function to convert julian day to gregorian calendar date. +/*! + * Function to convert julian day to gregorian calendar date. Algorithm from (Wertz, 2009, Eq. 4.3) + * \param julianDay Julian day to convert to gregorian calendar date. + * \return Gregorian calendar date at given julian day. + */ +boost::gregorian::date convertJulianDayToCalendarDate( const double julianDay ); + +//! Function to convert gregorian calendar date and fraction of day to a number of julian days since a reference epoch. +/*! + * Function to convert gregorian calendar date and fraction of day to a number of julian days since a reference epoch. + * \param calendarDate Calendar date at which conversion is to be performed. + * \param fractionOfDay Fraction of day in current gregorian date. + * \param epochSinceJulianDayZero Reference epoch since which number of julian days is to be determined. + * \return Number of julian days since a reference epoch. + */ +template< typename TimeScalarType > +TimeScalarType calculateJulianDaySinceEpoch( const boost::gregorian::date calendarDate, + const TimeScalarType fractionOfDay, + const TimeScalarType epochSinceJulianDayZero = JULIAN_DAY_ON_J2000 ) +{ + TimeScalarType julianDayAtMiddleOfDay = static_cast< TimeScalarType >( calendarDate.julian_day( ) ); + return julianDayAtMiddleOfDay + + ( fractionOfDay - mathematical_constants::getFloatingFraction< TimeScalarType >( 1, 2 ) ) - + epochSinceJulianDayZero; +} + +//! Function to determine whether the given year is a leap year (i.e. has 366 days) +/*! + * Function to determine whether the given year is a leap year (i.e. has 366 days) + * \param year Year for which it is to be determined whether it is a leap year. + * \return True if year is a leap year, false otherwise. + */ +bool isLeapYear( const int year ); + +//! Function that returns number of days in given month number +/*! + * Function that returns number of days in given month number (January = 1, December = 12) + * \param month Month number + * \param year Year + * \return Number of days in given month + */ +int getDaysInMonth( const int month, const int year ); + +//! Determine number of full days that have passed in current year +/*! + * Determine number of full days that have passed in current year (i.e. 0 to 364 or 0 to 365 for leap year) + * This function creates a boost::gregorian::date object and calls the overloaded function. + * \param day Day in current year (1 is first day) + * \param month Month in current year (1 is first month) + * \param year Current year (for leap year purposes) + * \return Number of full days that have passed in current year + */ +int convertDayMonthYearToDayOfYear( const int day, + const int month, + const int year ); + +//! Determine number of full days that have passed in current year +/*! + * Determine number of full days that have passed in current year (i.e. 0 to 364 or 0 to 365 for leap year) + * \param calendarDate Gregorian calendat date object containing current date information. + * \return Number of full days that have passed in current year + */ +int convertDayMonthYearToDayOfYear( const boost::gregorian::date calendarDate ); + +//! Determine number of seconds into current day of given time +/*! + * Determine number of seconds into current day of given time + * \param julianDay Current julian day + * \return Number of seconds into current day. + */ +double calculateSecondsInCurrentJulianDay( const double julianDay ); + +//! Function to create the calendar date from the year and the number of days in the year +/*! + * Function to create the calendar date from the year and the number of days in the year, where Jan. 1 is day in year 0. + * \param year Year in which days in year are given + * \param daysInYear Number of days in current year, note that day must be in current year (i.e <= 365 of 366 for leap year) + * \return Gregorian date object as generated from input + */ +boost::gregorian::date convertYearAndDaysInYearToDate( const int year, const int daysInYear ); + +//! Function to convert TCB to TDB times scale +/*! + * Function to convert TCB to TDB times scale, with both input and output referenced to the J2000 reference time. + * \param tcbTime Input time in TCB scale + * \return Converted time in TDB scale + */ +template< typename TimeType > +TimeType convertTcbToTdb( const TimeType tcbTime ) +{ + return tcbTime + getTdbSecondsOffsetAtSynchronization< TimeType >( ) - + physical_constants::getLbTimeRateTerm< TimeType >( ) * + ( tcbTime - getTimeOfTaiSynchronizationSinceJ2000< TimeType >( ) ); +} + +//! Function to convert TDB to TCB times scale +/*! + * Function to convert TDB to TCB times scale, with both input and output referenced to the J2000 reference time. + * \param tdbTime Input time in TDB scale + * \return Converted time in TCB scale + */ +template< typename TimeType > +TimeType convertTdbToTcb( const TimeType tdbTime ) +{ + return ( tdbTime - getTdbSecondsOffsetAtSynchronization< TimeType >( ) - + physical_constants::getLbTimeRateTerm< TimeType >( ) * + getTimeOfTaiSynchronizationSinceJ2000< TimeType >( ) ) / + ( mathematical_constants::getFloatingInteger< TimeType >( 1 ) - + physical_constants::getLbTimeRateTerm< TimeType >( ) ); +} + +//! Function to convert TCG to TT times scale +/*! + * Function to convert TCG to TT times scale, with both input and output referenced to the J2000 reference time. + * \param tcgTime Input time in TCG scale + * \return Converted time in TT scale */ template< typename TimeType > -TimeType doDummyTimeConversion( const TimeType inputTime ) +TimeType convertTcgToTt( const TimeType tcgTime ) { - return inputTime; + return tcgTime - physical_constants::getLgTimeRateTerm< TimeType >( ) * + ( tcgTime - getTimeOfTaiSynchronizationSinceJ2000< TimeType >( ) ); } +//! Function to convert TT to TCG times scale +/*! + * Function to convert TT to TCG times scale, with both input and output referenced to the J2000 reference time. + * \param ttTime Input time in TT scale + * \return Converted time in TCG scale + */ +template< typename TimeType > +TimeType convertTtToTcg( const TimeType ttTime ) +{ + return ttTime + physical_constants::getLgTimeRateTerm< TimeType >( ) + / ( mathematical_constants::getFloatingInteger< TimeType >( 1 ) - + physical_constants::getLgTimeRateTerm< TimeType >( ) ) * + ( ttTime - getTimeOfTaiSynchronizationSinceJ2000< TimeType >( ) ); +} + +//! Function to convert TAI to TT +/*! + * Function to convert TAI (International Atomic Time) to TT (Terrestrial Time) by adding bias as defined by Sofa. + * \param taiTime Time in TAI + * \return Time in TT + */ +template< typename TimeType > +TimeType convertTAItoTT( const TimeType taiTime ) +{ + return taiTime + getTTMinusTai< TimeType >( ); +} + + +//! Function to convert TT to TAI +/*! + * Function to convert TT (Terrestrial Time) to TAI (International Atomic Time) by subtracting bias as defined by Sofa. + * \param ttTime Time in TT + * \return Time in TAI + */ +template< typename TimeType > +TimeType convertTTtoTAI( const TimeType ttTime ) +{ + return ttTime - getTTMinusTai< TimeType >( ); +} + +//! Perform apprixmate conversion of TT to TDB +/*! + * Perform apprixmate conversion of TT to TDB, in which only the once-per-orbit sinusoidal effect of O(e) is taken into + * account. Accurate conversions are calculated usinf Sofa + * \param ttSecondsSinceJ2000 Terrestrial time in seconds since J2000 + * \return TDB in seconds since J2000 + */ +double approximateConvertTTtoTDB( const double ttSecondsSinceJ2000); + } // namespace basic_astrodynamics } // tudat diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/unifiedStateModelElementConversions.h b/Tudat/Astrodynamics/BasicAstrodynamics/unifiedStateModelElementConversions.h index 618b3094ab..f66d400a35 100644 --- a/Tudat/Astrodynamics/BasicAstrodynamics/unifiedStateModelElementConversions.h +++ b/Tudat/Astrodynamics/BasicAstrodynamics/unifiedStateModelElementConversions.h @@ -93,7 +93,7 @@ basic_mathematics::Vector6d convertUnifiedStateModelToKeplerianElements( const Eigen::Matrix< double, 7, 1 >& unifiedStateModelElements, const double centralBodyGravitationalParameter ); -} // close namespace orbital_element_conversion +} // namespace orbital_element_conversions } // close tudat diff --git a/Tudat/Astrodynamics/CMakeLists.txt b/Tudat/Astrodynamics/CMakeLists.txt index 049c7df060..5f6a5a87b8 100755 --- a/Tudat/Astrodynamics/CMakeLists.txt +++ b/Tudat/Astrodynamics/CMakeLists.txt @@ -44,8 +44,11 @@ set(GRAVITATIONDIR "${ASTRODYNAMICSDIR}/Gravitation") set(MISSIONSEGMENTSDIR "${ASTRODYNAMICSDIR}/MissionSegments") set(REFERENCEFRAMESDIR "${ASTRODYNAMICSDIR}/ReferenceFrames") set(OBSERVATIONMODELSDIR "${ASTRODYNAMICSDIR}/ObservationModels") -set(STATEDERIVATIVEMODELSDIR "${ASTRODYNAMICSDIR}/StateDerivativeModels") +set(ORBITDETERMINATIONDIR "${ASTRODYNAMICSDIR}/OrbitDetermination") set(PROPAGATORSDIR "${ASTRODYNAMICSDIR}/Propagators") +set(RELATIVITYDIR "${ASTRODYNAMICSDIR}/Relativity") +set(SYSTEMMODELSDIR "${ASTRODYNAMICSDIR}/SystemModels") +set(PROPULSIONDIR "${ASTRODYNAMICSDIR}/Propulsion") # Add subdirectories. add_subdirectory("${SRCROOT}${AERODYNAMICSDIR}") @@ -56,8 +59,12 @@ add_subdirectory("${SRCROOT}${GRAVITATIONDIR}") add_subdirectory("${SRCROOT}${MISSIONSEGMENTSDIR}") add_subdirectory("${SRCROOT}${REFERENCEFRAMESDIR}") add_subdirectory("${SRCROOT}${OBSERVATIONMODELSDIR}") -add_subdirectory("${SRCROOT}${STATEDERIVATIVEMODELSDIR}") +add_subdirectory("${SRCROOT}${ORBITDETERMINATIONDIR}") add_subdirectory("${SRCROOT}${PROPAGATORSDIR}") +add_subdirectory("${SRCROOT}${RELATIVITYDIR}") +add_subdirectory("${SRCROOT}${SYSTEMMODELSDIR}") +add_subdirectory("${SRCROOT}${PROPULSIONDIR}") + # Get target properties for static libraries. get_target_property(AERODYNAMICSSOURCES tudat_aerodynamics SOURCES) @@ -68,5 +75,8 @@ get_target_property(GRAVITATIONSOURCES tudat_gravitation SOURCES) get_target_property(MISSIONSEGMENTSSOURCES tudat_mission_segments SOURCES) get_target_property(REFERENCEFRAMESSOURCES tudat_reference_frames SOURCES) get_target_property(OBSERVATIONMODELSDIR tudat_observation_models SOURCES) -get_target_property(STATEDERIVATIVEMODELSSOURCES tudat_state_derivative_models SOURCES) +get_target_property(ORBITDETERMINATIONSOURCES tudat_orbit_determination SOURCES) +get_target_property(RELATIVITYSOURCES tudat_relativity SOURCES) get_target_property(PROPAGATORSDIR tudat_propagators SOURCES) +get_target_property(SYSTEMMODELSSOURCES tudat_system_models SOURCES) +get_target_property(PROPULSIONSOURCES tudat_propulsion SOURCES) diff --git a/Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.cpp b/Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.cpp index 80062f73b2..bac2c14b05 100644 --- a/Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.cpp +++ b/Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.cpp @@ -57,27 +57,5 @@ Eigen::Vector3d computeCannonBallRadiationPressureAcceleration( radiationPressure, vectorToSource, area, radiationPressureCoefficient ) / mass; } -//! Get radiation pressure acceleration. -Eigen::Vector3d CannonBallRadiationPressureAcceleration::getAcceleration( ) -{ - return computeCannonBallRadiationPressureAcceleration( - currentRadiationPressure_, currentVectorToSource_, currentArea_, - currentRadiationPressureCoefficient_, currentMass_ ); -} - -//! Update member variables used by the radiation pressure acceleration model. -void CannonBallRadiationPressureAcceleration::updateMembers( const double currentTime ) -{ - if( !( this->currentTime_ == currentTime ) ) - { - currentVectorToSource_ = ( sourcePositionFunction_( ) - - acceleratedBodyPositionFunction_( ) ).normalized( ); - currentRadiationPressure_ = radiationPressureFunction_( ); - currentRadiationPressureCoefficient_ = radiationPressureCoefficientFunction_( ); - currentArea_ = areaFunction_( ); - currentMass_ = massFunction_( ); - } -} - } // namespace electro_magnetism } // namespace tudat diff --git a/Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.h b/Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.h index ca7c9d4dd9..77b90fbb26 100644 --- a/Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.h +++ b/Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.h @@ -172,7 +172,12 @@ class CannonBallRadiationPressureAcceleration: public basic_astrodynamics::Accel * \return Radiation pressure acceleration. * \sa computeCannonBallRadiationPressureAcceleration(). */ - Eigen::Vector3d getAcceleration( ); + Eigen::Vector3d getAcceleration( ) + { + return computeCannonBallRadiationPressureAcceleration( + currentRadiationPressure_, currentVectorToSource_, currentArea_, + currentRadiationPressureCoefficient_, currentMass_ ); + } //! Update member variables used by the radiation pressure acceleration model. /*! @@ -181,7 +186,28 @@ class CannonBallRadiationPressureAcceleration: public basic_astrodynamics::Accel * not the function-pointers are then used by the getAcceleration( ) function. * \param currentTime Time at which acceleration model is to be updated. */ - void updateMembers( const double currentTime = TUDAT_NAN ); + void updateMembers( const double currentTime = TUDAT_NAN ) + { + if( !( this->currentTime_ == currentTime ) ) + { + currentVectorToSource_ = ( sourcePositionFunction_( ) + - acceleratedBodyPositionFunction_( ) ).normalized( ); + currentRadiationPressure_ = radiationPressureFunction_( ); + currentRadiationPressureCoefficient_ = radiationPressureCoefficientFunction_( ); + currentArea_ = areaFunction_( ); + currentMass_ = massFunction_( ); + } + } + + //! Function to retrieve the function pointer returning mass of accelerated body. + /*! + * Function to retrieve the function pointer returning mass of accelerated body. + * \return Function pointer returning mass of accelerated body. + */ + boost::function< double( ) > getMassFunction( ) + { + return massFunction_; + } private: diff --git a/Tudat/Astrodynamics/ElectroMagnetism/radiationPressureInterface.h b/Tudat/Astrodynamics/ElectroMagnetism/radiationPressureInterface.h index 8d96d9ad13..b800bea79d 100644 --- a/Tudat/Astrodynamics/ElectroMagnetism/radiationPressureInterface.h +++ b/Tudat/Astrodynamics/ElectroMagnetism/radiationPressureInterface.h @@ -175,6 +175,16 @@ class RadiationPressureInterface{ return radiationPressureCoefficient_; } + //! Function to reset the radiation pressure coefficient of the target body. + /*! + * Function to reset the radiation pressure coefficient of the target body. + * \param radiationPressureCoefficient The new radiation pressure coefficient of the target body. + */ + void resetRadiationPressureCoefficient( const double radiationPressureCoefficient ) + { + radiationPressureCoefficient_ = radiationPressureCoefficient; + } + //! Function to return the function returning the current total power (in W) emitted by the //! source body. /*! diff --git a/Tudat/Astrodynamics/Ephemerides/CMakeLists.txt b/Tudat/Astrodynamics/Ephemerides/CMakeLists.txt index 10f90e38e1..406da5049f 100644 --- a/Tudat/Astrodynamics/Ephemerides/CMakeLists.txt +++ b/Tudat/Astrodynamics/Ephemerides/CMakeLists.txt @@ -101,12 +101,10 @@ if(USE_CSPICE) add_executable(test_FrameManager "${SRCROOT}${EPHEMERIDESDIR}/UnitTests/unitTestFrameManager.cpp") setup_custom_test_program(test_FrameManager "${SRCROOT}${EPHEMERIDESDIR}") target_link_libraries(test_FrameManager tudat_ephemerides tudat_reference_frames tudat_input_output tudat_basic_astrodynamics tudat_basic_mathematics ${TUDAT_EXTERNAL_LIBRARIES} ${Boost_LIBRARIES}) -endif() -if(USE_CSPICE) add_executable(test_CompositeEphemeris "${SRCROOT}${EPHEMERIDESDIR}/UnitTests/unitTestCompositeEphemeris.cpp") setup_custom_test_program(test_CompositeEphemeris "${SRCROOT}${EPHEMERIDESDIR}") -target_link_libraries(test_CompositeEphemeris tudat_simulation_setup tudat_electro_magnetism tudat_gravitation tudat_ephemerides tudat_reference_frames tudat_aerodynamics tudat_reference_frames tudat_input_output tudat_basic_astrodynamics tudat_basic_mathematics ${TUDAT_EXTERNAL_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(test_CompositeEphemeris ${TUDAT_PROPAGATION_LIBRARIES} ${Boost_LIBRARIES}) endif() add_executable(test_KeplerEphemeris "${SRCROOT}${EPHEMERIDESDIR}/UnitTests/unitTestKeplerEphemeris.cpp") diff --git a/Tudat/Astrodynamics/Ephemerides/UnitTests/unitTestCompositeEphemeris.cpp b/Tudat/Astrodynamics/Ephemerides/UnitTests/unitTestCompositeEphemeris.cpp index 7e422b9e01..ecd60bd1c2 100644 --- a/Tudat/Astrodynamics/Ephemerides/UnitTests/unitTestCompositeEphemeris.cpp +++ b/Tudat/Astrodynamics/Ephemerides/UnitTests/unitTestCompositeEphemeris.cpp @@ -1,3 +1,13 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + #define BOOST_TEST_MAIN #include @@ -13,9 +23,9 @@ #include "Tudat/External/SpiceInterface/spiceInterface.h" #include "Tudat/InputOutput/basicInputOutput.h" -#include "Tudat/SimulationSetup/body.h" -#include "Tudat/SimulationSetup/defaultBodies.h" -#include "Tudat/SimulationSetup/createBodies.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h" namespace tudat @@ -67,54 +77,76 @@ BOOST_AUTO_TEST_CASE( testCompositeEphemeris ) double buffer = 5.0 * maximumTimeStep; // Create bodies needed in simulation - std::map< std::string, boost::shared_ptr< Body > > bodyMap = createBodies( + NamedBodyMap bodyMap = createBodies( getDefaultBodySettings( bodyNames,initialEphemerisTime - buffer, finalEphemerisTime + buffer ) ); setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" ); + // Retrieve Earth state/rotation objects boost::shared_ptr< Ephemeris > earthEphemeris = bodyMap.at( "Earth" )->getEphemeris( ); boost::shared_ptr< RotationalEphemeris > rotationModel = bodyMap.at( "Earth" )->getRotationalEphemeris( ); + // Create reference point CompositeEphemeris objects (double and long double state scalars). boost::shared_ptr< Ephemeris > ephemeris1 = createReferencePointEphemeris< double, double >( earthEphemeris, rotationModel, &getGroundStationPosition ); boost::shared_ptr< Ephemeris > ephemeris2 = createReferencePointEphemeris< double, long double >( earthEphemeris, rotationModel, &getGroundStationPosition ); double testTime = 1.05E7; + // Manually compute double state basic_mathematics::Vector6d doubleStateFromDoubleTime; doubleStateFromDoubleTime.segment( 0, 3 ) = earthEphemeris->getCartesianStateFromEphemeris( testTime ).segment( 0, 3 ) + rotationModel->getRotationToBaseFrame( testTime ) * getGroundStationPosition( testTime ).segment( 0, 3 ); doubleStateFromDoubleTime.segment( 3, 3 ) = earthEphemeris->getCartesianStateFromEphemeris( testTime ).segment( 3, 3 ) + rotationModel->getRotationToBaseFrame( testTime ) * getGroundStationPosition( testTime ).segment( 3, 3 ) + - rotationModel->getDerivativeOfRotationToBaseFrame( testTime ) * getGroundStationPosition( testTime ).segment( 0, 3 ); + rotationModel->getDerivativeOfRotationToBaseFrame( testTime ) * + getGroundStationPosition( testTime ).segment( 0, 3 ); + // Manually compute long double state Eigen::Matrix< long double, 6, 1 > longDoubleStateFromDoubleTime; - longDoubleStateFromDoubleTime.segment( 0, 3 ) = earthEphemeris->getCartesianLongStateFromEphemeris( testTime ).segment( 0, 3 ) + - ( rotationModel->getRotationToBaseFrame( testTime ) * getGroundStationPosition( testTime ).segment( 0, 3 ) ).cast< long double >( ); - longDoubleStateFromDoubleTime.segment( 3, 3 ) = earthEphemeris->getCartesianLongStateFromEphemeris( testTime ).segment( 3, 3 ) + - ( rotationModel->getRotationToBaseFrame( testTime ) * getGroundStationPosition( testTime ).segment( 3, 3 ) ).cast< long double >( ) + - ( rotationModel->getDerivativeOfRotationToBaseFrame( testTime ) * getGroundStationPosition( testTime ).segment( 0, 3 ) ).cast< long double >( ); + longDoubleStateFromDoubleTime.segment( 0, 3 ) = + earthEphemeris->getCartesianLongStateFromEphemeris( testTime ).segment( 0, 3 ) + + ( rotationModel->getRotationToBaseFrame( testTime ) * + getGroundStationPosition( testTime ).segment( 0, 3 ) ).cast< long double >( ); + longDoubleStateFromDoubleTime.segment( 3, 3 ) = + earthEphemeris->getCartesianLongStateFromEphemeris( testTime ).segment( 3, 3 ) + + ( rotationModel->getRotationToBaseFrame( testTime ) * + getGroundStationPosition( testTime ).segment( 3, 3 ) ).cast< long double >( ) + + ( rotationModel->getDerivativeOfRotationToBaseFrame( testTime ) * + getGroundStationPosition( testTime ).segment( 0, 3 ) ).cast< long double >( ); double doubleTolerance = std::numeric_limits< double >::epsilon( ); double longDoubleTolerance = std::numeric_limits< long double >::epsilon( ); - //NOTE: Make test robust for smaller entries 2 and 5 - - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( doubleStateFromDoubleTime, longDoubleStateFromDoubleTime.cast< double >( ), doubleTolerance ); - - - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( longDoubleStateFromDoubleTime.cast< double >( ), doubleStateFromDoubleTime, doubleTolerance ); - - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( ephemeris1->getCartesianStateFromEphemeris( testTime ), doubleStateFromDoubleTime, doubleTolerance ); - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( ephemeris1->getCartesianLongStateFromEphemeris( testTime ), doubleStateFromDoubleTime.cast< long double >( ), doubleTolerance ); - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( ephemeris2->getCartesianStateFromEphemeris( testTime ), doubleStateFromDoubleTime, doubleTolerance ); - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( ephemeris2->getCartesianLongStateFromEphemeris( testTime ).segment( 0, 2 ), - longDoubleStateFromDoubleTime.segment( 0, 2 ), longDoubleTolerance ); - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( ephemeris2->getCartesianLongStateFromEphemeris( testTime ).segment( 2, 1 ), - longDoubleStateFromDoubleTime.segment( 2, 1 ), ( 1000.0 * longDoubleTolerance ) ); - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( ephemeris2->getCartesianLongStateFromEphemeris( testTime ).segment( 3, 2 ), - longDoubleStateFromDoubleTime.segment( 3, 2 ), ( 10.0 * longDoubleTolerance ) );//Tolerance is not fully met because rotation is in double - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( ephemeris2->getCartesianLongStateFromEphemeris( testTime ).segment( 5, 1 ), - longDoubleStateFromDoubleTime.segment( 5, 1 ), ( 10000.0 * longDoubleTolerance ) ); + // Test consistency of manual functions + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + doubleStateFromDoubleTime, longDoubleStateFromDoubleTime.cast< double >( ), doubleTolerance ); + + // Test double composte ephemeris + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ephemeris1->getCartesianStateFromEphemeris( testTime ), + doubleStateFromDoubleTime, doubleTolerance ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ephemeris1->getCartesianLongStateFromEphemeris( testTime ), + doubleStateFromDoubleTime.cast< long double >( ), doubleTolerance ); + + // Test long double composte ephemeris, tolerances are not fully met as longDoubleTolerance because rotation is only + // defined using double state scalars. This is especially influential for the z-components, where the nominal value + // is much smaller. + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ephemeris2->getCartesianStateFromEphemeris( testTime ), + doubleStateFromDoubleTime, doubleTolerance ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ephemeris2->getCartesianLongStateFromEphemeris( testTime ).segment( 0, 2 ), + longDoubleStateFromDoubleTime.segment( 0, 2 ), longDoubleTolerance ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ephemeris2->getCartesianLongStateFromEphemeris( testTime ).segment( 2, 1 ), + longDoubleStateFromDoubleTime.segment( 2, 1 ), ( 1000.0 * longDoubleTolerance ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ephemeris2->getCartesianLongStateFromEphemeris( testTime ).segment( 3, 2 ), + longDoubleStateFromDoubleTime.segment( 3, 2 ), ( 10.0 * longDoubleTolerance ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ephemeris2->getCartesianLongStateFromEphemeris( testTime ).segment( 5, 1 ), + longDoubleStateFromDoubleTime.segment( 5, 1 ), ( 10000.0 * longDoubleTolerance ) ); } BOOST_AUTO_TEST_SUITE_END( ) diff --git a/Tudat/Astrodynamics/Ephemerides/UnitTests/unitTestFrameManager.cpp b/Tudat/Astrodynamics/Ephemerides/UnitTests/unitTestFrameManager.cpp index e070013c28..6d0fcb98d4 100644 --- a/Tudat/Astrodynamics/Ephemerides/UnitTests/unitTestFrameManager.cpp +++ b/Tudat/Astrodynamics/Ephemerides/UnitTests/unitTestFrameManager.cpp @@ -1,3 +1,13 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + #define BOOST_TEST_MAIN #include diff --git a/Tudat/Astrodynamics/Ephemerides/UnitTests/unitTestKeplerEphemeris.cpp b/Tudat/Astrodynamics/Ephemerides/UnitTests/unitTestKeplerEphemeris.cpp index 5942811209..641f354753 100644 --- a/Tudat/Astrodynamics/Ephemerides/UnitTests/unitTestKeplerEphemeris.cpp +++ b/Tudat/Astrodynamics/Ephemerides/UnitTests/unitTestKeplerEphemeris.cpp @@ -1,35 +1,11 @@ -/* Copyright (c) 2010-2014, Delft University of Technology - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, are - * permitted provided that the following conditions are met: - * - Redistributions of source code must retain the above copyright notice, this list of - * conditions and the following disclaimer. - * - Redistributions in binary form must reproduce the above copyright notice, this list of - * conditions and the following disclaimer in the documentation and/or other materials - * provided with the distribution. - * - Neither the name of the Delft University of Technology nor the names of its contributors - * may be used to endorse or promote products derived from this software without specific - * prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS - * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE - * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING - * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED - * OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Changelog - * YYMMDD Author Comment - * 150411 D. Dirkx Migrated and updated from personal code. - * - * References - * - * Notes +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. */ #define BOOST_TEST_MAIN diff --git a/Tudat/Astrodynamics/Ephemerides/approximatePlanetPositionsCircularCoplanar.cpp b/Tudat/Astrodynamics/Ephemerides/approximatePlanetPositionsCircularCoplanar.cpp index 3e336c0780..8023f7179c 100644 --- a/Tudat/Astrodynamics/Ephemerides/approximatePlanetPositionsCircularCoplanar.cpp +++ b/Tudat/Astrodynamics/Ephemerides/approximatePlanetPositionsCircularCoplanar.cpp @@ -37,6 +37,7 @@ * */ +#include #include #include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h" diff --git a/Tudat/Astrodynamics/Ephemerides/approximatePlanetPositionsDataContainer.h b/Tudat/Astrodynamics/Ephemerides/approximatePlanetPositionsDataContainer.h index 5286f95cc4..84925f419d 100644 --- a/Tudat/Astrodynamics/Ephemerides/approximatePlanetPositionsDataContainer.h +++ b/Tudat/Astrodynamics/Ephemerides/approximatePlanetPositionsDataContainer.h @@ -93,48 +93,47 @@ struct ApproximatePlanetPositionsDataContainer ApproximatePlanetPositionsDataContainer& approximatePlanetPositionsDataContainer ) { - using std::endl; - stream << "This is an ApproximatePlanetPositionsDataContainer object. " << endl; + stream << "This is an ApproximatePlanetPositionsDataContainer object. " << std::endl; stream << "The data corresponds to the table entry for " - << approximatePlanetPositionsDataContainer.planetName_ << endl; + << approximatePlanetPositionsDataContainer.planetName_ << std::endl; stream << "The semi-major axis in AU is set to: " - << approximatePlanetPositionsDataContainer.semiMajorAxis_ << endl; + << approximatePlanetPositionsDataContainer.semiMajorAxis_ << std::endl; stream << "The eccentricity in radians is set to: " - << approximatePlanetPositionsDataContainer.eccentricity_ << endl; + << approximatePlanetPositionsDataContainer.eccentricity_ << std::endl; stream << "The inclination in degrees is set to: " - << approximatePlanetPositionsDataContainer.inclination_ << endl; + << approximatePlanetPositionsDataContainer.inclination_ << std::endl; stream << "The mean longitude in degrees is set to: " - << approximatePlanetPositionsDataContainer.meanLongitude_ << endl; + << approximatePlanetPositionsDataContainer.meanLongitude_ << std::endl; stream << "The longitude of perihelion in degrees is set to: " - << approximatePlanetPositionsDataContainer.longitudeOfPerihelion_ << endl; + << approximatePlanetPositionsDataContainer.longitudeOfPerihelion_ << std::endl; stream << "The longitude of the ascending node in degrees is set to: " - << approximatePlanetPositionsDataContainer.longitudeOfAscendingNode_<< endl; + << approximatePlanetPositionsDataContainer.longitudeOfAscendingNode_<< std::endl; stream << "The rate of change of semi-major axis AU per century is set to: " - << approximatePlanetPositionsDataContainer.rateOfChangeOfSemiMajorAxis_ << endl; + << approximatePlanetPositionsDataContainer.rateOfChangeOfSemiMajorAxis_ << std::endl; stream << "The rate of change of eccentricity in radians per century is set to: " - << approximatePlanetPositionsDataContainer.rateOfChangeOfEccentricity_ << endl; + << approximatePlanetPositionsDataContainer.rateOfChangeOfEccentricity_ << std::endl; stream << "The rate of change of inclination in degrees per century is set to: " - << approximatePlanetPositionsDataContainer.rateOfChangeOfInclination_ << endl; + << approximatePlanetPositionsDataContainer.rateOfChangeOfInclination_ << std::endl; stream << "The rate of change of mean longitude in degrees per century is set to: " - << approximatePlanetPositionsDataContainer.rateOfChangeOfMeanLongitude_ << endl; + << approximatePlanetPositionsDataContainer.rateOfChangeOfMeanLongitude_ << std::endl; stream << "The rate of change of longitude of perihelion in degrees per century is set to: " - << approximatePlanetPositionsDataContainer.rateOfChangeOfLongitudeOfPerihelion_ << endl; + << approximatePlanetPositionsDataContainer.rateOfChangeOfLongitudeOfPerihelion_ << std::endl; stream << "The rate of change of longitude of ascending node in degrees per century is set " << "to: " << approximatePlanetPositionsDataContainer - .rateOfChangeOfLongitudeOfAscendingNode_ << endl; + .rateOfChangeOfLongitudeOfAscendingNode_ << std::endl; // Check if additional terms are defined for outer planets. if ( approximatePlanetPositionsDataContainer.additionalTermB_ != -0.0 ) { stream << "The additional term B is set to: " - << approximatePlanetPositionsDataContainer.additionalTermB_ << endl; + << approximatePlanetPositionsDataContainer.additionalTermB_ << std::endl; stream << "The additional term C is set to: " - << approximatePlanetPositionsDataContainer.additionalTermC_ << endl; + << approximatePlanetPositionsDataContainer.additionalTermC_ << std::endl; stream << "The additional term S is set to: " - << approximatePlanetPositionsDataContainer.additionalTermS_ << endl; + << approximatePlanetPositionsDataContainer.additionalTermS_ << std::endl; stream << "The additional term F is set to: " - << approximatePlanetPositionsDataContainer .additionalTermF_ << endl; + << approximatePlanetPositionsDataContainer .additionalTermF_ << std::endl; } // Return stream. diff --git a/Tudat/Astrodynamics/Ephemerides/compositeEphemeris.h b/Tudat/Astrodynamics/Ephemerides/compositeEphemeris.h index 0c973b075c..7c720c0f62 100644 --- a/Tudat/Astrodynamics/Ephemerides/compositeEphemeris.h +++ b/Tudat/Astrodynamics/Ephemerides/compositeEphemeris.h @@ -80,7 +80,8 @@ class CompositeEphemeris : public Ephemeris // Check whether chain starts with translation. if( translationIterator->first != 0 ) { - std::cerr << "Error, composite ephemeris must start with translation" << std::endl; + std::string errorMessage = "Error, composite ephemeris must start with translation"; + throw std::runtime_error( errorMessage ); } // Run over all indices and set order. @@ -109,7 +110,8 @@ class CompositeEphemeris : public Ephemeris // If index is not found, display error message. else { - std::cerr << "Error when making composite ephemeris, input indices inconsistent" << std::endl; + std::string errorMessage = "Error when making composite ephemeris, input indices inconsistentn"; + throw std::runtime_error( errorMessage ); } currentIndex++; } @@ -148,7 +150,8 @@ class CompositeEphemeris : public Ephemeris // Check whether chain starts with translation. if( translationIterator->first != 0 ) { - std::cerr << "Error, composite ephemeris must start with translation" << std::endl; + std::string errorMessage = "Error, composite ephemeris must start with translation"; + throw std::runtime_error( errorMessage ); } // Run over all indices and set order. @@ -180,7 +183,8 @@ class CompositeEphemeris : public Ephemeris // If index is not found, display error message. else { - std::cerr << "Error when making composite ephemeris, input indices inconsistent" << std::endl; + std::string errorMessage = "Error when making composite ephemeris, input indices inconsistent"; + throw std::runtime_error( errorMessage ); } currentIndex++; } @@ -223,7 +227,8 @@ class CompositeEphemeris : public Ephemeris //Check validity of input. if( add != 1 && add != -1 ) { - std::cerr << "Error when adding to composite ephemeris" << std::endl; + std::string errorMessage = "Error when adding to composite ephemeris"; + throw std::runtime_error( errorMessage ); } // Add translational ephemeris to end of chain @@ -252,6 +257,13 @@ class CompositeEphemeris : public Ephemeris std::vector< bool > isCurrentEphemerisTranslational_; }; +//! Interface function used to change the state scalar type of the output of a state function +/*! + * Interface function used to change the state scalar type of the output of a state function + * \param originalStateFunction State function with original scalar type + * \param currentTime Time at which state function is to be evaluated. + * \return State from originalStateFunction at currentTime, cast to NewStateScalarType. + */ template< typename OldStateScalarType, typename NewStateScalarType, typename TimeType, int StateSize > Eigen::Matrix< NewStateScalarType, StateSize, 1 > convertStateFunctionStateScalarOutput( const boost::function< Eigen::Matrix< OldStateScalarType, StateSize, 1 >( const double& ) > @@ -261,6 +273,17 @@ Eigen::Matrix< NewStateScalarType, StateSize, 1 > convertStateFunctionStateScala return originalStateFunction( currentTime ).template cast< NewStateScalarType >( ); } +//! Function to create the state function of a reference point. +/*! + * Function to create the state function of a reference point, for instance the state of a ground station in a + * barycentric frame. + * \param bodyEphemeris Global ephemeris of body on which reference point is located + * \param bodyRotationModel Rotation model between global and body-fixed frame + * \param referencePointRelativeStateFunction Function returning the body-fixed state of the reference point as + * a function of time + * \return Ephemeris describbing the state of the local reference point in the global frame (expressed global frame's + * coordinates, i.e. same as bodyEphemeris). + */ template< typename TimeType = double, typename StateScalarType = double > boost::shared_ptr< Ephemeris > createReferencePointEphemeris( boost::shared_ptr< Ephemeris > bodyEphemeris, @@ -269,6 +292,7 @@ boost::shared_ptr< Ephemeris > createReferencePointEphemeris( { typedef Eigen::Matrix< StateScalarType, 6, 1 > StateType; + // Cast state fucntion of body (global) and reference point (local) into correct form. std::map< int, boost::function< StateType( const TimeType& ) > > referencePointEphemerisVector; referencePointEphemerisVector[ 2 ] = boost::bind( &Ephemeris::getTemplatedStateFromEphemeris< StateScalarType, TimeType >, bodyEphemeris, _1 ); @@ -277,6 +301,7 @@ boost::shared_ptr< Ephemeris > createReferencePointEphemeris( referencePointRelativeStateFunction, _1 ); + // Crate rotation functions from local to global frame. boost::function< Eigen::Quaterniond( const double ) > rotationToFrameFunction = boost::bind( &RotationalEphemeris::getRotationToBaseFrame, bodyRotationModel, _1, basic_astrodynamics::JULIAN_DAY_ON_J2000 ); @@ -284,16 +309,14 @@ boost::shared_ptr< Ephemeris > createReferencePointEphemeris( boost::bind( &RotationalEphemeris::getDerivativeOfRotationToBaseFrame, bodyRotationModel, _1, basic_astrodynamics::JULIAN_DAY_ON_J2000 ); + // Create ephemeris std::map< int, boost::function< StateType( const double, const StateType& ) > > referencePointRotationVector; referencePointRotationVector[ 1 ] = boost::bind( transformStateToFrameFromRotationTimeFunctions< StateScalarType >, _2, _1, rotationToFrameFunction, rotationMatrixToFrameDerivativeFunction ); - boost::shared_ptr< Ephemeris > ephemeris - = boost::make_shared< CompositeEphemeris< TimeType, StateScalarType > >( + return boost::make_shared< CompositeEphemeris< TimeType, StateScalarType > >( referencePointEphemerisVector, referencePointRotationVector, "SSB", "ECLIPJ2000" ); - - return ephemeris; } } // namespace ephemerides diff --git a/Tudat/Astrodynamics/Ephemerides/ephemeris.cpp b/Tudat/Astrodynamics/Ephemerides/ephemeris.cpp index 3d1fd0c85e..3326399289 100644 --- a/Tudat/Astrodynamics/Ephemerides/ephemeris.cpp +++ b/Tudat/Astrodynamics/Ephemerides/ephemeris.cpp @@ -28,6 +28,14 @@ Eigen::Matrix< long double, 6, 1 > Ephemeris::getTemplatedStateFromEphemeris( co return getCartesianLongStateFromEphemeris( time, basic_astrodynamics::JULIAN_DAY_ON_J2000 ); } +//! Function to compute the relative state from two state functions. +void getRelativeState( + basic_mathematics::Vector6d& relativeState, + const boost::function< basic_mathematics::Vector6d( ) > stateFunctionOfBody, + const boost::function< basic_mathematics::Vector6d( ) > stateFunctionOfCentralBody ) +{ + relativeState = stateFunctionOfBody( ) - stateFunctionOfCentralBody( ); +} } // namespace ephemerides diff --git a/Tudat/Astrodynamics/Ephemerides/ephemeris.h b/Tudat/Astrodynamics/Ephemerides/ephemeris.h index 0e6e0d3284..1b428ecbeb 100644 --- a/Tudat/Astrodynamics/Ephemerides/ephemeris.h +++ b/Tudat/Astrodynamics/Ephemerides/ephemeris.h @@ -150,6 +150,20 @@ class Ephemeris //! Typedef for shared-pointer to Ephemeris object. typedef boost::shared_ptr< Ephemeris > EphemerisPointer; +//! Function to compute the relative state from two state functions. +/*! + * Function to compute the relative state from two state functions. + * \param relativeState Relative state returned by stateFunctionOfBody w.r.t. state returned by stateFunctionOfCentralBody + * (returned by reference). + * \param stateFunctionOfBody Function returning state of body for which relative state is to be computed. + * \param stateFunctionOfCentralBody Function returning state of central body w.r.t. which the relative state is to be + * computed. + */ +void getRelativeState( + basic_mathematics::Vector6d& relativeState, + const boost::function< basic_mathematics::Vector6d( ) > stateFunctionOfBody, + const boost::function< basic_mathematics::Vector6d( ) > stateFunctionOfCentralBody ); + } // namespace ephemerides } // namespace tudat diff --git a/Tudat/Astrodynamics/Ephemerides/frameManager.cpp b/Tudat/Astrodynamics/Ephemerides/frameManager.cpp index a99cfb9750..92b3f7eb44 100644 --- a/Tudat/Astrodynamics/Ephemerides/frameManager.cpp +++ b/Tudat/Astrodynamics/Ephemerides/frameManager.cpp @@ -35,28 +35,6 @@ bool isFrameInertial( const std::string& frame ) return isFrameInertial_; } - -//! Constructor from named list of bodies. -ReferenceFrameManager::ReferenceFrameManager( const simulation_setup::NamedBodyMap& bodyMap ) -{ - // Get ephemerides from bodies - std::map< std::string, boost::shared_ptr< Ephemeris > > ephemerides; - for( simulation_setup::NamedBodyMap::const_iterator bodyIterator = bodyMap.begin( ); - bodyIterator != bodyMap.end( ); bodyIterator++ ) - { - if( bodyIterator->second->getEphemeris( ) != NULL ) - { - ephemerides[ bodyIterator->first ] = bodyIterator->second->getEphemeris( ); - } - } - - // Set name of global base frame. - frameIndexList_[ getBaseFrameName( ) ] = -1; - - // Initialize frame book-keeping variables. - setEphemerides( ephemerides ); -} - //! Constructor from named list of ephemerides. ReferenceFrameManager::ReferenceFrameManager( const std::map< std::string, boost::shared_ptr< Ephemeris > >& ephemerisMap ) diff --git a/Tudat/Astrodynamics/Ephemerides/frameManager.h b/Tudat/Astrodynamics/Ephemerides/frameManager.h index 5eb66da646..6d225307aa 100644 --- a/Tudat/Astrodynamics/Ephemerides/frameManager.h +++ b/Tudat/Astrodynamics/Ephemerides/frameManager.h @@ -22,7 +22,6 @@ #include "Tudat/Astrodynamics/Ephemerides/compositeEphemeris.h" #include "Tudat/Astrodynamics/Ephemerides/constantEphemeris.h" -#include "Tudat/SimulationSetup/body.h" namespace tudat { @@ -57,13 +56,6 @@ class ReferenceFrameManager { public: - //! Constructor from named list of bodies. - /*! - * Constructor from named list of bodies. Ephemerides used in class are retrieved from bodies. - * \param bodyMap List of bodies used in simulations. - */ - ReferenceFrameManager( const simulation_setup::NamedBodyMap& bodyMap ); - //! Constructor from named list of ephemerides. /*! * Constructor from named list of ephemerides. diff --git a/Tudat/Astrodynamics/Ephemerides/rotationalEphemeris.h b/Tudat/Astrodynamics/Ephemerides/rotationalEphemeris.h index 9f7c9f5a55..b064e15a58 100644 --- a/Tudat/Astrodynamics/Ephemerides/rotationalEphemeris.h +++ b/Tudat/Astrodynamics/Ephemerides/rotationalEphemeris.h @@ -124,11 +124,38 @@ Eigen::Matrix< StateScalarType, 6, 1 > transformStateToFrameFromRotationFunction const boost::function< Eigen::Quaterniond( ) > rotationToFrameFunction, const boost::function< Eigen::Matrix3d( ) > rotationMatrixToFrameDerivativeFunction ) { - return transformStateToFrameFromRotations( + return transformStateToFrameFromRotations< StateScalarType >( stateInBaseFrame, rotationToFrameFunction( ), rotationMatrixToFrameDerivativeFunction( ) ); } +//! Transform a relative state (Cartesian position and velocity) from one frame to another. +/*! + * Transform a relative state (Cartesian position and velocity) from one frame to another, taking into + * account both the instantaneous rotational state of the two frames, and the rotational + * rate of one frame w.r.t. the other. + * \param stateInBaseFrame State that is to be transformed from base to target frame. + * \param centralBodyStateInBaseFrame State of central body w.r.t. which returned state is to be computed. + * State returned by this function must be in frame with same orientation as that returned by stateInBaseFrame. + * \param rotationToFrameFunction Function returning rotation from base to target frame. + * \param rotationMatrixToFrameDerivativeFunction Function returning time derivative of rotation + * matrix from base to target frame. + * \return State (Cartesian position and velocity) in target frame. + */ +template< typename StateScalarType > +Eigen::Matrix< StateScalarType, 6, 1 > transformRelativeStateToFrame( + const boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( ) > stateInBaseFrame, + const boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( ) > centralBodyStateInBaseFrame, + const boost::function< Eigen::Quaterniond( ) > rotationToFrameFunction, + const boost::function< Eigen::Matrix3d( ) > rotationMatrixToFrameDerivativeFunction ) +{ + return transformStateToFrameFromRotations< StateScalarType >( + stateInBaseFrame( ) - centralBodyStateInBaseFrame( ), rotationToFrameFunction( ), + rotationMatrixToFrameDerivativeFunction( ) ); +} + + + //! Transform a state (Cartesian position and velocity) from one frame to another. /*! * Transform a state (Cartesian position and velocity) from one frame to another, taking into @@ -330,8 +357,19 @@ class RotationalEphemeris }; +template< typename StateScalarType, typename TimeType > +Eigen::Matrix< StateScalarType, 6, 1 > transformStateToGlobalFrame( + const Eigen::Matrix< StateScalarType, 6, 1 >& stateInLocalFrame, + const TimeType currentTime, + const boost::shared_ptr< RotationalEphemeris > rotationalEphemeris ) +{ + return transformStateToFrameFromRotations< StateScalarType >( + stateInLocalFrame, rotationalEphemeris->getRotationToBaseFrame( currentTime ), + rotationalEphemeris->getDerivativeOfRotationToBaseFrame( currentTime ) ); + +} -} // namespace tudat } // namespace ephemerides +} // namespace tudat #endif // TUDAT_ROTATIONAL_EPHEMERIS_H diff --git a/Tudat/Astrodynamics/Ephemerides/simpleRotationalEphemeris.cpp b/Tudat/Astrodynamics/Ephemerides/simpleRotationalEphemeris.cpp index b8832332c3..3b7257bb75 100644 --- a/Tudat/Astrodynamics/Ephemerides/simpleRotationalEphemeris.cpp +++ b/Tudat/Astrodynamics/Ephemerides/simpleRotationalEphemeris.cpp @@ -89,5 +89,19 @@ Eigen::Matrix3d SimpleRotationalEphemeris::getDerivativeOfRotationToTargetFrame( * Eigen::Matrix3d( initialRotationToTargetFrame_ ); } +//! Function to reset the right ascension and declination of body's north pole. +void SimpleRotationalEphemeris::resetInitialPoleRightAscensionAndDeclination( const double rightAscension, + const double declination ) +{ + // Recalculate initial rotation quaternion + initialRotationToTargetFrame_ = + reference_frames::getInertialToPlanetocentricFrameTransformationQuaternion( + declination, rightAscension, initialEulerAngles_.z( ) ); + + // Reset angles in vector of Euler angles. + initialEulerAngles_.x( ) = rightAscension; + initialEulerAngles_.y( ) = declination; +} + } // namespace tudat } // namespace ephemerides diff --git a/Tudat/Astrodynamics/Ephemerides/simpleRotationalEphemeris.h b/Tudat/Astrodynamics/Ephemerides/simpleRotationalEphemeris.h index 99ed1a0e00..8a73301cbb 100644 --- a/Tudat/Astrodynamics/Ephemerides/simpleRotationalEphemeris.h +++ b/Tudat/Astrodynamics/Ephemerides/simpleRotationalEphemeris.h @@ -81,6 +81,9 @@ class SimpleRotationalEphemeris : public RotationalEphemeris inputReferenceJulianDay_( inputReferenceJulianDay ) { auxiliaryMatrix_<< 0.0, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0; + + initialEulerAngles_ = reference_frames::calculateInertialToPlanetFixedRotationAnglesFromMatrix( + Eigen::Matrix3d( initialRotationToTargetFrame_ ) ); } //! Constructor from rotation state angles. @@ -117,6 +120,11 @@ class SimpleRotationalEphemeris : public RotationalEphemeris inputReferenceJulianDay_( inputReferenceJulianDay ) { auxiliaryMatrix_<< 0.0, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0; + + // Set vector of euler angles + initialEulerAngles_.x( ) = poleRightAscension; + initialEulerAngles_.y( ) = poleDeclination; + initialEulerAngles_.z( ) = primeMeridianOfDate; } //! Calculate rotation quaternion from target frame to base frame. @@ -184,7 +192,7 @@ class SimpleRotationalEphemeris : public RotationalEphemeris * Returns rotation from target to base frame at initial time. * \return Rotation quaternion at initial time. */ - Eigen::Quaterniond getInitialRotationalState( ) { return initialRotationToTargetFrame_; } + Eigen::Quaterniond getInitialRotationToTargetFrame( ) { return initialRotationToTargetFrame_; } //! Get seconds since epoch at which initialRotationToTargetFrame_ is valid. /*! @@ -207,37 +215,73 @@ class SimpleRotationalEphemeris : public RotationalEphemeris */ double getRotationRate( ) { return rotationRate_; } + //! Function to reset the rotation rate of the body. + /*! + * Function to reset the rotation rate of the body. + * \param rotationRate New rotation rate [rad/s]. + */ + void resetRotationRate( const double rotationRate ) { rotationRate_ = rotationRate; } + + //! Function to get vector of euler angles at initialSecondsSinceEpoch_ + /*! + * Function to get vector of euler angles at initialSecondsSinceEpoch_, in order right ascension, declination, + * prime meridian. + * \return Vector of euler angles at initialSecondsSinceEpoch_, in order right ascension, declination, prime meridian. + */ + Eigen::Vector3d getInitialEulerAngles( ) + { + return initialEulerAngles_; + } + + //! Function to reset the right ascension and declination of body's north pole. + /*! + * Function to reset the right ascension and declination of body's north pole, + * recalculates the initialRotationToOriginalFrame_ member. + * \param rightAscension New right ascension of north pole. + * \param declination New declination of north pole. + */ + void resetInitialPoleRightAscensionAndDeclination( const double rightAscension, + const double declination ); + + private: //! Rotation rate of body (about local z-axis). /*! * Rotation rate of body (about local z-axis). */ - const double rotationRate_; + double rotationRate_; //! Rotation from target to base frame at initial time. /*! * Rotation from target to base frame at initial time. */ - const Eigen::Quaterniond initialRotationToTargetFrame_; + Eigen::Quaterniond initialRotationToTargetFrame_; //! Seconds since epoch at which initialRotationToTargetFrame_ is valid. /*! * Seconds since epoch at which initialRotationToTargetFrame is valid. */ - const double initialSecondsSinceEpoch_; + double initialSecondsSinceEpoch_; //! Julian day of reference epoch. /*! * Julian day of reference epoch. */ - const double inputReferenceJulianDay_; + double inputReferenceJulianDay_; - //! Auxiliary matrix used to calculate the time derivative of a rotation matrix. - Eigen::Matrix3d auxiliaryMatrix_; + //! Initial Euler angles describing the rotational state of the body at initialSecondsSinceEpoch_ + /*! + * Initial Euler angles describing the rotational state of the body at initialSecondsSinceEpoch_. Order of the vector + * is: right ascension (alpha), declination (delta), prime meridian of date (W) + */ + Eigen::Vector3d initialEulerAngles_; + + //! Auxiliary matrix used to calculate the time derivative of a rotation matrix. + Eigen::Matrix3d auxiliaryMatrix_; }; -} // namespace tudat } // namespace ephemerides +} // namespace tudat #endif // TUDAT_SIMPLE_ROTATIONAL_EPHEMERIS_H diff --git a/Tudat/Astrodynamics/Gravitation/CMakeLists.txt b/Tudat/Astrodynamics/Gravitation/CMakeLists.txt index fbcd824a97..8f521a392e 100644 --- a/Tudat/Astrodynamics/Gravitation/CMakeLists.txt +++ b/Tudat/Astrodynamics/Gravitation/CMakeLists.txt @@ -57,6 +57,7 @@ set(GRAVITATION_SOURCES "${SRCROOT}${GRAVITATIONDIR}/UnitTests/planetTestData.cpp" "${SRCROOT}${GRAVITATIONDIR}/unitConversionsCircularRestrictedThreeBodyProblem.cpp" "${SRCROOT}${GRAVITATIONDIR}/tabulatedGravityFieldVariations.cpp" + "${SRCROOT}${GRAVITATIONDIR}/mutualSphericalHarmonicGravityModel.cpp" ) # Set the header files. @@ -79,6 +80,7 @@ set(GRAVITATION_HEADERS "${SRCROOT}${GRAVITATIONDIR}/unitConversionsCircularRestrictedThreeBodyProblem.h" "${SRCROOT}${GRAVITATIONDIR}/UnitTests/planetTestData.h" "${SRCROOT}${GRAVITATIONDIR}/tabulatedGravityFieldVariations.h" + "${SRCROOT}${GRAVITATIONDIR}/mutualSphericalHarmonicGravityModel.h" ) # Add static libraries. @@ -120,10 +122,14 @@ target_link_libraries(test_SphericalHarmonicsGravityModel tudat_gravitation tuda add_executable(test_ThirdBodyPerturbation "${SRCROOT}${GRAVITATIONDIR}/UnitTests/unitTestThirdBodyPerturbation.cpp") setup_custom_test_program(test_ThirdBodyPerturbation "${SRCROOT}${GRAVITATIONDIR}") -target_link_libraries(test_ThirdBodyPerturbation tudat_gravitation ${Boost_LIBRARIES} ) +target_link_libraries(test_ThirdBodyPerturbation tudat_gravitation tudat_basic_mathematics ${Boost_LIBRARIES} ) if(USE_CSPICE) add_executable(test_GravityFieldVariations "${SRCROOT}${GRAVITATIONDIR}/UnitTests/unitTestGravityFieldVariations.cpp") setup_custom_test_program(test_GravityFieldVariations "${SRCROOT}${GRAVITATIONDIR}") target_link_libraries(test_GravityFieldVariations tudat_gravitation tudat_basic_mathematics tudat_spice_interface ${SPICE_LIBRARIES} ${Boost_LIBRARIES} ) + +add_executable(test_MutualSphericalHarmonicsGravityModel "${SRCROOT}${GRAVITATIONDIR}/UnitTests/unitTestMutualSphericalHarmonicAcceleration.cpp") +setup_custom_test_program(test_MutualSphericalHarmonicsGravityModel "${SRCROOT}${GRAVITATIONDIR}") +target_link_libraries(test_MutualSphericalHarmonicsGravityModel ${TUDAT_PROPAGATION_LIBRARIES} ${Boost_LIBRARIES}) endif() diff --git a/Tudat/Astrodynamics/Gravitation/UnitTests/unitTestMutualSphericalHarmonicAcceleration.cpp b/Tudat/Astrodynamics/Gravitation/UnitTests/unitTestMutualSphericalHarmonicAcceleration.cpp new file mode 100644 index 0000000000..562dd4167b --- /dev/null +++ b/Tudat/Astrodynamics/Gravitation/UnitTests/unitTestMutualSphericalHarmonicAcceleration.cpp @@ -0,0 +1,313 @@ +#define BOOST_TEST_MAIN + +#include +#include + +#include +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h" +#include "Tudat/Basics/testMacros.h" + +#include "Tudat/Astrodynamics/Gravitation/thirdBodyPerturbation.h" +#include "Tudat/Astrodynamics/Gravitation/mutualSphericalHarmonicGravityModel.h" +#include "Tudat/External/SpiceInterface/spiceInterface.h" +#include "Tudat/InputOutput/basicInputOutput.h" +#include "Tudat/SimulationSetup/PropagationSetup/createAccelerationModels.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createGravityField.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h" + + +namespace tudat +{ + +namespace unit_tests +{ + +using namespace tudat::spice_interface; +using namespace tudat::simulation_setup; +using namespace tudat::gravitation; +using namespace tudat::basic_astrodynamics; +using namespace tudat::simulation_setup; + +//! Generate (dummy_ spherical harmonic coefficients. +std::pair< Eigen::MatrixXd, Eigen::MatrixXd > generateCosineSineCoefficients( + const int maximumDegree, const int maximumOrder, const int bodyIndex ) +{ + Eigen::MatrixXd cosineCoefficients = Eigen::MatrixXd::Zero( maximumDegree + 1, maximumOrder + 1 ); + Eigen::MatrixXd sineCoefficients = Eigen::MatrixXd::Zero( maximumDegree + 1, maximumOrder + 1 ); + + cosineCoefficients( 0, 0 ) = 1.0; + + basic_mathematics::GlobalRandomNumberGeneratorType randomNumberGenerator( + static_cast< unsigned int >( bodyIndex ) ); + boost::uniform_01< boost::mt19937> distribution( randomNumberGenerator ); + + for( int i = 1; i < maximumDegree + 1; i++ ) + { + for( int j = 0; ( j < maximumOrder + 1 ) && ( j <= i ); j++ ) + { + cosineCoefficients( i, j ) = ( ( distribution( ) > 0.5 ) ? ( 1.0 ): ( -1.0 ) ) * distribution( ) * 1.0E-2; + if( j > 0 ) + { + sineCoefficients( i, j ) = ( ( distribution( ) > 0.5 ) ? ( 1.0 ): ( -1.0 ) ) * distribution( ) * 1.0E-2; + } + + } + } + + return std::make_pair( cosineCoefficients, sineCoefficients ); +} + +//! Generate gravity field object (with severely exagerated magnitude). +boost::shared_ptr< tudat::simulation_setup::GravityFieldSettings > getDummyJovianSystemGravityField( + const std::string& bodyName ) +{ + boost::shared_ptr< GravityFieldSettings > gravityFieldSettings; + + std::vector< double > randomNumberSettings; + randomNumberSettings.push_back( 0.0 ); + randomNumberSettings.push_back( 1.0E-4 ); + + std::pair< Eigen::MatrixXd, Eigen::MatrixXd > coefficients; + + if( bodyName == "Jupiter" ) + { + coefficients = generateCosineSineCoefficients( 10, 10, 0 ); + + gravityFieldSettings = boost::make_shared< SphericalHarmonicsGravityFieldSettings > + ( getBodyGravitationalParameter( "Jupiter" ), getAverageRadius( "Jupiter" ), + coefficients.first, coefficients.second, "IAU_Jupiter" ); + } + else if( bodyName == "Io" ) + { + coefficients = generateCosineSineCoefficients( 10, 10, 1 ); + + gravityFieldSettings = boost::make_shared< SphericalHarmonicsGravityFieldSettings > + ( 5.959916033410404E012, 200.0 * getAverageRadius( "Io" ), + coefficients.first, coefficients.second, "IAU_Io" ); + } + else if( bodyName == "Europa" ) + { + coefficients = generateCosineSineCoefficients( 10, 10, 2 ); + + gravityFieldSettings = boost::make_shared< SphericalHarmonicsGravityFieldSettings > + ( 3.202738774922892E12, 200.0 * getAverageRadius( "Europa" ), + coefficients.first, coefficients.second, "IAU_Europa" ); + } + + return gravityFieldSettings; + +} + + +BOOST_AUTO_TEST_SUITE( test_mutual_spherical_harmonic_gravity ) + +//! Test mutual spherical harmonic acceleration against manually combined spherical harmonic accelerations. +//! Note that the size of the Galilean moons, as well as the magnitude of the spherical harmonic coefficients has +//! been exaggerated to perform a more robust test (i.e ensure that typical errors in implementation are well above +//! numerical errors). +BOOST_AUTO_TEST_CASE( testMutualSphericalHarmonicGravity ) +{ + // Load spice kernels. + std::string kernelsPath = input_output::getSpiceKernelPath( ); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de-403-masses.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "naif0009.tls"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "pck00009.tpc"); + //spice_interface::loadSpiceKernelInTudat( kernelsPath + "jup230l.bsp"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de421.bsp"); + + // Create list of bodies to create. + std::vector< std::string > bodyNames; + bodyNames.push_back( "Jupiter" ); + bodyNames.push_back( "Io" ); + bodyNames.push_back( "Europa" ); + bodyNames.push_back( "Sun" ); + + // Specify initial time + double initialTime = 1.0E7; + double finalTime = 1.2E7; + + // Get body settings. + std::map< std::string, boost::shared_ptr< BodySettings > > bodySettings = + getDefaultBodySettings( bodyNames, initialTime, finalTime ); + bodySettings[ "Jupiter" ]->gravityFieldSettings = getDummyJovianSystemGravityField( "Jupiter" ); + bodySettings[ "Io" ]->gravityFieldSettings = getDummyJovianSystemGravityField( "Io" ); + bodySettings[ "Europa" ]->gravityFieldSettings = getDummyJovianSystemGravityField( "Europa" ); + + bodySettings[ "Jupiter" ]->ephemerisSettings = boost::make_shared< KeplerEphemerisSettings >( + ( basic_mathematics::Vector6d( )<< 778.57E9, 0.0489, 1.3 / 60.0, 0.0, 0.0, 0.0 ).finished( ), 0.0, + getBodyGravitationalParameter( "Sun" ), "Sun", "ECLIPJ2000" ); + bodySettings[ "Io" ]->ephemerisSettings = boost::make_shared< KeplerEphemerisSettings >( + ( basic_mathematics::Vector6d( )<< 421.8E6, 0.004, 0.04 / 60.0, 0.0, 0.0, 0.0 ).finished( ), 0.0, + getBodyGravitationalParameter( "Jupiter" ), "Sun", "ECLIPJ2000" ); + bodySettings[ "Europa" ]->ephemerisSettings = boost::make_shared< KeplerEphemerisSettings >( + ( basic_mathematics::Vector6d( )<< 671.1E6, 0.009, 0.47 / 60.0, 0.0, 0.0, 0.0 ).finished( ), 0.0, + getBodyGravitationalParameter( "Jupiter" ), "Sun", "ECLIPJ2000" ); + + + // Create bodies needed in simulation + NamedBodyMap bodyMap = createBodies( bodySettings ); + setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" ); + + // Set current state and rotation of bodies. + double currentTime = 1.1E7; + bodyMap[ "Jupiter" ]->setCurrentRotationToLocalFrameFromEphemeris( currentTime ); + bodyMap[ "Jupiter" ]->setStateFromEphemeris( currentTime ); + bodyMap[ "Io" ]->setCurrentRotationToLocalFrameFromEphemeris( currentTime ); + bodyMap[ "Io" ]->setStateFromEphemeris( currentTime ); + bodyMap[ "Europa" ]->setCurrentRotationToLocalFrameFromEphemeris( currentTime ); + bodyMap[ "Europa" ]->setStateFromEphemeris( currentTime ); + + // Retrieve gravity fields. + boost::shared_ptr< SphericalHarmonicsGravityField > jupiterGravityField = boost::dynamic_pointer_cast< SphericalHarmonicsGravityField >( + ( bodyMap.at( "Jupiter" ) )->getGravityFieldModel( ) ); + boost::shared_ptr< SphericalHarmonicsGravityField > ioGravityField = boost::dynamic_pointer_cast< SphericalHarmonicsGravityField >( + ( bodyMap.at( "Io" ) )->getGravityFieldModel( ) ); + boost::shared_ptr< SphericalHarmonicsGravityField > europaGravityField = boost::dynamic_pointer_cast< SphericalHarmonicsGravityField >( + ( bodyMap.at( "Europa" ) )->getGravityFieldModel( ) ); + + // Create central gravity acceleration (mu = Io + Jupiter) + boost::shared_ptr< AccelerationSettings > centralGravitySettings = boost::make_shared< AccelerationSettings >( central_gravity ); + boost::shared_ptr< CentralGravitationalAccelerationModel3d > centralGravity = + boost::dynamic_pointer_cast< CentralGravitationalAccelerationModel3d >( + createAccelerationModel( bodyMap.at( "Io" ), bodyMap.at( "Jupiter" ), centralGravitySettings, "Io", "Jupiter", + bodyMap.at( "Jupiter" ), "Jupiter" ) ); + + // Calculate central gravity acceleration. + centralGravity->updateMembers( ); + Eigen::Vector3d centralGravityAcceleration = centralGravity->getAcceleration( ); + + // Create spherical harmonic gravity of Jupiter on Io, Jupiter-fixed (mu = Io + Jupiter) + boost::shared_ptr< AccelerationSettings > sphericalHarmonicGravityOnIoFromJupiterSettings = + boost::make_shared< SphericalHarmonicAccelerationSettings >( 7, 7 ); + boost::shared_ptr< SphericalHarmonicsGravitationalAccelerationModel > sphericalHarmonicGravityOnIoFromJupiter = + boost::dynamic_pointer_cast< SphericalHarmonicsGravitationalAccelerationModel >( + createAccelerationModel( bodyMap.at( "Io" ), bodyMap.at( "Jupiter" ), sphericalHarmonicGravityOnIoFromJupiterSettings, + "Io", "Jupiter", bodyMap.at( "Jupiter" ), "Jupiter" ) ); + + // Calculate spherical harmonic gravity of Jupiter on Io. + sphericalHarmonicGravityOnIoFromJupiter->updateMembers( ); + Eigen::Vector3d sphericalHarmonicGravityOnIoFromJupiterAcceleration = sphericalHarmonicGravityOnIoFromJupiter->getAcceleration( ); + + // Create spherical harmonic gravity of Io on Jupiter, Io-fixed (mu = Io + Jupiter) + boost::shared_ptr< AccelerationSettings > sphericalHarmonicGravityOnJupiterFromIoSettings = + boost::make_shared< SphericalHarmonicAccelerationSettings >( 2, 2 ); + boost::shared_ptr< SphericalHarmonicsGravitationalAccelerationModel > sphericalHarmonicGravityOnJupiterFromIo = + boost::dynamic_pointer_cast< SphericalHarmonicsGravitationalAccelerationModel >( + createAccelerationModel( bodyMap.at( "Jupiter" ), bodyMap.at( "Io" ), sphericalHarmonicGravityOnJupiterFromIoSettings, + "Jupiter", "Io", bodyMap.at( "Io" ), "Io" ) ); + + // Calculate spherical harmonic gravity of Io on Jupiter. + sphericalHarmonicGravityOnJupiterFromIo->updateMembers( ); + Eigen::Vector3d sphericalHarmonicGravityOnJupiterFromIoAcceleration = sphericalHarmonicGravityOnJupiterFromIo->getAcceleration( ); + + // Create mutual spherical harmonic gravity between Io and Jupiter on Io, Jupiter fixed (mu = Io + Jupiter) + boost::shared_ptr< AccelerationSettings > mutualDirectJupiterIoShGravitySettings = + boost::make_shared< MutualSphericalHarmonicAccelerationSettings >( 7, 7, 2, 2 ); + boost::shared_ptr< MutualSphericalHarmonicsGravitationalAccelerationModel > mutualDirectJupiterIoShGravity = + boost::dynamic_pointer_cast< MutualSphericalHarmonicsGravitationalAccelerationModel >( + createAccelerationModel( bodyMap.at( "Io" ), bodyMap.at( "Jupiter" ), mutualDirectJupiterIoShGravitySettings, + "Io", "Jupiter", bodyMap.at( "Jupiter" ), "Jupiter" ) ); + + // Calculate mutual spherical harmonic gravity between Io and Jupiter on Io. + mutualDirectJupiterIoShGravity->updateMembers( ); + Eigen::Vector3d mutualDirectJupiterIoShGravityAcceleration = mutualDirectJupiterIoShGravity->getAcceleration( ); + + // Calculate expected mutual spherical harmonic gravity from sub-accelerations. + Eigen::Vector3d expectedAcceleration = -centralGravityAcceleration + sphericalHarmonicGravityOnIoFromJupiterAcceleration - + sphericalHarmonicGravityOnJupiterFromIoAcceleration; + + // Test against directly calculated mutual spherical harmonic gravity. + for( unsigned int i = 0; i < 3; i++ ) + { + BOOST_CHECK_SMALL( std::fabs( expectedAcceleration( i ) - mutualDirectJupiterIoShGravityAcceleration( i ) ), + 10.0 * std::numeric_limits< double >::epsilon( ) * expectedAcceleration.norm( ) ); + } + + // Create mutual spherical harmonic gravity between Io and Jupiter on Jupiter, Io fixed (mu = Io + Jupiter) + boost::shared_ptr< AccelerationSettings > mutualDirectJupiterIoShGravitySettings2 = + boost::make_shared< MutualSphericalHarmonicAccelerationSettings >( 2, 2, 7, 7 ); + boost::shared_ptr< MutualSphericalHarmonicsGravitationalAccelerationModel > mutualDirectJupiterIoShGravity2 = + boost::dynamic_pointer_cast< MutualSphericalHarmonicsGravitationalAccelerationModel >( + createAccelerationModel( bodyMap.at( "Jupiter" ), bodyMap.at( "Io" ), mutualDirectJupiterIoShGravitySettings2, + "Jupiter", "Io", bodyMap.at( "Io" ), "Io" ) ); + + // Calculate mutual spherical harmonic gravity between Io and Jupiter on Jupiter. + mutualDirectJupiterIoShGravity2->updateMembers( ); + Eigen::Vector3d mutualDirectJupiterIoShGravityAcceleration2 = mutualDirectJupiterIoShGravity2->getAcceleration( ); + + expectedAcceleration = centralGravityAcceleration - sphericalHarmonicGravityOnIoFromJupiterAcceleration + sphericalHarmonicGravityOnJupiterFromIoAcceleration; + + + // Test against directly calculated mutual spherical harmonic gravity. + for( unsigned int i = 0; i < 3; i++ ) + { + BOOST_CHECK_SMALL( std::fabs( expectedAcceleration( i ) - mutualDirectJupiterIoShGravityAcceleration2( i ) ), + 10.0 * ( std::numeric_limits< double >::epsilon( ) * expectedAcceleration.norm( ) ) ); + } + + // Test against directly calculated mutual spherical harmonic gravity. + for( unsigned int i = 0; i < 3; i++ ) + { + BOOST_CHECK_SMALL( std::fabs( mutualDirectJupiterIoShGravityAcceleration( i ) + mutualDirectJupiterIoShGravityAcceleration2( i ) ), + 10.0 * std::numeric_limits< double >::epsilon( ) * mutualDirectJupiterIoShGravityAcceleration.norm( ) ); + } + + + // Create 3rd body mutual spherical harmonics between Io and Europa on Europa, Jupiter fixed (mu = Io) + boost::shared_ptr< AccelerationSettings > mutualThirdBodyIoOnEuropaShGravitySettings = + boost::make_shared< MutualSphericalHarmonicAccelerationSettings >( 2, 2, 4, 4, 7, 7 ); + boost::shared_ptr< ThirdBodyMutualSphericalHarmonicsGravitationalAccelerationModel > mutualThirdBodyIoOnEuropaShGravity = + boost::dynamic_pointer_cast< ThirdBodyMutualSphericalHarmonicsGravitationalAccelerationModel >( + createAccelerationModel( bodyMap.at( "Europa" ), bodyMap.at( "Io" ), mutualThirdBodyIoOnEuropaShGravitySettings, + "Europa", "Io", bodyMap.at( "Jupiter" ), "Jupiter" ) ); + + // Calculate 3rd body mutual spherical harmonics between Io and Europa on Europa. + mutualThirdBodyIoOnEuropaShGravity->updateMembers( ); + Eigen::Vector3d mutualThirdBodyIoOnEuropaShGravityAcceleration = mutualThirdBodyIoOnEuropaShGravity->getAcceleration( ); + + + // Create mutual spherical harmonics between Io and Europa on Europa, Io fixed (mu = Io + Europa) + boost::shared_ptr< MutualSphericalHarmonicsGravitationalAccelerationModel > mutualDirectIoOnEuropaShGravity = + boost::dynamic_pointer_cast< MutualSphericalHarmonicsGravitationalAccelerationModel >( + createAccelerationModel( bodyMap.at( "Europa" ), bodyMap.at( "Io" ), mutualThirdBodyIoOnEuropaShGravitySettings, + "Europa", "Io", bodyMap.at( "Io" ), "Io" ) ); + mutualDirectIoOnEuropaShGravity->updateMembers( ); + Eigen::Vector3d mutualDirectIoOnEuropaShGravityAcceleration = mutualDirectIoOnEuropaShGravity->getAcceleration( ); + + // Get sub accelerations from 3rd body acceleration. + Eigen::Vector3d directAccelerationFromThirdBodyModel = mutualThirdBodyIoOnEuropaShGravity->getAccelerationModelForBodyUndergoingAcceleration( )->getAcceleration( ); + Eigen::Vector3d centralBodyAccelerationFromThirdBodyModel = mutualThirdBodyIoOnEuropaShGravity->getAccelerationModelForCentralBody( )-> + getAcceleration( ); + + for( unsigned int i = 0; i < 3; i++ ) + { + BOOST_CHECK_SMALL( + ( directAccelerationFromThirdBodyModel( i ) - + ( ioGravityField->getGravitationalParameter( ) / + ( ioGravityField->getGravitationalParameter( ) + europaGravityField->getGravitationalParameter( ) ) * + mutualDirectIoOnEuropaShGravityAcceleration( i ) ) ), + ( 10.0 * std::numeric_limits< double >::epsilon( ) * directAccelerationFromThirdBodyModel.norm( ) ) ); + + BOOST_CHECK_SMALL( + centralBodyAccelerationFromThirdBodyModel( i ) - + ( ioGravityField->getGravitationalParameter( ) / + ( ioGravityField->getGravitationalParameter( ) + jupiterGravityField->getGravitationalParameter( ) ) * + mutualDirectJupiterIoShGravityAcceleration2( i ) ), + ( 10.0 * std::numeric_limits< double >::epsilon( ) * centralBodyAccelerationFromThirdBodyModel.norm( ) ) ); + + BOOST_CHECK_SMALL( + mutualThirdBodyIoOnEuropaShGravityAcceleration( i ) - + ( directAccelerationFromThirdBodyModel( i ) - centralBodyAccelerationFromThirdBodyModel( i ) ), + ( std::numeric_limits< double >::epsilon( ) * mutualThirdBodyIoOnEuropaShGravityAcceleration.norm( ) ) ); + } +} + +BOOST_AUTO_TEST_SUITE_END( ) + +} + +} diff --git a/Tudat/Astrodynamics/Gravitation/UnitTests/unitTestSphericalHarmonicsGravityModel.cpp b/Tudat/Astrodynamics/Gravitation/UnitTests/unitTestSphericalHarmonicsGravityModel.cpp index 46c172439c..6a000b775e 100644 --- a/Tudat/Astrodynamics/Gravitation/UnitTests/unitTestSphericalHarmonicsGravityModel.cpp +++ b/Tudat/Astrodynamics/Gravitation/UnitTests/unitTestSphericalHarmonicsGravityModel.cpp @@ -57,6 +57,7 @@ #include "Tudat/Basics/testMacros.h" #include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h" +#include "Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h" namespace tudat { @@ -97,7 +98,8 @@ BOOST_AUTO_TEST_CASE( test_SphericalHarmonicsGravitationalAcceleration_Demo1 ) degree, order, cosineCoefficient, - sineCoefficient ); + sineCoefficient, + boost::make_shared< basic_mathematics::SphericalHarmonicsCache >( 3, 1 ) ); // Define expected acceleration according to the MATLAB function 'gravitysphericalharmonic' // described by Mathworks [2012] [m s^-2]. @@ -140,7 +142,8 @@ BOOST_AUTO_TEST_CASE( test_SphericalHarmonicsGravitationalAcceleration_Demo2 ) degree, order, cosineCoefficient, - sineCoefficient ); + sineCoefficient, + boost::make_shared< basic_mathematics::SphericalHarmonicsCache >( 3, 2 ) ); // Define expected acceleration according to the MATLAB function 'gravitysphericalharmonic' // described by Mathworks [2012] [m s^-2]. @@ -183,7 +186,8 @@ BOOST_AUTO_TEST_CASE( test_SphericalHarmonicsGravitationalAcceleration_Demo3 ) degree, order, cosineCoefficient, - sineCoefficient ); + sineCoefficient, + boost::make_shared< basic_mathematics::SphericalHarmonicsCache >( 3, 3 ) ); // Define expected acceleration according to the MATLAB function 'gravitysphericalharmonic' // described by Mathworks [2012] [m s^-2]. @@ -240,7 +244,8 @@ BOOST_AUTO_TEST_CASE( test_SphericalHarmonicsGravitationalAcceleration_Demo4 ) gravitationalParameter, planetaryRadius, cosineCoefficients, - sineCoefficients ); + sineCoefficients, + boost::make_shared< basic_mathematics::SphericalHarmonicsCache >( 6, 6 ) ); // Define expected acceleration according to the MATLAB function 'gravitysphericalharmonic' // described by Mathworks [2012] [m s^-2]. @@ -294,8 +299,8 @@ BOOST_AUTO_TEST_CASE( test_SphericalHarmonicsGravitationalAccelerationWrapperCla const Eigen::Vector3d position( 7.0e6, 8.0e6, 9.0e6 ); // Declare spherical harmonics gravitational acceleration class object. - SphericalHarmonicsGravitationalAccelerationModelXdPointer earthGravity - = boost::make_shared< SphericalHarmonicsGravitationalAccelerationModelXd >( + SphericalHarmonicsGravitationalAccelerationModelPointer earthGravity + = boost::make_shared< SphericalHarmonicsGravitationalAccelerationModel >( boost::lambda::constant( position ), gravitationalParameter, planetaryRadius, cosineCoefficients, sineCoefficients ); diff --git a/Tudat/Astrodynamics/Gravitation/basicSolidBodyTideGravityFieldVariations.h b/Tudat/Astrodynamics/Gravitation/basicSolidBodyTideGravityFieldVariations.h index f46f109232..30f04f17b0 100644 --- a/Tudat/Astrodynamics/Gravitation/basicSolidBodyTideGravityFieldVariations.h +++ b/Tudat/Astrodynamics/Gravitation/basicSolidBodyTideGravityFieldVariations.h @@ -8,8 +8,8 @@ * http://tudat.tudelft.nl/LICENSE. */ -#ifndef BASICSOLIDBODYTIDEGRAVITYFIELDVARIATIONS_H -#define BASICSOLIDBODYTIDEGRAVITYFIELDVARIATIONS_H +#ifndef TUDAT_BASICSOLIDBODYTIDEGRAVITYFIELDVARIATIONS_H +#define TUDAT_BASICSOLIDBODYTIDEGRAVITYFIELDVARIATIONS_H #include #include @@ -250,16 +250,19 @@ class BasicSolidBodyTideGravityFieldVariations: public GravityFieldVariations loveNumbers_[ degree - 2 ] = loveNumbers; } else - { - std::cerr<<"Error, tried to set love numbers at degree "<( degree ) + " in BasicSolidBodyTideGravityFieldVariations with" + + boost::lexical_cast< std::string >( loveNumbers.size( ) ) + " orders"; + throw std::runtime_error( errorMessage ); } } else { - std::cerr<<"Error, tried to set love numbers at degree "<( degree ) + + " in BasicSolidBodyTideGravityFieldVariations: not available"; + throw std::runtime_error( errorMessage ); } } @@ -554,7 +557,7 @@ class BasicSolidBodyTideGravityFieldVariations: public GravityFieldVariations }; -} +} // namespace gravitation -} -#endif // BASICSOLIDBODYTIDEGRAVITYFIELDVARIATIONS_H +} // namespace tudat +#endif // TUDAT_BASICSOLIDBODYTIDEGRAVITYFIELDVARIATIONS_H diff --git a/Tudat/Astrodynamics/Gravitation/centralGravityModel.cpp b/Tudat/Astrodynamics/Gravitation/centralGravityModel.cpp index 35a57a119d..e47d18abc1 100644 --- a/Tudat/Astrodynamics/Gravitation/centralGravityModel.cpp +++ b/Tudat/Astrodynamics/Gravitation/centralGravityModel.cpp @@ -60,10 +60,10 @@ Eigen::Vector3d computeGravitationalAcceleration( const double gravitationalParameterOfBodyExertingAcceleration, const Eigen::Vector3d& positionOfBodyExertingAcceleration ) { + double distance = ( positionOfBodySubjectToAcceleration - positionOfBodyExertingAcceleration ).norm( ); return -gravitationalParameterOfBodyExertingAcceleration * ( positionOfBodySubjectToAcceleration - positionOfBodyExertingAcceleration ) - / std::pow( ( positionOfBodySubjectToAcceleration - - positionOfBodyExertingAcceleration ).norm( ), 3.0 ); + / ( distance * distance * distance ); } //! Compute gravitational force. diff --git a/Tudat/Astrodynamics/Gravitation/centralGravityModel.h b/Tudat/Astrodynamics/Gravitation/centralGravityModel.h index 82c4558c50..f08a424251 100644 --- a/Tudat/Astrodynamics/Gravitation/centralGravityModel.h +++ b/Tudat/Astrodynamics/Gravitation/centralGravityModel.h @@ -192,15 +192,21 @@ class CentralGravitationalAccelerationModel * \param aGravitationalParameter A (constant) gravitational parameter [m^2 s^-3]. * \param positionOfBodyExertingAccelerationFunction Pointer to function returning position of * body exerting gravitational acceleration (default = (0,0,0)). + * \param isMutualAttractionUsed Variable denoting whether attraction from body undergoing acceleration on + * body exerting acceleration is included (i.e. whether aGravitationalParameter refers to the property + * of the body exerting the acceleration, if variable is false, or the sum of the gravitational parameters, + * if the variable is true. */ CentralGravitationalAccelerationModel( const typename Base::StateFunction positionOfBodySubjectToAccelerationFunction, const double aGravitationalParameter, const typename Base::StateFunction positionOfBodyExertingAccelerationFunction - = boost::lambda::constant( StateMatrix::Zero( ) ) ) + = boost::lambda::constant( StateMatrix::Zero( ) ), + const bool isMutualAttractionUsed = false ) : Base( positionOfBodySubjectToAccelerationFunction, boost::lambda::constant( aGravitationalParameter ), - positionOfBodyExertingAccelerationFunction ) + positionOfBodyExertingAccelerationFunction, + isMutualAttractionUsed ) { this->updateMembers( ); } @@ -220,15 +226,21 @@ class CentralGravitationalAccelerationModel * parameter [m^2 s^-3]. * \param positionOfBodyExertingAccelerationFunction Pointer to function returning position of * body exerting gravitational acceleration (default = (0,0,0)). + * \param isMutualAttractionUsed Variable denoting whether attraction from body undergoing acceleration on + * body exerting acceleration is included (i.e. whether aGravitationalParameter refers to the property + * of the body exerting the acceleration, if variable is false, or the sum of the gravitational parameters, + * if the variable is true. */ CentralGravitationalAccelerationModel( const typename Base::StateFunction positionOfBodySubjectToAccelerationFunction, const boost::function< double( ) > aGravitationalParameterFunction, const typename Base::StateFunction positionOfBodyExertingAccelerationFunction - = boost::lambda::constant( StateMatrix::Zero( ) ) ) + = boost::lambda::constant( StateMatrix::Zero( ) ), + const bool isMutualAttractionUsed = false ) : Base( positionOfBodySubjectToAccelerationFunction, aGravitationalParameterFunction, - positionOfBodyExertingAccelerationFunction ) + positionOfBodyExertingAccelerationFunction, + isMutualAttractionUsed ) { this->updateMembers( ); } diff --git a/Tudat/Astrodynamics/Gravitation/centralJ2GravityModel.h b/Tudat/Astrodynamics/Gravitation/centralJ2GravityModel.h index 460f2ec9f2..ae7d1856ac 100644 --- a/Tudat/Astrodynamics/Gravitation/centralJ2GravityModel.h +++ b/Tudat/Astrodynamics/Gravitation/centralJ2GravityModel.h @@ -137,7 +137,7 @@ class CentralJ2GravitationalAccelerationModel = boost::lambda::constant( Eigen::Vector3d::Zero( ) ) ) : Base( positionOfBodySubjectToAccelerationFunction, aGravitationalParameter, - positionOfBodyExertingAccelerationFunction ), + positionOfBodyExertingAccelerationFunction, false ), equatorialRadius( anEquatorialRadius ), j2GravityCoefficient( aJ2GravityCoefficient ) { diff --git a/Tudat/Astrodynamics/Gravitation/centralJ2J3GravityModel.h b/Tudat/Astrodynamics/Gravitation/centralJ2J3GravityModel.h index 9d02f5b92d..956fc968cf 100644 --- a/Tudat/Astrodynamics/Gravitation/centralJ2J3GravityModel.h +++ b/Tudat/Astrodynamics/Gravitation/centralJ2J3GravityModel.h @@ -142,7 +142,7 @@ class CentralJ2J3GravitationalAccelerationModel = boost::lambda::constant( Eigen::Vector3d::Zero( ) ) ) : Base( positionOfBodySubjectToAccelerationFunction, aGravitationalParameter, - positionOfBodyExertingAccelerationFunction ), + positionOfBodyExertingAccelerationFunction, false ), equatorialRadius( anEquatorialRadius ), j2GravityCoefficient( aJ2GravityCoefficient ), j3GravityCoefficient( aJ3GravityCoefficient ) diff --git a/Tudat/Astrodynamics/Gravitation/centralJ2J3J4GravityModel.h b/Tudat/Astrodynamics/Gravitation/centralJ2J3J4GravityModel.h index 67ecfd771c..e11e4e65a2 100644 --- a/Tudat/Astrodynamics/Gravitation/centralJ2J3J4GravityModel.h +++ b/Tudat/Astrodynamics/Gravitation/centralJ2J3J4GravityModel.h @@ -191,7 +191,7 @@ class CentralJ2J3J4GravitationalAccelerationModel = boost::lambda::constant( Eigen::Vector3d::Zero( ) ) ) : Base( positionOfBodySubjectToAccelerationFunction, aGravitationalParameter, - positionOfBodyExertingAccelerationFunction ), + positionOfBodyExertingAccelerationFunction, false ), equatorialRadius( anEquatorialRadius ), j2GravityCoefficient( aJ2GravityCoefficient ), j3GravityCoefficient( aJ3GravityCoefficient ), diff --git a/Tudat/Astrodynamics/Gravitation/gravityFieldModel.cpp b/Tudat/Astrodynamics/Gravitation/gravityFieldModel.cpp index 6fed5d9ae3..54eb220e91 100644 --- a/Tudat/Astrodynamics/Gravitation/gravityFieldModel.cpp +++ b/Tudat/Astrodynamics/Gravitation/gravityFieldModel.cpp @@ -33,7 +33,9 @@ */ #include + #include +#include #include "Tudat/Astrodynamics/Gravitation/gravityFieldModel.h" @@ -151,8 +153,10 @@ boost::shared_ptr< GravityFieldModel > getPredefinedCentralGravityField( default: - // Print cerr statement. - std::cerr << "Desired predefined central gravity field does not exist." << std::endl; + std::string errorMessage = "Desired predefined central gravity field " + + boost::lexical_cast< std::string >( bodyWithPredefinedCentralGravityField ) + + " does not exist"; + throw std::runtime_error( errorMessage ); } return boost::make_shared< GravityFieldModel >( gravitationalParameter ); } diff --git a/Tudat/Astrodynamics/Gravitation/gravityFieldVariations.cpp b/Tudat/Astrodynamics/Gravitation/gravityFieldVariations.cpp index 3f0350a01a..88d4ed21fa 100644 --- a/Tudat/Astrodynamics/Gravitation/gravityFieldVariations.cpp +++ b/Tudat/Astrodynamics/Gravitation/gravityFieldVariations.cpp @@ -99,9 +99,12 @@ GravityFieldVariationsSet::getGravityFieldVariation( // Provide warning if no matches are found if( isCorrectIdentifierFound == 0 ) { - std::cerr << "Error when retrieving gravity field variation of type " << - deformationType << ", none of " << - numberOfEntries << " potential entries match identifier." << std::endl; + std::string errorMessage = "Error when retrieving gravity field variation of typ " + + boost::lexical_cast< std::string >( deformationType ) + + ", none of " + + boost::lexical_cast< std::string >( numberOfEntries ) + + " potential entries match identifier."; + throw std::runtime_error( errorMessage ); } } @@ -184,11 +187,11 @@ GravityFieldVariationsSet::GravityFieldVariationsSet( // Check consistency of input data vector sizes. if( variationObjects_.size( ) != variationType_.size( ) ) { - std::cerr << "Error when making GravityFieldVariationsSet, inconsistent input, type 1" << std::endl; + throw std::runtime_error( "Error when making GravityFieldVariationsSet, inconsistent input, type 1" ); } if( variationObjects_.size( ) != variationIdentifier_.size( ) ) { - std::cerr << "Error when making GravityFieldVariationsSet, inconsistent input, type 2" << std::endl; + throw std::runtime_error( "Error when making GravityFieldVariationsSet, inconsistent input, type 2" ); } // Check if interpolation information is provided where required. @@ -199,20 +202,23 @@ GravityFieldVariationsSet::GravityFieldVariationsSet( { if( initialTimes_.count( interpolatorSettingsIterator->first ) == 0 ) { - std::cerr << "Error when making GravityFieldVariationsSet, inconsistent input, type 4, " << - interpolatorSettingsIterator->first << std::endl; + std::string errorMessage = "Error when making GravityFieldVariationsSet, inconsistent input, type 4, " + + boost::lexical_cast< std::string >( interpolatorSettingsIterator->first ); + throw std::runtime_error( errorMessage ); } if( finalTimes_.count( interpolatorSettingsIterator->first ) == 0 ) { - std::cerr << "Error when making GravityFieldVariationsSet, inconsistent input, type 5, " << - interpolatorSettingsIterator->first << std::endl; + std::string errorMessage = "Error when making GravityFieldVariationsSet, inconsistent input, type 5, " + + boost::lexical_cast< std::string >( interpolatorSettingsIterator->first ); + throw std::runtime_error( errorMessage ); } if( timeSteps_.count( interpolatorSettingsIterator->first ) == 0 ) { - std::cerr << "Error when making GravityFieldVariationsSet, inconsistent input, type 6, " << - interpolatorSettingsIterator->first << std::endl; + std::string errorMessage = "Error when making GravityFieldVariationsSet, inconsistent input, type 6, " + + boost::lexical_cast< std::string >( interpolatorSettingsIterator->first ); + throw std::runtime_error( errorMessage ); } } } diff --git a/Tudat/Astrodynamics/Gravitation/mutualSphericalHarmonicGravityModel.cpp b/Tudat/Astrodynamics/Gravitation/mutualSphericalHarmonicGravityModel.cpp new file mode 100644 index 0000000000..4719ee2151 --- /dev/null +++ b/Tudat/Astrodynamics/Gravitation/mutualSphericalHarmonicGravityModel.cpp @@ -0,0 +1,20 @@ +#include "Tudat/Astrodynamics/Gravitation/mutualSphericalHarmonicGravityModel.h" + +namespace tudat +{ + +namespace gravitation +{ + +//! Function to manually remove the C(0,0) term from cosine coefficients, +Eigen::MatrixXd setDegreeAndOrderCoefficientToZero( + const boost::function< Eigen::MatrixXd( ) > originalCosineCoefficientFunction ) +{ + Eigen::MatrixXd newCoefficients = originalCosineCoefficientFunction( ); + newCoefficients( 0, 0 ) = 0.0; + return newCoefficients; +} + +} + +} diff --git a/Tudat/Astrodynamics/Gravitation/mutualSphericalHarmonicGravityModel.h b/Tudat/Astrodynamics/Gravitation/mutualSphericalHarmonicGravityModel.h new file mode 100644 index 0000000000..225f8cabd0 --- /dev/null +++ b/Tudat/Astrodynamics/Gravitation/mutualSphericalHarmonicGravityModel.h @@ -0,0 +1,267 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_MUTUALSPHERICALHARMONICGRAVITYMODEL_H +#define TUDAT_MUTUALSPHERICALHARMONICGRAVITYMODEL_H + + +#include +#include +#include +#include +#include + +#include +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" +#include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModelBase.h" +#include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h" +#include "Tudat/Mathematics/BasicMathematics/legendrePolynomials.h" +#include "Tudat/Mathematics/BasicMathematics/linearAlgebraTypes.h" + +namespace tudat +{ + +namespace gravitation +{ + +//! Function to manually remove the C(0,0) term from cosine coefficients, +/*! + * Function to manually remove the C(0,0) term from cosine coefficients, used by + * MutualSphericalHarmonicsGravitationalAccelerationModel. By using this function, the original function returning the sh + * coefficients is left untouched, not compromising the environment model, while the C(0,0) term is not calculated doubly + * by the MutualSphericalHarmonicsGravitationalAccelerationModel class. + */ +Eigen::MatrixXd setDegreeAndOrderCoefficientToZero( const boost::function< Eigen::MatrixXd( ) > originalCosineCoefficientFunction ); + +//! Class to calculate the mutual spherical harmonic gravitational acceleration between two bodies. +/*! + * Class to calculate the mutual spherical harmonic gravitational acceleration between two extended bodies A and B. + * The calculations include the interaction of the point masses A and B, extended body A and point mass B, as well as + * extended bodyB and point mass A. As an example, the model can be used for precise calculations of the dynamics of + * planetary system (Earth-Moon, Mars-Phoboss-Deimos, Jupiter-Galilean satellites). + * Model is taken from Lainey et al. (2001, 2004) + */ +class MutualSphericalHarmonicsGravitationalAccelerationModel + : public basic_astrodynamics::AccelerationModel< Eigen::Vector3d > +{ +private: + + //! Typedef for coefficient-matrix-returning function. + typedef boost::function< Eigen::MatrixXd( ) > CoefficientMatrixReturningFunction; + + //! Typedef for function returning body position. + typedef boost::function< Eigen::Vector3d( ) > StateFunction; + + //! Typedef for function returning gravitational parameter. + typedef boost::function< double( ) > DataReturningFunction; + +public: + + //! Constructor. + /*! + * Constructor, provides the position functions of the involved bodies, and the required data on their gravitational + * fields. + * \param positionOfBodySubjectToAccelerationFunction Function returning the current position of the body undergoing + * the acceleration. + * \param positionOfBodyExertingAccelerationFunction Function returning the current position of the body exerting + * the acceleration. + * \param gravitationalParameterFunction Function returning the current gravitational parameter, either of the body + * exerting the acceleration or the sum of that of both bodies, depending on value of useCentralBodyFixedFrame, + * (false for former, true for latter). + * \param equatorialRadiusOfBodyExertingAcceleration Equatorial radius used in representation of spherical harmonic + * coefficients of body exerting acceleration. + * \param equatorialRadiusOfBodyUndergoingAcceleration Equatorial radius used in representation of spherical harmonic + * coefficients of body undergoing acceleration. + * \param cosineHarmonicCoefficientsFunctionOfBodyExertingAcceleration Function returning the spherical harmonic cosine + * coefficients of the body exerting the acceleration. + * \param sineHarmonicCoefficientsFunctionOfBodyExertingAcceleration Function returning the spherical harmonic sine + * coefficients of the body exerting the acceleration. + * \param cosineHarmonicCoefficientsFunctionOfBodyUndergoingAcceleration Function returning the spherical harmonic + * cosine coefficients of the body undergoing the acceleration. + * \param sineHarmonicCoefficientsFunctionOfBodyUndergoingAcceleration Function returning the spherical harmonic + * sine coefficients of the body undergoing the acceleration. + * \param toLocalFrameOfBodyExertingAccelerationTransformation Function returning the quaternion to rotate from the + * body-fixed frame of the body exerting the acceleration, in which the spherical harmonic coefficients are defined, + * to the inertially oriented frame, in which the acceleration is expressed. + * \param toLocalFrameOfBodyUndergoingAccelerationTransformation Function returning the quaternion to rotate from the + * body-fixed frame of the body undergoing the acceleration, in which the spherical harmonic coefficients are defined, + * to the inertially oriented frame, in which the acceleration is expressed. + * \param useCentralBodyFixedFrame Boolean denoting whether the acceleration is expressed in a frame centered on the + * body exerting the acceleration, in which case the gravitational parameter that is used is the some of the + * gravitational parameters of both bodies, to take into account the inertial acceleration of the reference frame in + * which the acceleration is performed. + * \param sphericalHarmonicsCacheOfBodyExertingAcceleration Caching object for computation of spherical harmonic + * potential (gradient) of body exerting acceleration. + * * \param sphericalHarmonicsCacheOfBodyUndergoingAcceleration Caching object for computation of spherical harmonic + * potential (gradient) of body undergoing acceleration. + */ + MutualSphericalHarmonicsGravitationalAccelerationModel( + const StateFunction& positionOfBodySubjectToAccelerationFunction, + const StateFunction& positionOfBodyExertingAccelerationFunction, + const DataReturningFunction& gravitationalParameterFunction, + const double equatorialRadiusOfBodyExertingAcceleration, + const double equatorialRadiusOfBodyUndergoingAcceleration, + const CoefficientMatrixReturningFunction& cosineHarmonicCoefficientsFunctionOfBodyExertingAcceleration, + const CoefficientMatrixReturningFunction& sineHarmonicCoefficientsFunctionOfBodyExertingAcceleration, + const CoefficientMatrixReturningFunction& cosineHarmonicCoefficientsFunctionOfBodyUndergoingAcceleration, + const CoefficientMatrixReturningFunction& sineHarmonicCoefficientsFunctionOfBodyUndergoingAcceleration, + const boost::function< Eigen::Quaterniond( ) >& toLocalFrameOfBodyExertingAccelerationTransformation, + const boost::function< Eigen::Quaterniond( ) >& toLocalFrameOfBodyUndergoingAccelerationTransformation, + const bool useCentralBodyFixedFrame, + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > + sphericalHarmonicsCacheOfBodyExertingAcceleration = + boost::make_shared< basic_mathematics::SphericalHarmonicsCache >( ), + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > + sphericalHarmonicsCacheOfBodyUndergoingAcceleration = + boost::make_shared< basic_mathematics::SphericalHarmonicsCache >( ) ): + useCentralBodyFixedFrame_( useCentralBodyFixedFrame ), + gravitationalParameterFunction_( gravitationalParameterFunction ) + { + + // Create spherical harmonic acceleration due to expansion of body exerting acceleration + accelerationModelFromShExpansionOfBodyExertingAcceleration_ = boost::make_shared< + SphericalHarmonicsGravitationalAccelerationModel >( + positionOfBodySubjectToAccelerationFunction, gravitationalParameterFunction, + equatorialRadiusOfBodyExertingAcceleration, + cosineHarmonicCoefficientsFunctionOfBodyExertingAcceleration, + sineHarmonicCoefficientsFunctionOfBodyExertingAcceleration, + positionOfBodyExertingAccelerationFunction, + toLocalFrameOfBodyExertingAccelerationTransformation, + useCentralBodyFixedFrame, sphericalHarmonicsCacheOfBodyExertingAcceleration ); + + // Create spherical harmonic acceleration due to expansion of body undergoing acceleration, with the C(0,0) term set + // to zero to prevent the double computation of the central term. Note that the order of the position functions is + // switched wrt the regular input, to ensure that the acceleration vector points in the right direction + // (i.e. from body undergoing to body exerting acceleration). + accelerationModelFromShExpansionOfBodyundergoingAcceleration_ = boost::make_shared< + SphericalHarmonicsGravitationalAccelerationModel >( + positionOfBodyExertingAccelerationFunction, gravitationalParameterFunction, + equatorialRadiusOfBodyUndergoingAcceleration, + boost::bind( &setDegreeAndOrderCoefficientToZero, + cosineHarmonicCoefficientsFunctionOfBodyUndergoingAcceleration ), + sineHarmonicCoefficientsFunctionOfBodyUndergoingAcceleration, + positionOfBodySubjectToAccelerationFunction, + toLocalFrameOfBodyUndergoingAccelerationTransformation, + useCentralBodyFixedFrame, sphericalHarmonicsCacheOfBodyUndergoingAcceleration ); + } + + //! Update member variables used by the acceleration model. + /*! + * Update member variables used by the two constituent sh acceleration models. + */ + virtual void updateMembers( const double currentTime = TUDAT_NAN ) + { + accelerationModelFromShExpansionOfBodyExertingAcceleration_->updateMembers( currentTime ); + accelerationModelFromShExpansionOfBodyundergoingAcceleration_->updateMembers( currentTime ); + this->currentTime_ = currentTime; + } + + //! Function to reset the current time + /*! + * Function to reset the current time of the acceleration model. + * \param currentTime Current time (default NaN). + */ + virtual void resetTime( const double currentTime = TUDAT_NAN ) + { + currentTime_ = currentTime; + + accelerationModelFromShExpansionOfBodyExertingAcceleration_->resetTime( currentTime ); + accelerationModelFromShExpansionOfBodyundergoingAcceleration_->resetTime( currentTime ); + } + + //! Function to get the mutual sh acceleration value. + /*! + * Function to get the mutual sh acceleration value, determined from the sum of the two constituent acceleration models. + */ + Eigen::Vector3d getAcceleration( ) + { + return accelerationModelFromShExpansionOfBodyExertingAcceleration_->getAcceleration( ) - + accelerationModelFromShExpansionOfBodyundergoingAcceleration_->getAcceleration( ); + } + + //! Function returning whether the acceleration is expressed in a frame centered on the body exerting the acceleration. + /*! + * Function returning whether the acceleration is expressed in a frame centered on the body exerting the acceleration. + */ + bool getUseCentralBodyFixedFrame( ) + { + return useCentralBodyFixedFrame_; + } + + //! Function to return the function returning the relevant gravitational parameter. + /*! + * Function to return the function returning the relevant gravitational parameter. + * \return Function returning the gravitational parameter used in the computations. + */ + DataReturningFunction getGravitationalParameterFunction( ) + { + return gravitationalParameterFunction_; + } + + //! Function returning the object calculating spherical harmonic acceleration due to the body exerting acceleration + /*! + * Function returning the object calculating spherical harmonic acceleration due to the body exerting acceleration + */ + boost::shared_ptr< SphericalHarmonicsGravitationalAccelerationModel > + getAccelerationModelFromShExpansionOfBodyExertingAcceleration( ) + { + return accelerationModelFromShExpansionOfBodyExertingAcceleration_; + } + + //! Function returning the object calculating spherical harmonic acceleration due to the body undergoing acceleration + /*! + * Function returning the object calculating spherical harmonic acceleration due to the body undergoing acceleration + */ + boost::shared_ptr< SphericalHarmonicsGravitationalAccelerationModel > + getAccelerationModelFromShExpansionOfBodyUndergoingAcceleration( ) + { + return accelerationModelFromShExpansionOfBodyundergoingAcceleration_; + } + +protected: + + //! Boolean denoting whether the acceleration is expressed in a frame centered on the body exerting the acceleration + /*! + * Boolean denoting whether the acceleration is expressed in a frame centered on the body exerting the acceleration, + * in which case the gravitational parameter that is used is the some of the gravitational parameters of both bodies, + * to take into account the inertial acceleration of the reference frame in which the acceleration is performed. + */ + bool useCentralBodyFixedFrame_; + + //! Function returning the gravitational parameter used for both spherical harmonic accelerations. + DataReturningFunction gravitationalParameterFunction_; + + //! Object calculating spherical harmonic acceleration due to the body exerting acceleration + /*! + * Object calculating spherical harmonic acceleration due to the body exerting acceleration + */ + boost::shared_ptr< SphericalHarmonicsGravitationalAccelerationModel > + accelerationModelFromShExpansionOfBodyExertingAcceleration_; + + //! Object calculating spherical harmonic acceleration due to the body undergoing acceleration + /*! + * Object calculating spherical harmonic acceleration due to the body undergoing acceleration, as felt by the body + * undergoing the acceleration due to the figure-point mass coupling between the body undergoing and the body + * exerting the acceleration. Note that this acceleration has no central-central term (i.e. C(0,0) is set to zero), + * as this term is only calculated by accelerationModelFromShExpansionOfBodyExertingAcceleration_. + */ + boost::shared_ptr< SphericalHarmonicsGravitationalAccelerationModel > + accelerationModelFromShExpansionOfBodyundergoingAcceleration_; + + +}; + +} // namespace gravitation + +} // namespace tudat + +#endif // TUDAT_MUTUALSPHERICALHARMONICGRAVITYMODEL_H diff --git a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.cpp b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.cpp index f038a951a7..c3a2cf0383 100644 --- a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.cpp +++ b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.cpp @@ -51,7 +51,7 @@ #include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.h" #include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h" - +#include "Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h" namespace tudat { @@ -61,8 +61,11 @@ namespace gravitation //! Function to calculate the gravitational potential from a spherical harmonic field expansion. double calculateSphericalHarmonicGravitationalPotential( const Eigen::Vector3d& bodyFixedPosition, const double gravitationalParameter, - const double referenceRadius, const Eigen::MatrixXd& cosineCoefficients, - const Eigen::MatrixXd& sineCoefficients, const int minimumumDegree, + const double referenceRadius, + const Eigen::MatrixXd& cosineCoefficients, + const Eigen::MatrixXd& sineCoefficients, + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache, + const int minimumumDegree, const int minimumumOrder ) { // Initialize (distance/reference radius)^n (n=ratioToPowerDegree) @@ -91,9 +94,12 @@ double calculateSphericalHarmonicGravitationalPotential( else { startDegree = minimumumDegree; - ratioToPowerDegree *= std::pow( radiusRatio, startDegree - 1 ); + ratioToPowerDegree *= basic_mathematics::raiseToIntegerPower< double >( radiusRatio, startDegree - 1 ); } + basic_mathematics::LegendreCache& legendreCacheReference = *sphericalHarmonicsCache->getLegendreCache( ); + legendreCacheReference.update( std::sin( latitude ) ); + // Iterate over all degrees for( int degree = startDegree; degree < cosineCoefficients.rows( ); degree++ ) { @@ -105,7 +111,7 @@ double calculateSphericalHarmonicGravitationalPotential( { // Calculate legendre polynomial (geodesy-normalized) at current degree and order legendrePolynomial = basic_mathematics::computeGeodesyLegendrePolynomial( - degree, order, std::sin( latitude ) ); + degree, order, legendreCacheReference ); // Calculate contribution to potential from current degree and order singleDegreeTerm += legendrePolynomial * ( cosineCoefficients( degree, order ) * diff --git a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.h b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.h index a36dc33d6b..0132092882 100644 --- a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.h +++ b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.h @@ -79,6 +79,8 @@ namespace gravitation * \param referenceRadius Reference radius of spherical harmonic field expansion. * \param cosineCoefficients Cosine spherical harmonic coefficients (geodesy normalized). * \param sineCoefficients Sine spherical harmonic coefficients (geodesy normalized). + * \param sphericalHarmonicsCache Cache object containing current values of trigonometric funtions of latitude anf longitude, + * as well as legendre polynomials at current state. * \param minimumumDegree Maximum degree of spherical harmonic expansion. * \param minimumumOrder Maximum order of spherical harmonic expansion. @@ -88,6 +90,7 @@ double calculateSphericalHarmonicGravitationalPotential( const Eigen::Vector3d& bodyFixedPosition, const double gravitationalParameter, const double referenceRadius, const Eigen::MatrixXd& cosineCoefficients, const Eigen::MatrixXd& sineCoefficients, + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache, const int minimumumDegree = 0, const int minimumumOrder = 0 ); //! Class to represent a spherical harmonic gravity field expansion. @@ -118,7 +121,11 @@ class SphericalHarmonicsGravityField: public GravityFieldModel : GravityFieldModel( gravitationalParameter ), referenceRadius_( referenceRadius ), cosineCoefficients_( cosineCoefficients ), sineCoefficients_( sineCoefficients ), fixedReferenceFrame_( fixedReferenceFrame ) - { } + { + sphericalHarmonicsCache_ = boost::make_shared< basic_mathematics::SphericalHarmonicsCache >( ); + sphericalHarmonicsCache_->resetMaximumDegreeAndOrder( cosineCoefficients_.rows( ) + 1, + cosineCoefficients_.cols( ) + 1 ); + } //! Virtual destructor. /*! @@ -263,6 +270,7 @@ class SphericalHarmonicsGravityField: public GravityFieldModel bodyFixedPosition, gravitationalParameter_, referenceRadius_, cosineCoefficients_.block( 0, 0, maximumDegree + 1, maximumOrder + 1 ), sineCoefficients_.block( 0, 0, maximumDegree + 1, maximumOrder + 1 ), + sphericalHarmonicsCache_, minimumDegree, minimumOrder ); } @@ -277,7 +285,7 @@ class SphericalHarmonicsGravityField: public GravityFieldModel Eigen::Vector3d getGradientOfPotential( const Eigen::Vector3d& bodyFixedPosition ) { return getGradientOfPotential( bodyFixedPosition, cosineCoefficients_.rows( ), - sineCoefficients_.cols( ) ); + sineCoefficients_.cols( ) ); } //! Get the gradient of the potential. @@ -289,13 +297,13 @@ class SphericalHarmonicsGravityField: public GravityFieldModel * \return Gradient of potential. */ Eigen::Vector3d getGradientOfPotential( const Eigen::Vector3d& bodyFixedPosition, - const double maximumDegree, - const double maximumOrder ) + const double maximumDegree, + const double maximumOrder ) { return computeGeodesyNormalizedGravitationalAccelerationSum( bodyFixedPosition, gravitationalParameter_, referenceRadius_, cosineCoefficients_.block( 0, 0, maximumDegree, maximumOrder ), - sineCoefficients_.block( 0, 0, maximumDegree, maximumOrder ) ); + sineCoefficients_.block( 0, 0, maximumDegree, maximumOrder ), sphericalHarmonicsCache_ ); } //! Function to retrieve the tdentifier for body-fixed reference frame @@ -333,6 +341,9 @@ class SphericalHarmonicsGravityField: public GravityFieldModel * Identifier for body-fixed reference frame */ std::string fixedReferenceFrame_; + + //! Cache object for potential calculations. + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache_; }; } // namespace gravitation diff --git a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.cpp b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.cpp index 88cbd4a66a..1fa6cad7eb 100644 --- a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.cpp +++ b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.cpp @@ -57,59 +57,34 @@ namespace tudat namespace gravitation { -//! Compute gravitational acceleration due to multiple spherical harmonics terms, defined using -//! geodesy-normalization. +//! Compute gravitational acceleration due to multiple spherical harmonics terms, defined using geodesy-normalization. Eigen::Vector3d computeGeodesyNormalizedGravitationalAccelerationSum( const Eigen::Vector3d& positionOfBodySubjectToAcceleration, const double gravitationalParameter, const double equatorialRadius, const Eigen::MatrixXd& cosineHarmonicCoefficients, - const Eigen::MatrixXd& sineHarmonicCoefficients ) + const Eigen::MatrixXd& sineHarmonicCoefficients, + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache ) { // Set highest degree and order. const int highestDegree = cosineHarmonicCoefficients.rows( ); const int highestOrder = cosineHarmonicCoefficients.cols( ); // Declare spherical position vector. - Eigen::Vector3d sphericalpositionOfBodySubjectToAcceleration = Eigen::Vector3d::Zero( ); + Eigen::Vector3d sphericalpositionOfBodySubjectToAcceleration = coordinate_conversions:: + convertCartesianToSpherical( positionOfBodySubjectToAcceleration ); + sphericalpositionOfBodySubjectToAcceleration( 1 ) = mathematical_constants::PI / 2.0 - + sphericalpositionOfBodySubjectToAcceleration( 1 ); - // Convert Cartesian coordinates to cylindrical. - const Eigen::Vector3d cylindricalCoordinates = coordinate_conversions:: - convertCartesianToCylindrical( positionOfBodySubjectToAcceleration ); + double sineOfAngle = std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) ); + sphericalHarmonicsCache->update( sphericalpositionOfBodySubjectToAcceleration( 0 ), + sineOfAngle, + sphericalpositionOfBodySubjectToAcceleration( 2 ), + equatorialRadius ); - // Compute radius coordinate. - sphericalpositionOfBodySubjectToAcceleration( 0 ) - = std::sqrt( cylindricalCoordinates( 0 ) * cylindricalCoordinates( 0 ) - + cylindricalCoordinates( 2 ) * cylindricalCoordinates( 2 ) ); + boost::shared_ptr< basic_mathematics::LegendreCache > legendreCacheReference = + sphericalHarmonicsCache->getLegendreCache( ); - // If radius coordinate is smaller than planetary radius... - if ( sphericalpositionOfBodySubjectToAcceleration( 0 ) < equatorialRadius ) - { - // ...throw runtime error. - boost::throw_exception( - boost::enable_error_info( - std::runtime_error( - "Distance to origin is smaller than the size of the main body." ) ) ); - } - - // If radius coordinate is zero... - if ( std::fabs( cylindricalCoordinates( 0 ) ) < std::numeric_limits< double >::epsilon( ) ) - { - // ...set latitude coordinate to 90 degrees. - sphericalpositionOfBodySubjectToAcceleration( 1 ) - = mathematical_constants::PI / 2.0; - } - - // Else... - else - { - // ...compute latitude coordinate. - sphericalpositionOfBodySubjectToAcceleration( 1 ) - = std::atan( cylindricalCoordinates( 2 ) / cylindricalCoordinates( 0 ) ); - } - - // Compute longitude coordinate. - sphericalpositionOfBodySubjectToAcceleration( 2 ) = cylindricalCoordinates( 1 ); // Compute gradient premultiplier. const double preMultiplier = gravitationalParameter / equatorialRadius; @@ -121,38 +96,29 @@ Eigen::Vector3d computeGeodesyNormalizedGravitationalAccelerationSum( for ( int degree = 0; degree < highestDegree; degree++ ) { // Loop through all orders. - for ( int order = 0; order <= degree && order < highestOrder; order++ ) + for ( int order = 0; ( order <= degree ) && ( order < highestOrder ); order++ ) { // Compute geodesy-normalized Legendre polynomials. - const double legendrePolynomial = basic_mathematics::computeGeodesyLegendrePolynomial( - degree, order, - std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) ) ); - const double incrementedLegendrePolynomial = - basic_mathematics::computeGeodesyLegendrePolynomial( - degree, order + 1, - std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) ) ); + const double legendrePolynomial = legendreCacheReference->getLegendrePolynomial( degree, order ); // Compute geodesy-normalized Legendre polynomial derivative. - const double legendrePolynomialDerivative = - basic_mathematics::computeGeodesyLegendrePolynomialDerivative( - degree, order, - std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) ), - legendrePolynomial, incrementedLegendrePolynomial ); + const double legendrePolynomialDerivative = legendreCacheReference->getLegendrePolynomialDerivative( + degree, order ); // Compute the potential gradient of a single spherical harmonic term. sphericalGradient += basic_mathematics::computePotentialGradient( sphericalpositionOfBodySubjectToAcceleration, - equatorialRadius, preMultiplier, degree, order, cosineHarmonicCoefficients( degree, order ), sineHarmonicCoefficients( degree, order ), legendrePolynomial, - legendrePolynomialDerivative ); + legendrePolynomialDerivative, sphericalHarmonicsCache ); } } + // Convert from spherical gradient to Cartesian gradient (which equals acceleration vector) and // return the resulting acceleration vector. return coordinate_conversions::convertSphericalToCartesianGradient( @@ -167,78 +133,42 @@ Eigen::Vector3d computeSingleGeodesyNormalizedGravitationalAcceleration( const int degree, const int order, const double cosineHarmonicCoefficient, - const double sineHarmonicCoefficient ) + const double sineHarmonicCoefficient, + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache ) { // Declare spherical position vector. - Eigen::Vector3d sphericalpositionOfBodySubjectToAcceleration; + Eigen::Vector3d sphericalpositionOfBodySubjectToAcceleration = coordinate_conversions:: + convertCartesianToSpherical( positionOfBodySubjectToAcceleration ); + sphericalpositionOfBodySubjectToAcceleration( 1 ) = mathematical_constants::PI / 2.0 - + sphericalpositionOfBodySubjectToAcceleration( 1 ); - // Convert Cartesian coordinates to cylindrical. - const Eigen::Vector3d cylindricalCoordinates = coordinate_conversions:: - convertCartesianToCylindrical( positionOfBodySubjectToAcceleration ); - - // Compute radius coordinate. - sphericalpositionOfBodySubjectToAcceleration( 0 ) - = std::sqrt(cylindricalCoordinates( 0 ) * cylindricalCoordinates( 0 ) - + cylindricalCoordinates( 2 ) * cylindricalCoordinates( 2 ) ); - - // If radius coordinate is smaller than planetary radius... - if (sphericalpositionOfBodySubjectToAcceleration( 0 ) < equatorialRadius) - { - // ...trow runtime error. - boost::throw_exception( - boost::enable_error_info( - std::runtime_error( - "Distance to origin is smaller than the size of the main body." ) ) ); - } - - // If radius coordinate is zero... - if ( std::fabs( cylindricalCoordinates( 0 ) ) < std::numeric_limits< double >::epsilon( ) ) - { - // ...set latitude coordinate to 90 degrees. - sphericalpositionOfBodySubjectToAcceleration( 1 ) - = mathematical_constants::PI / 2.0; - } - - // Else... - else - { - // ...compute latitude coordinate. - sphericalpositionOfBodySubjectToAcceleration( 1 ) - = std::atan( cylindricalCoordinates( 2 ) / cylindricalCoordinates( 0 ) ); - } - // Compute longitude coordinate. - sphericalpositionOfBodySubjectToAcceleration( 2 ) = cylindricalCoordinates( 1 ); + double sineOfAngle = std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) ); + sphericalHarmonicsCache->update( sphericalpositionOfBodySubjectToAcceleration( 0 ), + sineOfAngle, + sphericalpositionOfBodySubjectToAcceleration( 2 ), + equatorialRadius ); // Compute gradient premultiplier. const double preMultiplier = gravitationalParameter / equatorialRadius; // Compute geodesy-normalized Legendre polynomials. - const double legendrePolynomial = basic_mathematics::computeGeodesyLegendrePolynomial( - degree, order, std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) ) ); - const double incrementedLegendrePolynomial = - basic_mathematics::computeGeodesyLegendrePolynomial( - degree, order + 1, - std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) ) ); + const double legendrePolynomial = sphericalHarmonicsCache->getLegendreCache( )->getLegendrePolynomial( degree, order ); // Compute geodesy-normalized Legendre polynomial derivative. const double legendrePolynomialDerivative = - basic_mathematics::computeGeodesyLegendrePolynomialDerivative( - degree, order, - std::sin( sphericalpositionOfBodySubjectToAcceleration( 1 ) ), legendrePolynomial, - incrementedLegendrePolynomial ); + sphericalHarmonicsCache->getLegendreCache( )->getLegendrePolynomialDerivative( degree, order ); - // Compute the potential gradient resulting from the spherical harmonic term. - const Eigen::Vector3d sphericalGradient = basic_mathematics::computePotentialGradient( + // Compute the potential gradient of a single spherical harmonic term. + Eigen::Vector3d sphericalGradient = basic_mathematics::computePotentialGradient( sphericalpositionOfBodySubjectToAcceleration, - equatorialRadius, preMultiplier, degree, order, cosineHarmonicCoefficient, sineHarmonicCoefficient, legendrePolynomial, - legendrePolynomialDerivative ); + legendrePolynomialDerivative, sphericalHarmonicsCache ); // Convert from spherical gradient to Cartesian gradient (which equals acceleration vector), // and return resulting acceleration vector. diff --git a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h index c965473664..0c76337a36 100644 --- a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h +++ b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h @@ -44,17 +44,17 @@ #ifndef TUDAT_SPHERICAL_HARMONICS_GRAVITY_MODEL_H #define TUDAT_SPHERICAL_HARMONICS_GRAVITY_MODEL_H -#include - #include #include #include +#include #include #include #include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" #include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModelBase.h" +#include "Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h" namespace tudat { @@ -88,15 +88,17 @@ namespace gravitation * position( 0 ) = x coordinate [m], * position( 1 ) = y coordinate [m], * position( 2 ) = z coordinate [m]. + * \param gravitationalParameter Gravitational parameter associated with the spherical harmonics + * [m^3 s^-2]. + * \param equatorialRadius Reference radius of the spherical harmonics [m]. * \param cosineHarmonicCoefficients Matrix with geodesy-normalized cosine harmonic * coefficients. The row index indicates the degree and the column index indicates the order * of coefficients. * \param sineHarmonicCoefficients Matrix with geodesy-normalized sine harmonic coefficients. * The row index indicates the degree and the column index indicates the order of * coefficients. The matrix must be equal in size to cosineHarmonicCoefficients. - * \param gravitationalParameter Gravitational parameter associated with the spherical harmonics - * [m^3 s^-2]. - * \param equatorialRadius Reference radius of the spherical harmonics [m]. + * \param sphericalHarmonicsCache Cache object for computing/retrieving repeated terms in spherical harmonics potential + * gradient calculation. * \return Cartesian acceleration vector resulting from the summation of all harmonic terms. * The order is important! * acceleration( 0 ) = x acceleration [m s^-2], @@ -108,7 +110,8 @@ Eigen::Vector3d computeGeodesyNormalizedGravitationalAccelerationSum( const double gravitationalParameter, const double equatorialRadius, const Eigen::MatrixXd& cosineHarmonicCoefficients, - const Eigen::MatrixXd& sineHarmonicCoefficients ); + const Eigen::MatrixXd& sineHarmonicCoefficients, + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache ); //! Compute gravitational acceleration due to single spherical harmonics term. /*! @@ -137,12 +140,14 @@ Eigen::Vector3d computeGeodesyNormalizedGravitationalAccelerationSum( * position( 2 ) = z coordinate [m]. * \param degree Degree of the harmonic term. * \param order Order of the harmonic term. - * \param cosineHarmonicCoefficient Geodesy-normalized cosine harmonic + * * \param cosineHarmonicCoefficient Geodesy-normalized cosine harmonic * coefficient. * \param sineHarmonicCoefficient Geodesy-normalized sine harmonic coefficient. * \param gravitationalParameter Gravitational parameter associated with the spherical harmonic * [m^3 s^-2]. * \param equatorialRadius Reference radius of the spherical harmonic [m]. + * \param sphericalHarmonicsCache Cache object for computing/retrieving repeated terms in spherical harmonics potential + * gradient calculation. * \return Cartesian acceleration vector resulting from the spherical harmonic term. * The order is important! * acceleration( 0 ) = x acceleration [m s^-2], @@ -156,7 +161,8 @@ Eigen::Vector3d computeSingleGeodesyNormalizedGravitationalAcceleration( const int degree, const int order, const double cosineHarmonicCoefficient, - const double sineHarmonicCoefficient ); + const double sineHarmonicCoefficient, + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache ); //! Template class for general spherical harmonics gravitational acceleration model. /*! @@ -165,10 +171,7 @@ Eigen::Vector3d computeSingleGeodesyNormalizedGravitationalAcceleration( * (Heiskanen & Moritz, 1967), implemented in the * computeGeodesyNormalizedGravitationalAccelerationSum() function. The acceleration computed is a * sum, based on the matrix of coefficients of the model provided. - * \tparam CoefficientMatrixType Data type for cosine and sine coefficients in spherical harmonics - * expansion; may be used for compile-time definition of maximum degree and order. */ -template< typename CoefficientMatrixType = Eigen::MatrixXd > class SphericalHarmonicsGravitationalAccelerationModel : public basic_astrodynamics::AccelerationModel< Eigen::Vector3d >, public SphericalHarmonicsGravitationalAccelerationModelBase< Eigen::Vector3d > @@ -179,7 +182,7 @@ class SphericalHarmonicsGravitationalAccelerationModel typedef SphericalHarmonicsGravitationalAccelerationModelBase< Eigen::Vector3d > Base; //! Typedef for coefficient-matrix-returning function. - typedef boost::function< CoefficientMatrixType( ) > CoefficientMatrixReturningFunction; + typedef boost::function< Eigen::MatrixXd( ) > CoefficientMatrixReturningFunction; public: @@ -205,28 +208,44 @@ class SphericalHarmonicsGravitationalAccelerationModel * body exerting gravitational acceleration (default = (0,0,0)). * \param rotationFromBodyFixedToIntegrationFrameFunction Function providing the rotation from * body-fixes from to the frame in which the numerical integration is performed. + * \param isMutualAttractionUsed Variable denoting whether attraction from body undergoing acceleration on + * body exerting acceleration is included (i.e. whether aGravitationalParameter refers to the property + * of the body exerting the acceleration, if variable is false, or the sum of the gravitational parameters, + * if the variable is true. + * \param sphericalHarmonicsCache Cache object for computing/retrieving repeated terms in spherical harmonics potential + * gradient calculation. */ SphericalHarmonicsGravitationalAccelerationModel( const StateFunction positionOfBodySubjectToAccelerationFunction, const double aGravitationalParameter, const double anEquatorialRadius, - const CoefficientMatrixType aCosineHarmonicCoefficientMatrix, - const CoefficientMatrixType aSineHarmonicCoefficientMatrix, + const Eigen::MatrixXd aCosineHarmonicCoefficientMatrix, + const Eigen::MatrixXd aSineHarmonicCoefficientMatrix, const StateFunction positionOfBodyExertingAccelerationFunction = boost::lambda::constant( Eigen::Vector3d::Zero( ) ), const boost::function< Eigen::Quaterniond( ) > rotationFromBodyFixedToIntegrationFrameFunction = - boost::lambda::constant( Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ) ) ) + boost::lambda::constant( Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ) ), + const bool isMutualAttractionUsed = 0, + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache = + boost::make_shared< basic_mathematics::SphericalHarmonicsCache >( ) ) : Base( positionOfBodySubjectToAccelerationFunction, aGravitationalParameter, - positionOfBodyExertingAccelerationFunction ), + positionOfBodyExertingAccelerationFunction, + isMutualAttractionUsed ), equatorialRadius( anEquatorialRadius ), getCosineHarmonicsCoefficients( boost::lambda::constant(aCosineHarmonicCoefficientMatrix ) ), getSineHarmonicsCoefficients( boost::lambda::constant(aSineHarmonicCoefficientMatrix ) ), rotationFromBodyFixedToIntegrationFrameFunction_( - rotationFromBodyFixedToIntegrationFrameFunction ) + rotationFromBodyFixedToIntegrationFrameFunction ), + sphericalHarmonicsCache_( sphericalHarmonicsCache ), + currentAcceleration_( Eigen::Vector3d::Zero( ) ) + { + sphericalHarmonicsCache_->resetMaximumDegreeAndOrder( + std::max< int >( static_cast< int >( getCosineHarmonicsCoefficients( ).rows( ) ), sphericalHarmonicsCache_->getMaximumDegree( ) ), + std::max< int >( static_cast< int >( getCosineHarmonicsCoefficients( ).cols( ) ), sphericalHarmonicsCache_->getMaximumOrder( ) ) + 1 ); this->updateMembers( ); } @@ -251,6 +270,11 @@ class SphericalHarmonicsGravitationalAccelerationModel * body exerting gravitational acceleration (default = (0,0,0)). * \param rotationFromBodyFixedToIntegrationFrameFunction Function providing the rotation from * body-fixes from to the frame in which the numerical integration is performed. + * \param isMutualAttractionUsed Variable denoting whether attraction from body undergoing acceleration on + * body exerting acceleration is included (i.e. whether aGravitationalParameter refers to the property + * of the body exerting the acceleration, if variable is false, or the sum of the gravitational parameters, + * if the variable is true. + * \param sphericalHarmonicsCache Cache object for computing/retrieving repeated terms in spherical harmonics potential */ SphericalHarmonicsGravitationalAccelerationModel( const StateFunction positionOfBodySubjectToAccelerationFunction, @@ -262,15 +286,26 @@ class SphericalHarmonicsGravitationalAccelerationModel = boost::lambda::constant( Eigen::Vector3d::Zero( ) ), const boost::function< Eigen::Quaterniond( ) > rotationFromBodyFixedToIntegrationFrameFunction = - boost::lambda::constant( Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ) ) ) + boost::lambda::constant( Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ) ), + const bool isMutualAttractionUsed = 0, + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache + = boost::make_shared< basic_mathematics::SphericalHarmonicsCache >( ) ) : Base( positionOfBodySubjectToAccelerationFunction, aGravitationalParameterFunction, - positionOfBodyExertingAccelerationFunction ), + positionOfBodyExertingAccelerationFunction, + isMutualAttractionUsed ), equatorialRadius( anEquatorialRadius ), getCosineHarmonicsCoefficients( cosineHarmonicCoefficientsFunction ), getSineHarmonicsCoefficients( sineHarmonicCoefficientsFunction ), - rotationFromBodyFixedToIntegrationFrameFunction_( rotationFromBodyFixedToIntegrationFrameFunction ) + rotationFromBodyFixedToIntegrationFrameFunction_( rotationFromBodyFixedToIntegrationFrameFunction ), + sphericalHarmonicsCache_( sphericalHarmonicsCache ), + currentAcceleration_( Eigen::Vector3d::Zero( ) ) { + sphericalHarmonicsCache_->resetMaximumDegreeAndOrder( + std::max< int >( static_cast< int >( getCosineHarmonicsCoefficients( ).rows( ) ), sphericalHarmonicsCache_->getMaximumDegree( ) ), + std::max< int >( static_cast< int >( getCosineHarmonicsCoefficients( ).cols( ) ), sphericalHarmonicsCache_->getMaximumOrder( ) ) + 1 ); + + this->updateMembers( ); } @@ -281,7 +316,10 @@ class SphericalHarmonicsGravitationalAccelerationModel * computeGeodesyNormalizedGravitationalAccelerationSum() function. * \return Computed gravitational acceleration vector. */ - Eigen::Vector3d getAcceleration( ); + Eigen::Vector3d getAcceleration( ) + { + return currentAcceleration_; + } //! Update class members. /*! @@ -297,9 +335,75 @@ class SphericalHarmonicsGravitationalAccelerationModel sineHarmonicCoefficients = getSineHarmonicsCoefficients( ); rotationToIntegrationFrame_ = rotationFromBodyFixedToIntegrationFrameFunction_( ); this->updateBaseMembers( ); + currentAcceleration_ = rotationToIntegrationFrame_ * + computeGeodesyNormalizedGravitationalAccelerationSum( + rotationToIntegrationFrame_.inverse( ) * ( + this->positionOfBodySubjectToAcceleration - this->positionOfBodyExertingAcceleration ), + gravitationalParameter, + equatorialRadius, + cosineHarmonicCoefficients, + sineHarmonicCoefficients, sphericalHarmonicsCache_ ); } } + //! Function to retrieve the spherical harmonics cache for this acceleration. + /*! + * Function to retrieve the spherical harmonics cache for this acceleration. + * \return Spherical harmonics cache for this acceleration + */ + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > getSphericalHarmonicsCache( ) + { + return sphericalHarmonicsCache_; + } + + //! Function to retrieve the spherical harmonics reference radius. + /*! + * Function to retrieve the spherical harmonics reference radius. + * \return Spherical harmonics reference radius. + */ + double getReferenceRadius( ) + { + return equatorialRadius; + } + + //! Matrix of cosine coefficients. + /*! + * Matrix containing coefficients of cosine terms for spherical harmonics expansion. + */ + CoefficientMatrixReturningFunction getCosineHarmonicCoefficientsFunction( ) + { + return getCosineHarmonicsCoefficients; + } + + //! Matrix of sine coefficients. + /*! + * Matrix containing coefficients of sine terms for spherical harmonics expansion. + */ + CoefficientMatrixReturningFunction getSineHarmonicCoefficientsFunction( ) + { + return getSineHarmonicsCoefficients; + } + + //! Function to retrieve the current rotation from body-fixed frame to integration frame, in the form of a quaternion. + /*! + * Function to retrieve the current rotation from body-fixed frame to integration frame, in the form of a quaternion. + * \return current rotation from body-fixed frame to integration frame, in the form of a quaternion. + */ + Eigen::Quaterniond getCurrentRotationToIntegrationFrame( ) + { + return rotationToIntegrationFrame_; + } + + //! Function to retrieve the current rotation from body-fixed frame to integration frame, as a rotation matrix. + /*! + * Function to retrieve the current rotation from body-fixed frame to integration frame, as a rotation matrix. + * \return current rotation from body-fixed frame to integration frame, as a rotation matrix. + */ + Eigen::Matrix3d getCurrentRotationToIntegrationFrameMatrix( ) + { + return rotationToIntegrationFrame_.toRotationMatrix( ); + } + protected: private: @@ -314,13 +418,13 @@ class SphericalHarmonicsGravitationalAccelerationModel /*! * Matrix containing coefficients of cosine terms for spherical harmonics expansion. */ - CoefficientMatrixType cosineHarmonicCoefficients; + Eigen::MatrixXd cosineHarmonicCoefficients; //! Matrix of sine coefficients. /*! * Matrix containing coefficients of sine terms for spherical harmonics expansion. */ - CoefficientMatrixType sineHarmonicCoefficients; + Eigen::MatrixXd sineHarmonicCoefficients; //! Pointer to function returning cosine harmonics coefficients matrix. /*! @@ -342,32 +446,22 @@ class SphericalHarmonicsGravitationalAccelerationModel //! Current rotation from body-fixed frame to integration frame. Eigen::Quaterniond rotationToIntegrationFrame_; -}; + //! Spherical harmonics cache for this acceleration + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache_; + + //! Current acceleration, as computed by last call to updateMembers function + Eigen::Vector3d currentAcceleration_; -//! Typedef for SphericalHarmonicsGravitationalAccelerationModelXd. -typedef SphericalHarmonicsGravitationalAccelerationModel< > -SphericalHarmonicsGravitationalAccelerationModelXd; +}; -//! Typedef for shared-pointer to SphericalHarmonicsGravitationalAccelerationModelXd. -typedef boost::shared_ptr< SphericalHarmonicsGravitationalAccelerationModelXd > -SphericalHarmonicsGravitationalAccelerationModelXdPointer; -// Template class source. -// The code given below is effectively the ".cpp file" for the template class definition, so you -// only need to look at the code below if you are interested in the source implementation. +//! Typedef for shared-pointer to SphericalHarmonicsGravitationalAccelerationModel. +typedef boost::shared_ptr< SphericalHarmonicsGravitationalAccelerationModel > +SphericalHarmonicsGravitationalAccelerationModelPointer; -//! Get gravitational acceleration. -template< typename CoefficientMatrixType > -Eigen::Vector3d SphericalHarmonicsGravitationalAccelerationModel< CoefficientMatrixType > -::getAcceleration( ) -{ - return rotationToIntegrationFrame_ * computeGeodesyNormalizedGravitationalAccelerationSum( - rotationToIntegrationFrame_.inverse( ) * - ( this->positionOfBodySubjectToAcceleration - this->positionOfBodyExertingAcceleration ), - gravitationalParameter, equatorialRadius, cosineHarmonicCoefficients, sineHarmonicCoefficients ); -} } // namespace gravitation + } // namespace tudat #endif // TUDAT_SPHERICAL_HARMONICS_GRAVITY_MODEL_H diff --git a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModelBase.h b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModelBase.h index ba07f8f811..8f44e15c3d 100644 --- a/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModelBase.h +++ b/Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModelBase.h @@ -75,14 +75,20 @@ class SphericalHarmonicsGravitationalAccelerationModelBase * \param aGravitationalParameter Gravitational parameter of body exerting gravitational acceleration. * \param positionOfBodyExertingAccelerationFunction Pointer to function returning position of * body exerting gravitational acceleration. + * \param isMutualAttractionUsed Variable denoting whether attraction from body undergoing acceleration on + * body exerting acceleration is included (i.e. whether aGravitationalParameter refers to the property + * of the body exerting the acceleration, if variable is false, or the sum of the gravitational parameters, + * if the variable is true. */ SphericalHarmonicsGravitationalAccelerationModelBase( const StateFunction positionOfBodySubjectToAccelerationFunction, const double aGravitationalParameter, - const StateFunction positionOfBodyExertingAccelerationFunction ) + const StateFunction positionOfBodyExertingAccelerationFunction, + const bool isMutualAttractionUsed ) : subjectPositionFunction( positionOfBodySubjectToAccelerationFunction ), gravitationalParameterFunction( boost::lambda::constant( aGravitationalParameter ) ), - sourcePositionFunction( positionOfBodyExertingAccelerationFunction ) + sourcePositionFunction( positionOfBodyExertingAccelerationFunction ), + isMutualAttractionUsed_( isMutualAttractionUsed ) { } //! Default constructor taking position of body subject to acceleration, variable @@ -99,14 +105,20 @@ class SphericalHarmonicsGravitationalAccelerationModelBase * of body exerting gravitational acceleration. * \param positionOfBodyExertingAccelerationFunction Pointer to function returning position of * body exerting gravitational acceleration. + * \param isMutualAttractionUsed Variable denoting whether attraction from body undergoing acceleration on + * body exerting acceleration is included (i.e. whether aGravitationalParameter refers to the property + * of the body exerting the acceleration, if variable is false, or the sum of the gravitational parameters, + * if the variable is true. */ SphericalHarmonicsGravitationalAccelerationModelBase( const StateFunction positionOfBodySubjectToAccelerationFunction, const boost::function< double( ) > aGravitationalParameterFunction, - const StateFunction positionOfBodyExertingAccelerationFunction ) + const StateFunction positionOfBodyExertingAccelerationFunction, + const bool isMutualAttractionUsed ) : subjectPositionFunction( positionOfBodySubjectToAccelerationFunction ), gravitationalParameterFunction( aGravitationalParameterFunction ), - sourcePositionFunction( positionOfBodyExertingAccelerationFunction ) + sourcePositionFunction( positionOfBodyExertingAccelerationFunction ), + isMutualAttractionUsed_( isMutualAttractionUsed ) { } //! Virtual destructor. @@ -123,14 +135,68 @@ class SphericalHarmonicsGravitationalAccelerationModelBase * \return True; this should be modified to return a flag indicating if the update was * successful. */ - bool updateBaseMembers( ) + void updateBaseMembers( ) { this->gravitationalParameter = this->gravitationalParameterFunction( ); this->positionOfBodySubjectToAcceleration = this->subjectPositionFunction( ); this->positionOfBodyExertingAcceleration = this->sourcePositionFunction( ); - return true; } + //! Function to return the function returning the relevant gravitational parameter. + /*! + * Function to return the function returning the relevant gravitational parameter. + * \return Function returning the gravitational parameter used in the computations. + */ + boost::function< double( ) > getGravitationalParameterFunction( ) + { return gravitationalParameterFunction; } + + //! Function to return the function returning position of body exerting acceleration. + /*! + * Function to return the function returning position of body exerting acceleration. + * \return Function returning position of body exerting acceleration. + */ + boost::function< StateMatrix( ) > getStateFunctionOfBodyExertingAcceleration( ) + { return sourcePositionFunction; } + + //! Function to return the function returning position of body subject to acceleration. + /*! + * Function to return the function returning position of body subject to acceleration. + * \return Function returning position of body subject to acceleration. + */ + boost::function< StateMatrix( ) > getStateFunctionOfBodyUndergoingAcceleration( ) + { return subjectPositionFunction; } + + //! Function to return whether the mutual attraction is used. + /*! + * Function to return whether the mutual attraction is used. + * \return Boolean defining whether the mutual attraction is used. + */ + bool getIsMutualAttractionUsed( ) + { + return isMutualAttractionUsed_; + } + + //! Function to return current position vector of body exerting gravitational acceleration in inertial frame. + /*! + * Function to return current position vector of body exerting gravitational acceleration in inertial frame. + * \return Current position vector of body exerting gravitational acceleration in inertial frame. + */ + StateMatrix getCurrentPositionOfBodySubjectToAcceleration( ) + { + return positionOfBodySubjectToAcceleration; + } + + //! Function to return current position vector of body undergoing gravitational acceleration in inertial frame. + /*! + * Function to return current position vector of body undergoing gravitational acceleration in inertial frame. + * \return Current position vector of body undergoing gravitational acceleration in inertial frame. + */ + StateMatrix getCurrentPositionOfBodyExertingAcceleration( ) + { + return positionOfBodyExertingAcceleration; + } + + protected: //! Position of body subject to acceleration. @@ -171,6 +237,16 @@ class SphericalHarmonicsGravitationalAccelerationModelBase */ const StateFunction sourcePositionFunction; + //! Variable denoting whether mutual acceleration between bodies is included. + /*! + * Variable denoting whether attraction from body undergoing acceleration on + * body exerting acceleration is included (i.e. whether aGravitationalParameter refers to the property + * of the body exerting the acceleration, if variable is false, or the sum of the gravitational parameters, + * if the variable is true. + */ + bool isMutualAttractionUsed_; + + private: }; diff --git a/Tudat/Astrodynamics/Gravitation/tabulatedGravityFieldVariations.cpp b/Tudat/Astrodynamics/Gravitation/tabulatedGravityFieldVariations.cpp index b0629678da..8bfcbcf16d 100644 --- a/Tudat/Astrodynamics/Gravitation/tabulatedGravityFieldVariations.cpp +++ b/Tudat/Astrodynamics/Gravitation/tabulatedGravityFieldVariations.cpp @@ -45,8 +45,7 @@ void TabulatedGravityFieldVariations::resetCoefficientInterpolator( // Check consistency of map sizes. if( cosineCoefficientCorrections_.size( ) != sineCoefficientCorrections_.size( ) ) { - std::cerr<<"Error when resetting tabulated gravity field corrections, sine and "<< - "cosine data size incompatible"<first != sineIterator->first ) { - std::cerr<<"Warning when resetting tabulated gravity field corrections, sine and "<< - "cosine data time differ by "<< - cosineIterator->first - sineIterator->first<( cosineIterator->first - sineIterator->first ); + throw std::runtime_error( errorMessage ); } - // Check whether matrix sizes are consistent. if( ( cosineIterator->second.rows( ) != numberOfDegrees_ ) || ( sineIterator->second.rows( ) != numberOfDegrees_ ) || ( cosineIterator->second.cols( ) != numberOfOrders_ ) || ( sineIterator->second.cols( ) != numberOfOrders_ ) ) { - std::cerr<<"Error when resetting tabulated gravity field corrections, sine and "<< - "cosine blocks of inconsistent size"<currentTime_ == currentTime ) ) { // Update two constituent acceleration models. - accelerationModelForBodyUndergoingAcceleration_->updateMembers( ); - accelerationModelForCentralBody_->updateMembers( ); + accelerationModelForBodyUndergoingAcceleration_->updateMembers( currentTime ); + accelerationModelForCentralBody_->updateMembers( currentTime ); } } + //! Function to reset the current time + /*! + * Function to reset the current time of the acceleration model. + * \param currentTime Current time (default NaN). + */ + void resetTime( const double currentTime = TUDAT_NAN ) + { + accelerationModelForBodyUndergoingAcceleration_->resetTime( currentTime ); + accelerationModelForCentralBody_->resetTime( currentTime ); + } + //! Function to return the direct acceleration model on body undergoing acceleration. /*! * Function to return the direct acceleration model on body undergoing acceleration. @@ -207,9 +219,14 @@ typedef ThirdBodyAcceleration< CentralGravitationalAccelerationModel3d > ThirdBodyCentralGravityAcceleration; //! Typedef for third body spherical harmonic gravity acceleration. -typedef ThirdBodyAcceleration< SphericalHarmonicsGravitationalAccelerationModelXd > +typedef ThirdBodyAcceleration< SphericalHarmonicsGravitationalAccelerationModel > ThirdBodySphericalHarmonicsGravitationalAccelerationModel; +//! Typedef for third body mutual spherical harmonic gravity acceleration. +typedef ThirdBodyAcceleration< MutualSphericalHarmonicsGravitationalAccelerationModel > +ThirdBodyMutualSphericalHarmonicsGravitationalAccelerationModel; + + } // namespace gravitation } // namespace tudat diff --git a/Tudat/Astrodynamics/Gravitation/timeDependentSphericalHarmonicsGravityField.h b/Tudat/Astrodynamics/Gravitation/timeDependentSphericalHarmonicsGravityField.h index 9522fc3219..afe752f223 100644 --- a/Tudat/Astrodynamics/Gravitation/timeDependentSphericalHarmonicsGravityField.h +++ b/Tudat/Astrodynamics/Gravitation/timeDependentSphericalHarmonicsGravityField.h @@ -123,8 +123,7 @@ class TimeDependentSphericalHarmonicsGravityField: public SphericalHarmonicsGrav // Check if field variation set exists. if( gravityFieldVariationsSet_ == NULL ) { - std::cerr << "Warning, gravity field coefficient update functions are NULL " << - " when requesting update" << std::endl; + throw std::runtime_error( "Warning, gravity field coefficient update functions are NULL when requesting update" ); } else { @@ -190,7 +189,7 @@ class TimeDependentSphericalHarmonicsGravityField: public SphericalHarmonicsGrav } else { - std::cerr << "Error when resetting nominal cosine coefficient" << std::endl; + throw std::runtime_error( "Error when resetting nominal cosine coefficient" ); } } @@ -230,7 +229,7 @@ class TimeDependentSphericalHarmonicsGravityField: public SphericalHarmonicsGrav } else { - std::cerr << "Error when resetting nominal sine coefficient" << std::endl; + throw std::runtime_error( "Error when resetting nominal sine coefficient" ); } } diff --git a/Tudat/Astrodynamics/MissionSegments/CMakeLists.txt b/Tudat/Astrodynamics/MissionSegments/CMakeLists.txt index 9dca05acb7..e6beb532e3 100755 --- a/Tudat/Astrodynamics/MissionSegments/CMakeLists.txt +++ b/Tudat/Astrodynamics/MissionSegments/CMakeLists.txt @@ -108,7 +108,6 @@ add_executable(test_MultiRevolutionLambertTargeterIzzo "${SRCROOT}${MISSIONSEGME setup_custom_test_program(test_MultiRevolutionLambertTargeterIzzo "${SRCROOT}${MISSIONSEGMENTSDIR}") target_link_libraries(test_MultiRevolutionLambertTargeterIzzo tudat_mission_segments tudat_root_finders tudat_basic_astrodynamics tudat_basic_mathematics ${Boost_LIBRARIES}) -add_executable(test_MathematicalShapeFunctions -"${SRCROOT}${MISSIONSEGMENTSDIR}/UnitTests/unitTestMathematicalShapeFunctions.cpp") +add_executable(test_MathematicalShapeFunctions "${SRCROOT}${MISSIONSEGMENTSDIR}/UnitTests/unitTestMathematicalShapeFunctions.cpp") setup_custom_test_program(test_MathematicalShapeFunctions "${SRCROOT}${MISSIONSEGMENTSDIR}") target_link_libraries(test_MathematicalShapeFunctions tudat_mission_segments tudat_basic_mathematics ${Boost_LIBRARIES}) diff --git a/Tudat/Astrodynamics/MissionSegments/lambertRoutines.cpp b/Tudat/Astrodynamics/MissionSegments/lambertRoutines.cpp index 22991e2df7..0f16871ea9 100644 --- a/Tudat/Astrodynamics/MissionSegments/lambertRoutines.cpp +++ b/Tudat/Astrodynamics/MissionSegments/lambertRoutines.cpp @@ -226,8 +226,9 @@ void solveLambertProblemIzzo( const Eigen::Vector3d& cartesianPositionAtDepartur // Verify that root-finder has converged. if ( iterator == maximumNumberOfIterations ) { - std::cerr << "Lambert Solver did not converge within the maximum number of iterations (" - << maximumNumberOfIterations << ")." << std::endl; + std::string errorMessage = "Lambert Solver did not converge within the maximum number of iterations: " + + boost::lexical_cast< std::string >( maximumNumberOfIterations ); + throw std::runtime_error( errorMessage ); } // Revert to x parameter. diff --git a/Tudat/Astrodynamics/MissionSegments/zeroRevolutionLambertTargeterIzzo.h b/Tudat/Astrodynamics/MissionSegments/zeroRevolutionLambertTargeterIzzo.h index 822b15b502..2242ef0f49 100644 --- a/Tudat/Astrodynamics/MissionSegments/zeroRevolutionLambertTargeterIzzo.h +++ b/Tudat/Astrodynamics/MissionSegments/zeroRevolutionLambertTargeterIzzo.h @@ -46,8 +46,8 @@ * */ -#ifndef ZERO_REVOLUTION_LAMBERT_TARGETER_IZZO_H -#define ZERO_REVOLUTION_LAMBERT_TARGETER_IZZO_H +#ifndef TUDAT_ZERO_REVOLUTION_LAMBERT_TARGETER_IZZO_H +#define TUDAT_ZERO_REVOLUTION_LAMBERT_TARGETER_IZZO_H #include // For Vector3d @@ -323,4 +323,4 @@ class ZeroRevolutionLambertTargeterIzzo : public LambertTargeter } // namespace mission_segments } // namespace tudat -#endif // ZERO_REVOLUTION_LAMBERT_TARGETER_IZZO_H +#endif // TUDAT_ZERO_REVOLUTION_LAMBERT_TARGETER_IZZO_H diff --git a/Tudat/Astrodynamics/ObservationModels/CMakeLists.txt b/Tudat/Astrodynamics/ObservationModels/CMakeLists.txt index 08a4a6c7f2..fd036081f5 100644 --- a/Tudat/Astrodynamics/ObservationModels/CMakeLists.txt +++ b/Tudat/Astrodynamics/ObservationModels/CMakeLists.txt @@ -14,16 +14,32 @@ # 130226 D. Dirkx File created. # + +set(OBSERVABLECORRECTIONSDIR "${OBSERVATIONMODELSDIR}/ObservableCorrections") + + # Set the source files. set(OBSERVATION_MODELS_SOURCES "${SRCROOT}${OBSERVATIONMODELSDIR}/lightTimeSolution.cpp" + "${SRCROOT}${OBSERVATIONMODELSDIR}/observableTypes.cpp" + "${SRCROOT}${OBSERVABLECORRECTIONSDIR}/firstOrderRelativisticLightTimeCorrection.cpp" ) # Set the header files. set(OBSERVATION_MODELS_HEADERS + "${SRCROOT}${OBSERVATIONMODELSDIR}/angularPositionObservationModel.h" "${SRCROOT}${OBSERVATIONMODELSDIR}/lightTimeSolution.h" + "${SRCROOT}${OBSERVATIONMODELSDIR}/linkTypeDefs.h" + "${SRCROOT}${OBSERVATIONMODELSDIR}/observableTypes.h" + "${SRCROOT}${OBSERVATIONMODELSDIR}/observationModel.h" + "${SRCROOT}${OBSERVATIONMODELSDIR}/observationBias.h" + "${SRCROOT}${OBSERVATIONMODELSDIR}/oneWayRangeObservationModel.h" + "${SRCROOT}${OBSERVATIONMODELSDIR}/positionObservationModel.h" "${SRCROOT}${OBSERVATIONMODELSDIR}/UnitTests/testLightTimeCorrections.h" - ) + "${SRCROOT}${OBSERVABLECORRECTIONSDIR}/lightTimeCorrection.h" + "${SRCROOT}${OBSERVABLECORRECTIONSDIR}/firstOrderRelativisticLightTimeCorrection.h" +) + # Add static libraries. add_library(tudat_observation_models STATIC ${OBSERVATION_MODELS_SOURCES} ${OBSERVATION_MODELS_HEADERS}) @@ -33,6 +49,18 @@ if(USE_CSPICE) add_executable(test_LightTime "${SRCROOT}${OBSERVATIONMODELSDIR}/UnitTests/unitTestLightTimeSolution.cpp") setup_custom_test_program(test_LightTime "${SRCROOT}${OBSERVATIONMODELSDIR}") - target_link_libraries(test_LightTime tudat_observation_models tudat_ephemerides tudat_spice_interface ${SPICE_LIBRARIES} ${Boost_LIBRARIES}) + target_link_libraries(test_LightTime tudat_observation_models tudat_ephemerides tudat_basic_astrodynamics tudat_spice_interface ${SPICE_LIBRARIES} ${Boost_LIBRARIES}) + + add_executable(test_AngularPositionModel "${SRCROOT}${OBSERVATIONMODELSDIR}/UnitTests/unitTestAngularPositionModel.cpp") + setup_custom_test_program(test_AngularPositionModel "${SRCROOT}${OBSERVATIONMODELSDIR}") + target_link_libraries(test_AngularPositionModel ${TUDAT_ESTIMATION_LIBRARIES} ${Boost_LIBRARIES}) + + add_executable(test_OneWayRangeModel "${SRCROOT}${OBSERVATIONMODELSDIR}/UnitTests/unitTestOneWayRangeObservationModel.cpp") + setup_custom_test_program(test_OneWayRangeModel "${SRCROOT}${OBSERVATIONMODELSDIR}") + target_link_libraries(test_OneWayRangeModel ${TUDAT_ESTIMATION_LIBRARIES} ${Boost_LIBRARIES}) + + add_executable(test_PositionObservationModel "${SRCROOT}${OBSERVATIONMODELSDIR}/UnitTests/unitTestPositionObservationModel.cpp") + setup_custom_test_program(test_PositionObservationModel "${SRCROOT}${OBSERVATIONMODELSDIR}") + target_link_libraries(test_PositionObservationModel ${TUDAT_ESTIMATION_LIBRARIES} ${Boost_LIBRARIES}) endif( ) diff --git a/Tudat/Astrodynamics/ObservationModels/ObservableCorrections/firstOrderRelativisticLightTimeCorrection.cpp b/Tudat/Astrodynamics/ObservationModels/ObservableCorrections/firstOrderRelativisticLightTimeCorrection.cpp new file mode 100644 index 0000000000..bd3d97a90f --- /dev/null +++ b/Tudat/Astrodynamics/ObservationModels/ObservableCorrections/firstOrderRelativisticLightTimeCorrection.cpp @@ -0,0 +1,56 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h" + +#include "Tudat/Astrodynamics/Relativity/relativisticLightTimeCorrection.h" +#include "Tudat/Astrodynamics/ObservationModels/ObservableCorrections/firstOrderRelativisticLightTimeCorrection.h" + + +namespace tudat +{ + +namespace observation_models +{ + +//! Function to calculate first order relativistic light time correction due to set of gravitating point masses. +double FirstOrderLightTimeCorrectionCalculator::calculateLightTimeCorrection( + const basic_mathematics::Vector6d& transmitterState, + const basic_mathematics::Vector6d& receiverState, + const double transmissionTime, + const double receptionTime ) +{ + // Retrieve ppn parameter gamma. + double ppnParameterGamma = ppnParameterGammaFunction_( ); + + // Initialize correction to zero. + currentTotalLightTimeCorrection_ = 0.0; + + double evaluationTime = TUDAT_NAN; + // Iterate over all gravitating bodies. + for( unsigned int i = 0; i < perturbingBodyStateFunctions_.size( ); i++ ) + { + evaluationTime = transmissionTime + lightTimeEvaluationContribution_.at( i ) * ( receptionTime - transmissionTime ); + // Calculate correction due to current body and add to total. + currentLighTimeCorrectionComponents_[ i ] = relativity::calculateFirstOrderLightTimeCorrectionFromCentralBody( + perturbingBodyGravitationalParameterFunctions_[ i ]( ), + transmitterState.segment( 0, 3 ), receiverState.segment( 0, 3 ), + perturbingBodyStateFunctions_[ i ]( evaluationTime ).segment( 0, 3 ), + ppnParameterGamma ); + currentTotalLightTimeCorrection_ += currentLighTimeCorrectionComponents_[ i ]; + } + + return currentTotalLightTimeCorrection_; +} + +} + +} + diff --git a/Tudat/Astrodynamics/ObservationModels/ObservableCorrections/firstOrderRelativisticLightTimeCorrection.h b/Tudat/Astrodynamics/ObservationModels/ObservableCorrections/firstOrderRelativisticLightTimeCorrection.h new file mode 100644 index 0000000000..ed23d89617 --- /dev/null +++ b/Tudat/Astrodynamics/ObservationModels/ObservableCorrections/firstOrderRelativisticLightTimeCorrection.h @@ -0,0 +1,198 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_FIRSTORDERRELATIVISTICLIGHTTIMECORRECTION_H +#define TUDAT_FIRSTORDERRELATIVISTICLIGHTTIMECORRECTION_H + +#include +#include + +#include +#include + +#include + +#include "Tudat/Mathematics/BasicMathematics/linearAlgebraTypes.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h" +#include "Tudat/Astrodynamics/ObservationModels/ObservableCorrections/lightTimeCorrection.h" + +namespace tudat +{ + +namespace observation_models +{ + +//! Class to calculate first order relativistic light time correction (Shapiro time delay) due to a set of point masses. +/*! + * Class to calculate first order relativistic light time correction (Shapiro time delay) due to a set of point masses. + * This class has the properties (mass and position) of the gravitating bodies as member variables. It receives the states + * and times of receiver and transmitter as input each time a light time is to be calculated. + * NOTE: If the perturbing body is equal to the transmitting or receiving body, the transmission or reception time, + * respectively, are used to evaluate this perturbing body's state when computing the correction. For all other bodies, + * the mid-way point of the light time is used. + */ +class FirstOrderLightTimeCorrectionCalculator: public LightTimeCorrection +{ +public: + + //! Constructor, takes and sets gravitating body properties. + /*! + * Constructor, takes and sets gravitating body properties. + * \param perturbingBodyStateFunctions Set of function returning the state of the gravitating bodies as a function + * of time. + * \param perturbingBodyGravitationalParameterFunctions Set of functions returning the gravitational parameters of + * the gravitating bodies. + * \param perturbingBodyNames Names of bodies causing light-time correction. + * \param transmittingBody Name of transmitting body + * \param receivingBody Name of receiving body + * \param ppnParameterGammaFunction Function returning the parametric post-Newtonian parameter gamma, a measure + * for the space-time curvature due to a unit rest mass (default 1.0; value from GR) + */ + FirstOrderLightTimeCorrectionCalculator( + const std::vector< boost::function< basic_mathematics::Vector6d( const double ) > >& perturbingBodyStateFunctions, + const std::vector< boost::function< double( ) > >& perturbingBodyGravitationalParameterFunctions, + const std::vector< std::string > perturbingBodyNames, + const std::string transmittingBody, + const std::string receivingBody, + const boost::function< double( ) >& ppnParameterGammaFunction = boost::lambda::constant( 1.0 ) ): + LightTimeCorrection( first_order_relativistic ), + perturbingBodyStateFunctions_( perturbingBodyStateFunctions ), + perturbingBodyGravitationalParameterFunctions_( perturbingBodyGravitationalParameterFunctions ), + perturbingBodyNames_( perturbingBodyNames ), + ppnParameterGammaFunction_( ppnParameterGammaFunction ) + { + currentTotalLightTimeCorrection_ = 0.0; + currentLighTimeCorrectionComponents_.resize( perturbingBodyNames_.size( ) ); + + // Check if perturbing body is transmitting/receiving body, and set evaluation time settings accordingly + for( unsigned int i = 0; i < perturbingBodyNames.size( ); i++ ) + { + if( perturbingBodyNames.at( i ) == transmittingBody ) + { + lightTimeEvaluationContribution_.push_back( 0.0 ); + } + else if( perturbingBodyNames.at( i ) == receivingBody ) + { + lightTimeEvaluationContribution_.push_back( 1.0 ); + } + else + { + lightTimeEvaluationContribution_.push_back( 0.5 ); + } + } + } + + //! Destructor + ~FirstOrderLightTimeCorrectionCalculator( ){ } + + //! Function to calculate first-order relativistic light time correction due to set of gravitating point masses. + /*! + * Function to calculate first order relativistic light time correction due to set of gravitating point masses, + * according to Eq. (11.17) of 2010 IERS conventions. Calculation are performed by calling ca + * calculateFirstOrderLightTimeCorrectionFromCentralBody function for each gravitating body. + * \param transmitterState State of transmitter at transmission time. + * \param receiverState State of receiver at reception time + * \param transmissionTime Time of signal transmission + * \param receptionTime Time of signal reception + * \return Total light time correction due to gravitating masses defined by perturbingBodyStateFunctions_ and + * perturbingBodyGravitationalParameterFunctions_ member variables. + */ + double calculateLightTimeCorrection( const basic_mathematics::Vector6d& transmitterState, + const basic_mathematics::Vector6d& receiverState, + const double transmissionTime, + const double receptionTime ); + + //! Function to get the names of bodies causing light-time correction. + /*! + * Function to get the names of bodies causing light-time correction. + * \return Names of bodies causing light-time correction. + */ + std::vector< std::string > getPerturbingBodyNames( ) + { + return perturbingBodyNames_; + } + + //! Function to get the set of functions returning the gravitational parameters of the gravitating bodies. + /*! + * Function to get the set of functions returning the gravitational parameters of the gravitating bodies. + * \return Set of functions returning the gravitational parameters of the gravitating bodies. + */ + std::vector< boost::function< double( ) > > getPerturbingBodyGravitationalParameterFunctions( ) + { + return perturbingBodyGravitationalParameterFunctions_; + } + + //! Function to get the total light-time correction, as computed by last call to calculateLightTimeCorrection. + /*! + * Function to get the total light-time correction, as computed by last call to calculateLightTimeCorrection. + * \return Total light-time correction, as computed by last call to calculateLightTimeCorrection. + */ + double getCurrentTotalLightTimeCorrection( ) + { + return currentTotalLightTimeCorrection_; + } + + //! Function to get the light-time correction of given perturbing body, as computed by last call to + //! calculateLightTimeCorrection. + /*! + * Function to get the light-time correction of given perturbing body, as computed by last call to + * calculateLightTimeCorrection. + * \param bodyIndex Index in list of bodies for which the correction is to be returbed + * \return Light-time correction of given perturbing body, as computed by last call to + * calculateLightTimeCorrection. + */ + double getCurrentLightTimeCorrectionComponent( const int bodyIndex ) + { + return currentLighTimeCorrectionComponents_.at( bodyIndex ); + } + + //! Function to get the function returning the parametric post-Newtonian parameter gamma + /*! + * Function to get the function returning the parametric post-Newtonian parameter gamma + * \return Function returning the parametric post-Newtonian parameter gamma + */ + boost::function< double( ) > getPpnParameterGammaFunction_( ) + { + return ppnParameterGammaFunction_; + } + +private: + + //! Set of function returning the state of the gravitating bodies as a function of time. + std::vector< boost::function< basic_mathematics::Vector6d( const double ) > > perturbingBodyStateFunctions_; + + //! Set of functions returning the gravitational parameters of the gravitating bodies. + std::vector< boost::function< double( ) > > perturbingBodyGravitationalParameterFunctions_; + + //! Names of bodies causing light-time correction. + std::vector< std::string > perturbingBodyNames_; + + //! Function returning the parametric post-Newtonian parameter gamma + /*! + * Function returning the parametric post-Newtonian parameter gamma, a measure for the space-time curvature due to a + * unit rest mass (1.0 in GR) + */ + boost::function< double( ) > ppnParameterGammaFunction_; + + //! List of values (between 0 and 1) of how far into the light-time the state of each perturbing body is to be evaluated. + std::vector< double > lightTimeEvaluationContribution_; + + //! List of light-time correction due to each separate perturbing body, as computed by last call to + //! calculateLightTimeCorrection. + std::vector< double > currentLighTimeCorrectionComponents_; + + //! Total light-time correction, as computed by last call to calculateLightTimeCorrection. + double currentTotalLightTimeCorrection_; +}; + +} // namespace observation_models + +} // namespace tudat +#endif // TUDAT_FIRSTORDERRELATIVISTICLIGHTTIMECORRECTION_H diff --git a/Tudat/Astrodynamics/ObservationModels/ObservableCorrections/lightTimeCorrection.h b/Tudat/Astrodynamics/ObservationModels/ObservableCorrections/lightTimeCorrection.h new file mode 100644 index 0000000000..ae56333290 --- /dev/null +++ b/Tudat/Astrodynamics/ObservationModels/ObservableCorrections/lightTimeCorrection.h @@ -0,0 +1,89 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_LIGHTTIMECORRECTION_H +#define TUDAT_LIGHTTIMECORRECTION_H + +#include "Tudat/Mathematics/BasicMathematics/linearAlgebraTypes.h" + +namespace tudat +{ + +namespace observation_models +{ + +//! Enum defining different types of light time corrections. +enum LightTimeCorrectionType +{ + first_order_relativistic, + function_wrapper_light_time_correction +}; + + +//! Base class for computing deviations from the light-time w.r.t. straight-line propagation at constant velocity (c). +/*! + * Base class for computing deviations from the light-time w.r.t. straight-line propagation at constant velocity (c). + * This base class is non-functional, and each time of light-time correction must be defined in a dedicated derived class + */ +class LightTimeCorrection +{ +public: + + //! Constructor + /*! + * Constructor + * \param lightTimeCorrectionType Type of light-time correction represented by instance of class. + */ + LightTimeCorrection( const LightTimeCorrectionType lightTimeCorrectionType ): + lightTimeCorrectionType_( lightTimeCorrectionType ){ } + + //! Destructor + virtual ~LightTimeCorrection( ){ } + + //! Pure virtual function to compute the light-time correction + /*! + * Pure virtual function to compute the light-time correction, function is to be implemented in derived class + * for specific correction model. The input is the states and times of the two link ends (at the current iteration of + * the solution of the implicit light-time equation), which is the information of the link already computed by the + * light time calculator, ensuring that no double computations are performed. + * \param transmitterState State of transmitted at transmission time + * \param receiverState State of receiver at reception time + * \param transmissionTime Time of signal transmission + * \param receptionTime Time of singal reception + * \return + */ + virtual double calculateLightTimeCorrection( + const basic_mathematics::Vector6d& transmitterState, + const basic_mathematics::Vector6d& receiverState, + const double transmissionTime, + const double receptionTime ) = 0; + + //! Function to retrieve the type of light-time correction represented by instance of class. + /*! + * Function to retrieve the type of light-time correction represented by instance of class. + * \return Type of light-time correction represented by instance of class. + */ + LightTimeCorrectionType getLightTimeCorrectionType( ) + { + return lightTimeCorrectionType_; + } + +protected: + + //! Type of light-time correction represented by instance of class. + LightTimeCorrectionType lightTimeCorrectionType_; + +}; + +} // namespace observation_models + +} // namespace tudat + +#endif // TUDAT_LIGHTTIMECORRECTION_H diff --git a/Tudat/Astrodynamics/ObservationModels/UnitTests/testLightTimeCorrections.h b/Tudat/Astrodynamics/ObservationModels/UnitTests/testLightTimeCorrections.h index 309339cf3d..a11609ca0b 100644 --- a/Tudat/Astrodynamics/ObservationModels/UnitTests/testLightTimeCorrections.h +++ b/Tudat/Astrodynamics/ObservationModels/UnitTests/testLightTimeCorrections.h @@ -1,36 +1,11 @@ -/* Copyright (c) 2010-2015, Delft University of Technology - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, are - * permitted provided that the following conditions are met: - * - Redistributions of source code must retain the above copyright notice, this list of - * conditions and the following disclaimer. - * - Redistributions in binary form must reproduce the above copyright notice, this list of - * conditions and the following disclaimer in the documentation and/or other materials - * provided with the distribution. - * - Neither the name of the Delft University of Technology nor the names of its contributors - * may be used to endorse or promote products derived from this software without specific - * prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS - * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE - * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING - * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED - * OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Changelog - * YYMMDD Author Comment - * 130226 D. Dirkx File created. - * 130522 E.D. Brandon Minor changes during code check. - * - * References - * - * Notes +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. */ #ifndef TUDAT_TEST_LIGHT_TIME_CORRECTIONS_H diff --git a/Tudat/Astrodynamics/ObservationModels/UnitTests/unitTestAngularPositionModel.cpp b/Tudat/Astrodynamics/ObservationModels/UnitTests/unitTestAngularPositionModel.cpp new file mode 100644 index 0000000000..5c93b3c03b --- /dev/null +++ b/Tudat/Astrodynamics/ObservationModels/UnitTests/unitTestAngularPositionModel.cpp @@ -0,0 +1,187 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#define BOOST_TEST_MAIN + +#include +#include + +#include +#include +#include + +#include "Tudat/Basics/testMacros.h" + +#include "Tudat/InputOutput/basicInputOutput.h" + +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" +#include "Tudat/Astrodynamics/ObservationModels/angularPositionObservationModel.h" +#include "Tudat/SimulationSetup/EstimationSetup/createObservationModel.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h" + +namespace tudat +{ +namespace unit_tests +{ + +using namespace tudat::observation_models; +using namespace tudat::spice_interface; +using namespace tudat::ephemerides; +using namespace tudat::simulation_setup; + + +BOOST_AUTO_TEST_SUITE( test_angular_position_model ) + + +BOOST_AUTO_TEST_CASE( testAngularPositionModel ) +{ + std::string kernelsPath = input_output::getSpiceKernelPath( ); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de-403-masses.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "naif0009.tls"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "pck00009.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de421.bsp"); + + // Define bodies to use. + std::vector< std::string > bodiesToCreate; + bodiesToCreate.push_back( "Earth" ); + bodiesToCreate.push_back( "Sun" ); + bodiesToCreate.push_back( "Mars" ); + + // Specify initial time + double initialEphemerisTime = 0.0; + double finalEphemerisTime = initialEphemerisTime + 7.0 * 86400.0; + double maximumTimeStep = 3600.0; + double buffer = 10.0 * maximumTimeStep; + + // Create bodies settings needed in simulation + std::map< std::string, boost::shared_ptr< BodySettings > > defaultBodySettings = + getDefaultBodySettings( + bodiesToCreate, initialEphemerisTime - buffer, finalEphemerisTime + buffer ); + + // Create bodies + NamedBodyMap bodyMap = createBodies( defaultBodySettings ); + + setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" ); + + // Define link ends for observations. + LinkEnds linkEnds; + linkEnds[ transmitter ] = std::make_pair( "Earth" , "" ); + linkEnds[ receiver ] = std::make_pair( "Mars" , "" ); + + // Create observation model. + boost::shared_ptr< ObservationBias< 2 > > observationBias = + boost::make_shared< ConstantObservationBias< 2 > >( + ( Eigen::Vector2d( ) << 3.2E-9, -1.5E-8 ).finished( ) ); + std::vector< std::string > lightTimePerturbingBodies = boost::assign::list_of( "Sun" ); + std::vector< boost::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionSettings; + lightTimeCorrectionSettings.push_back( boost::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( + lightTimePerturbingBodies ) ); + boost::shared_ptr< ObservationModel< 2, double, double, double > > observationModel = + ObservationModelCreator< 2, double, double, double >::createObservationModel( + angular_position, linkEnds, bodyMap, lightTimeCorrectionSettings, observationBias ); + + // Compute observation separately with two functions. + double receiverObservationTime = ( finalEphemerisTime + initialEphemerisTime ) / 2.0; + std::vector< double > linkEndTimes; + std::vector< basic_mathematics::Vector6d > linkEndStates; + Eigen::Vector2d observationFromReceptionTime = observationModel->computeObservations( + receiverObservationTime, receiver ); + Eigen::Vector2d observationFromReceptionTime2 = observationModel->computeObservationsWithLinkEndData( + receiverObservationTime, receiver, linkEndTimes, linkEndStates ); + BOOST_CHECK_EQUAL( linkEndTimes.size( ), 2 ); + BOOST_CHECK_EQUAL( linkEndStates.size( ), 2 ); + + // Manually create and compute light time corrections + boost::shared_ptr< LightTimeCorrection > lightTimeCorrectionCalculator = + createLightTimeCorrections( + lightTimeCorrectionSettings.at( 0 ), bodyMap, linkEnds[ transmitter ], linkEnds[ receiver ] ); + double lightTimeCorrection = lightTimeCorrectionCalculator->calculateLightTimeCorrection( + linkEndStates.at( 0 ), linkEndStates.at( 1 ), linkEndTimes.at( 0 ), linkEndTimes.at( 1 ) ); + + // Check equality of computed observations. + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + observationFromReceptionTime, observationFromReceptionTime2, std::numeric_limits< double >::epsilon( ) ); + + // Check consistency of link end states and time. + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + linkEndStates.at( 0 ), bodyMap.at( "Earth" )->getStateInBaseFrameFromEphemeris( linkEndTimes.at( 0 ) ), + std::numeric_limits< double >::epsilon( ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + linkEndStates.at( 1 ), bodyMap.at( "Mars" )->getStateInBaseFrameFromEphemeris( linkEndTimes.at( 1 ) ), + std::numeric_limits< double >::epsilon( ) ); + + // Check that reception time is kept fixed. + BOOST_CHECK_CLOSE_FRACTION( static_cast< double >( receiverObservationTime ), + linkEndTimes[ 1 ], std::numeric_limits< double >::epsilon( ) ); + + // Manually compute light time + Eigen::Vector3d positionDifference = ( linkEndStates[ 0 ] - linkEndStates[ 1 ] ).segment( 0, 3 ); + BOOST_CHECK_CLOSE_FRACTION( + positionDifference.norm( ) / physical_constants::SPEED_OF_LIGHT + lightTimeCorrection, + linkEndTimes[ 1 ] - linkEndTimes[ 0 ], + std::numeric_limits< double >::epsilon( ) * 1000.0 ); + // Poor tolerance due to rounding errors when subtracting times + + // Check computed right ascension/declination from link end states + Eigen::Vector3d sphericalRelativeCoordinates = coordinate_conversions::convertCartesianToSpherical( + positionDifference ); + BOOST_CHECK_CLOSE_FRACTION( + sphericalRelativeCoordinates.z( ) + observationBias->getObservationBias( + std::vector< double >( ), std::vector< basic_mathematics::Vector6d >( ) ).x( ), + observationFromReceptionTime( 0 ), + std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( + mathematical_constants::PI / 2.0 - sphericalRelativeCoordinates.y( ) + + observationBias->getObservationBias( + std::vector< double >( ), std::vector< basic_mathematics::Vector6d>( ) ).y( ), + observationFromReceptionTime( 1 ), + std::numeric_limits< double >::epsilon( ) ); + + // Compute transmission time from light time. + double transmitterObservationTime = receiverObservationTime - ( linkEndTimes[ 1 ] - linkEndTimes[ 0 ] ); + + // Compare computed against returned transmission time. + BOOST_CHECK_CLOSE_FRACTION( + static_cast< double >( transmitterObservationTime ), linkEndTimes[ 0 ], + std::numeric_limits< double >::epsilon( ) ); + + // Recompute observable while fixing transmission time. + std::vector< double > linkEndTimes2; + std::vector< basic_mathematics::Vector6d > linkEndStates2; + Eigen::Vector2d observationFromTransmissionTime = observationModel->computeObservations( + transmitterObservationTime, transmitter ); + Eigen::Vector2d observationFromTransmissionTime2 = observationModel->computeObservationsWithLinkEndData( + transmitterObservationTime, transmitter, linkEndTimes2, linkEndStates2 ); + + // Compare results against those obtained when keeping reception fixed. + for( unsigned int i = 0; i < 2; i++ ) + { + BOOST_CHECK_SMALL( observationFromTransmissionTime( i ) - observationFromTransmissionTime2( i ), 1.0E-15 ); + BOOST_CHECK_SMALL( observationFromTransmissionTime( i ) - observationFromReceptionTime( i ), 1.0E-15 ); + BOOST_CHECK_CLOSE_FRACTION( linkEndTimes2.at( i ), linkEndTimes2.at( i ), 1.0E-15 ); + } + + + // Check consistency of link end states and time. + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + linkEndStates2.at( 0 ), bodyMap.at( "Earth" )->getStateInBaseFrameFromEphemeris( linkEndTimes2.at( 0 ) ), + std::numeric_limits< double >::epsilon( ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + linkEndStates2.at( 1 ), bodyMap.at( "Mars" )->getStateInBaseFrameFromEphemeris( linkEndTimes2.at( 1 ) ), + std::numeric_limits< double >::epsilon( ) ); +} + +BOOST_AUTO_TEST_SUITE_END( ) + +} + +} + diff --git a/Tudat/Astrodynamics/ObservationModels/UnitTests/unitTestLightTimeSolution.cpp b/Tudat/Astrodynamics/ObservationModels/UnitTests/unitTestLightTimeSolution.cpp index 485564324d..9d8a0a0f86 100644 --- a/Tudat/Astrodynamics/ObservationModels/UnitTests/unitTestLightTimeSolution.cpp +++ b/Tudat/Astrodynamics/ObservationModels/UnitTests/unitTestLightTimeSolution.cpp @@ -1,37 +1,11 @@ -/* Copyright (c) 2010-2015, Delft University of Technology - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, are - * permitted provided that the following conditions are met: - * - Redistributions of source code must retain the above copyright notice, this list of - * conditions and the following disclaimer. - * - Redistributions in binary form must reproduce the above copyright notice, this list of - * conditions and the following disclaimer in the documentation and/or other materials - * provided with the distribution. - * - Neither the name of the Delft University of Technology nor the names of its contributors - * may be used to endorse or promote products derived from this software without specific - * prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS - * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE - * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING - * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED - * OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Changelog - * YYMMDD Author Comment - * 130226 D. Dirkx Migrated from personal code. - * 130521 E. Brandon Minor changes during code check. - * 140127 D. Dirkx Adapted for custom Spice kernel folder. - * - * References - * - * Notes +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. */ #define BOOST_TEST_MAIN @@ -88,8 +62,8 @@ BOOST_AUTO_TEST_CASE( testLightWithSpice ) moon, "SSB", false, false, false, frame ); // Create light-time calculator, Earth center transmitter, Moon center receiver. - boost::shared_ptr< LightTimeCalculator > lightTimeEarthToMoon = - boost::make_shared< LightTimeCalculator > + boost::shared_ptr< LightTimeCalculator< > > lightTimeEarthToMoon = + boost::make_shared< LightTimeCalculator< > > ( boost::bind( &Ephemeris::getCartesianStateFromEphemeris, earthEphemeris, _1, basic_astrodynamics::JULIAN_DAY_ON_J2000 ), boost::bind( &Ephemeris::getCartesianStateFromEphemeris, moonEphemeris, _1, @@ -182,8 +156,8 @@ BOOST_AUTO_TEST_CASE( testLightWithSpice ) lightTimeCorrections.push_back( &getTimeDifferenceLightTimeCorrection ); // Create light-time object with correction. - boost::shared_ptr< LightTimeCalculator > lightTimeEarthToMoonWithCorrection = - boost::make_shared< LightTimeCalculator > + boost::shared_ptr< LightTimeCalculator< > > lightTimeEarthToMoonWithCorrection = + boost::make_shared< LightTimeCalculator< > > ( boost::bind( &Ephemeris::getCartesianStateFromEphemeris, earthEphemeris, _1, basic_astrodynamics::JULIAN_DAY_ON_J2000 ), boost::bind( &Ephemeris::getCartesianStateFromEphemeris, moonEphemeris, _1, @@ -209,7 +183,7 @@ BOOST_AUTO_TEST_CASE( testLightWithSpice ) // Create light-time object with correction, without iterating light-time corrections. lightTimeEarthToMoonWithCorrection = - boost::make_shared< LightTimeCalculator > + boost::make_shared< LightTimeCalculator< > > ( boost::bind( &Ephemeris::getCartesianStateFromEphemeris, earthEphemeris, _1, basic_astrodynamics::JULIAN_DAY_ON_J2000 ), boost::bind( &Ephemeris::getCartesianStateFromEphemeris, moonEphemeris, _1, @@ -239,7 +213,7 @@ BOOST_AUTO_TEST_CASE( testLightWithSpice ) // Create light-time object with multiple corrections. lightTimeEarthToMoonWithCorrection = - boost::make_shared< LightTimeCalculator > + boost::make_shared< LightTimeCalculator< > > ( boost::bind( &Ephemeris::getCartesianStateFromEphemeris, earthEphemeris, _1, basic_astrodynamics::JULIAN_DAY_ON_J2000 ), boost::bind( &Ephemeris::getCartesianStateFromEphemeris, moonEphemeris, _1, diff --git a/Tudat/Astrodynamics/ObservationModels/UnitTests/unitTestOneWayRangeObservationModel.cpp b/Tudat/Astrodynamics/ObservationModels/UnitTests/unitTestOneWayRangeObservationModel.cpp new file mode 100644 index 0000000000..dc8a6fc31f --- /dev/null +++ b/Tudat/Astrodynamics/ObservationModels/UnitTests/unitTestOneWayRangeObservationModel.cpp @@ -0,0 +1,181 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#define BOOST_TEST_MAIN + +#include +#include + +#include +#include +#include + +#include "Tudat/Basics/testMacros.h" + +#include "Tudat/InputOutput/basicInputOutput.h" + +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" +#include "Tudat/Astrodynamics/ObservationModels/oneWayRangeObservationModel.h" +#include "Tudat/SimulationSetup/EstimationSetup/createObservationModel.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h" + +namespace tudat +{ +namespace unit_tests +{ + +using namespace tudat::observation_models; +using namespace tudat::spice_interface; +using namespace tudat::ephemerides; +using namespace tudat::simulation_setup; + + +BOOST_AUTO_TEST_SUITE( test_one_way_range_model ) + + +BOOST_AUTO_TEST_CASE( testOneWayRangeModel ) +{ + std::string kernelsPath = input_output::getSpiceKernelPath( ); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de-403-masses.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "naif0009.tls"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "pck00009.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de421.bsp"); + + // Define bodies to use. + std::vector< std::string > bodiesToCreate; + bodiesToCreate.push_back( "Earth" ); + bodiesToCreate.push_back( "Sun" ); + bodiesToCreate.push_back( "Mars" ); + + // Specify initial time + double initialEphemerisTime = 0.0; + double finalEphemerisTime = initialEphemerisTime + 7.0 * 86400.0; + double maximumTimeStep = 3600.0; + double buffer = 10.0 * maximumTimeStep; + + // Create bodies settings needed in simulation + std::map< std::string, boost::shared_ptr< BodySettings > > defaultBodySettings = + getDefaultBodySettings( + bodiesToCreate, initialEphemerisTime - buffer, finalEphemerisTime + buffer ); + + // Create bodies + NamedBodyMap bodyMap = createBodies( defaultBodySettings ); + + setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" ); + + // Define link ends for observations. + LinkEnds linkEnds; + linkEnds[ transmitter ] = std::make_pair( "Earth" , "" ); + linkEnds[ receiver ] = std::make_pair( "Mars" , "" ); + + // Create observation model. + boost::shared_ptr< ObservationBias< 1 > > observationBias = + boost::make_shared< ConstantObservationBias< 1 > >( + ( Eigen::Matrix< double, 1, 1 >( ) <<2.56294 ).finished( ) ); + std::vector< std::string > lightTimePerturbingBodies = boost::assign::list_of( "Sun" ); + std::vector< boost::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionSettings; + lightTimeCorrectionSettings.push_back( boost::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( + lightTimePerturbingBodies ) ); + boost::shared_ptr< ObservationModel< 1, double, double, double > > observationModel = + ObservationModelCreator< 1, double, double, double >::createObservationModel( + oneWayRange, linkEnds, bodyMap, lightTimeCorrectionSettings, observationBias ); + + // Compute observation separately with two functions. + double receiverObservationTime = ( finalEphemerisTime + initialEphemerisTime ) / 2.0; + std::vector< double > linkEndTimes; + std::vector< basic_mathematics::Vector6d > linkEndStates; + double observationFromReceptionTime = observationModel->computeObservations( + receiverObservationTime, receiver )( 0 ); + double observationFromReceptionTime2 = observationModel->computeObservationsWithLinkEndData( + receiverObservationTime, receiver, linkEndTimes, linkEndStates )( 0 ); + BOOST_CHECK_EQUAL( linkEndTimes.size( ), 2 ); + BOOST_CHECK_EQUAL( linkEndStates.size( ), 2 ); + + // Manually create and compute light time corrections + boost::shared_ptr< LightTimeCorrection > lightTimeCorrectionCalculator = + createLightTimeCorrections( + lightTimeCorrectionSettings.at( 0 ), bodyMap, linkEnds[ transmitter ], linkEnds[ receiver ] ); + double lightTimeCorrection = lightTimeCorrectionCalculator->calculateLightTimeCorrection( + linkEndStates.at( 0 ), linkEndStates.at( 1 ), linkEndTimes.at( 0 ), linkEndTimes.at( 1 ) ); + + // Check equality of computed observations. + BOOST_CHECK_CLOSE_FRACTION( + observationFromReceptionTime, observationFromReceptionTime2, std::numeric_limits< double >::epsilon( ) ); + + // Check consistency of link end states and time. + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + linkEndStates.at( 0 ), bodyMap.at( "Earth" )->getStateInBaseFrameFromEphemeris( linkEndTimes.at( 0 ) ), + std::numeric_limits< double >::epsilon( ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + linkEndStates.at( 1 ), bodyMap.at( "Mars" )->getStateInBaseFrameFromEphemeris( linkEndTimes.at( 1 ) ), + std::numeric_limits< double >::epsilon( ) ); + + // Check that reception time is kept fixed. + BOOST_CHECK_CLOSE_FRACTION( static_cast< double >( receiverObservationTime ), + linkEndTimes[ 1 ], std::numeric_limits< double >::epsilon( ) ); + + // Manually compute light time + Eigen::Vector3d positionDifference = ( linkEndStates[ 0 ] - linkEndStates[ 1 ] ).segment( 0, 3 ); + BOOST_CHECK_CLOSE_FRACTION( + positionDifference.norm( ) / physical_constants::SPEED_OF_LIGHT + lightTimeCorrection, + linkEndTimes[ 1 ] - linkEndTimes[ 0 ], + std::numeric_limits< double >::epsilon( ) * 1000.0 ); + // Poor tolerance due to rounding errors when subtracting times + + // Check computed range from link end states + BOOST_CHECK_CLOSE_FRACTION( + positionDifference.norm( ) + lightTimeCorrection * physical_constants::SPEED_OF_LIGHT + + observationBias->getObservationBias( + std::vector< double >( ), std::vector< basic_mathematics::Vector6d >( ) )( 0 ), + observationFromReceptionTime, + std::numeric_limits< double >::epsilon( ) ); + + // Compute transmission time from light time. + double transmitterObservationTime = receiverObservationTime - ( linkEndTimes[ 1 ] - linkEndTimes[ 0 ] ); + + // Compare computed against returned transmission time. + BOOST_CHECK_CLOSE_FRACTION( + static_cast< double >( transmitterObservationTime ), linkEndTimes[ 0 ], + std::numeric_limits< double >::epsilon( ) ); + + // Recompute observable while fixing transmission time. + std::vector< double > linkEndTimes2; + std::vector< basic_mathematics::Vector6d > linkEndStates2; + double observationFromTransmissionTime = observationModel->computeObservations( + transmitterObservationTime, transmitter )( 0 ); + double observationFromTransmissionTime2 = observationModel->computeObservationsWithLinkEndData( + transmitterObservationTime, transmitter, linkEndTimes2, linkEndStates2 )( 0 ); + + // Compare results against those obtained when keeping reception fixed. + BOOST_CHECK_SMALL( observationFromTransmissionTime - observationFromTransmissionTime2, 1.0E-15 ); + BOOST_CHECK_SMALL( observationFromTransmissionTime - observationFromReceptionTime, 1.0E-15 ); + + for( unsigned int i = 0; i < 2; i++ ) + { + BOOST_CHECK_CLOSE_FRACTION( linkEndTimes2.at( i ), linkEndTimes2.at( i ), 1.0E-15 ); + } + + + // Check consistency of link end states and time. + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + linkEndStates2.at( 0 ), bodyMap.at( "Earth" )->getStateInBaseFrameFromEphemeris( linkEndTimes2.at( 0 ) ), + std::numeric_limits< double >::epsilon( ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + linkEndStates2.at( 1 ), bodyMap.at( "Mars" )->getStateInBaseFrameFromEphemeris( linkEndTimes2.at( 1 ) ), + std::numeric_limits< double >::epsilon( ) ); +} + +BOOST_AUTO_TEST_SUITE_END( ) + +} + +} + diff --git a/Tudat/Astrodynamics/ObservationModels/UnitTests/unitTestPositionObservationModel.cpp b/Tudat/Astrodynamics/ObservationModels/UnitTests/unitTestPositionObservationModel.cpp new file mode 100644 index 0000000000..56b6541aad --- /dev/null +++ b/Tudat/Astrodynamics/ObservationModels/UnitTests/unitTestPositionObservationModel.cpp @@ -0,0 +1,137 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#define BOOST_TEST_MAIN + +#include +#include + +#include +#include +#include + +#include "Tudat/Basics/testMacros.h" + +#include "Tudat/InputOutput/basicInputOutput.h" + +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" +#include "Tudat/Astrodynamics/ObservationModels/angularPositionObservationModel.h" +#include "Tudat/SimulationSetup/EstimationSetup/createObservationModel.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h" + +namespace tudat +{ +namespace unit_tests +{ + +using namespace tudat::observation_models; +using namespace tudat::spice_interface; +using namespace tudat::ephemerides; +using namespace tudat::simulation_setup; + + +BOOST_AUTO_TEST_SUITE( test_position_obsevable_model ) + + +BOOST_AUTO_TEST_CASE( testPositionObsevableModel ) +{ + std::string kernelsPath = input_output::getSpiceKernelPath( ); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de-403-masses.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "naif0009.tls"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "pck00009.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de421.bsp"); + + // Define bodies to use. + std::vector< std::string > bodiesToCreate; + bodiesToCreate.push_back( "Earth" ); + bodiesToCreate.push_back( "Sun" ); + + // Specify initial time + double initialEphemerisTime = 0.0; + double finalEphemerisTime = initialEphemerisTime + 7.0 * 86400.0; + double maximumTimeStep = 3600.0; + double buffer = 10.0 * maximumTimeStep; + + // Create bodies settings needed in simulation + std::map< std::string, boost::shared_ptr< BodySettings > > defaultBodySettings = + getDefaultBodySettings( + bodiesToCreate, initialEphemerisTime - buffer, finalEphemerisTime + buffer ); + + // Create bodies + NamedBodyMap bodyMap = createBodies( defaultBodySettings ); + + setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" ); + + // Define link ends for observations. + LinkEnds linkEnds; + linkEnds[ observed_body ] = std::make_pair( "Earth" , "" ); + + // Create observation model. + boost::shared_ptr< ObservationBias< 3 > > observationBias = + boost::make_shared< ConstantObservationBias< 3 > >( + ( Eigen::Vector3d( ) << 543.2454, -34.244, 3431.24345 ).finished( ) ); + + boost::shared_ptr< ObservationModel< 3, double, double, double > > observationModel = + ObservationModelCreator< 3, double, double, double >::createObservationModel( + position_observable, linkEnds, bodyMap, + std::vector< boost::shared_ptr< LightTimeCorrectionSettings > >( ), observationBias ); + + // Compute observation separately with two functions. + double observationTime = ( finalEphemerisTime + initialEphemerisTime ) / 2.0; + std::vector< double > linkEndTimes; + std::vector< basic_mathematics::Vector6d > linkEndStates; + Eigen::Vector3d observation = observationModel->computeObservations( + observationTime, observed_body ); + Eigen::Vector3d observation2 = observationModel->computeObservationsWithLinkEndData( + observationTime, observed_body, linkEndTimes, linkEndStates ); + + // Check size of link end state/time. + BOOST_CHECK_EQUAL( linkEndTimes.size( ), 1 ); + BOOST_CHECK_EQUAL( linkEndStates.size( ), 1 ); + + // Check link end state/time. + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + bodyMap.at( "Earth" )->getStateInBaseFrameFromEphemeris( observationTime ), + linkEndStates[ 0 ], std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( observationTime, linkEndTimes[ 0 ], std::numeric_limits< double >::epsilon( ) ); + + // Check biased observable + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ( bodyMap.at( "Earth" )->getStateInBaseFrameFromEphemeris( observationTime ).segment( 0, 3 ) + + observationBias->getObservationBias( + std::vector< double >( ), std::vector< basic_mathematics::Vector6d>( ) ) ), + observation, std::numeric_limits< double >::epsilon( ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + observation, observation2, std::numeric_limits< double >::epsilon( ) ); + + // Check ideal observable + observation = observationModel->computeIdealObservations( + observationTime, observed_body ); + observation2 = observationModel->computeIdealObservationsWithLinkEndData( + observationTime, observed_body, linkEndTimes, linkEndStates ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + bodyMap.at( "Earth" )->getStateInBaseFrameFromEphemeris( observationTime ), + linkEndStates[ 0 ], std::numeric_limits< double >::epsilon( ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + bodyMap.at( "Earth" )->getStateInBaseFrameFromEphemeris( observationTime ).segment( 0, 3 ), + observation, std::numeric_limits< double >::epsilon( ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + observation, observation2, std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( observationTime, linkEndTimes[ 0 ], std::numeric_limits< double >::epsilon( ) ); + +} + +BOOST_AUTO_TEST_SUITE_END( ) + +} + +} + diff --git a/Tudat/Astrodynamics/ObservationModels/angularPositionObservationModel.h b/Tudat/Astrodynamics/ObservationModels/angularPositionObservationModel.h new file mode 100644 index 0000000000..335393a92f --- /dev/null +++ b/Tudat/Astrodynamics/ObservationModels/angularPositionObservationModel.h @@ -0,0 +1,155 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_ANGULARPOSITIONOBSERVATIONMODEL_H +#define TUDAT_ANGULARPOSITIONOBSERVATIONMODEL_H + +#include +#include + +#include + +#include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h" +#include "Tudat/Astrodynamics/ObservationModels/lightTimeSolution.h" +#include "Tudat/Astrodynamics/ObservationModels/observationModel.h" + +namespace tudat +{ + +namespace observation_models +{ + +//! Class for simulating angular position (right ascension/declination) observables. +/*! + * Class for simulating angular position (right ascension/declination), using light-time (with light-time corrections) + * to determine the states of the link ends (source and receiver). + * The user may add observation biases to model system-dependent deviations between measured and true observation. + */ +template< typename ObservationScalarType = double, typename TimeType = double, typename StateScalarType = ObservationScalarType > +class AngularPositionObservationModel: public ObservationModel< 2, ObservationScalarType, TimeType, StateScalarType > +{ +public: + + typedef Eigen::Matrix< StateScalarType, 6, 1 > StateType; + typedef Eigen::Matrix< StateScalarType, 6, 1 > PositionType; + + //! Constructor. + /*! + * Constructor, + * \param lightTimeCalculator Object to compute the light-time (including any corrections w.r.t. Euclidean case) + * between source and receiver + * \param observationBiasCalculator Object for calculating system-dependent errors in the + * observable, i.e. deviations from the physically ideal observable between reference points (default none). + */ + AngularPositionObservationModel( + const boost::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType, StateScalarType > > lightTimeCalculator, + const boost::shared_ptr< ObservationBias< 2 > > observationBiasCalculator = NULL ): + ObservationModel< 2, ObservationScalarType, TimeType, StateScalarType >( angular_position, observationBiasCalculator ), + lightTimeCalculator_( lightTimeCalculator ) { } + + //! Destructor + ~AngularPositionObservationModel( ){ } + + //! Function to compute ideal angular position observation at given time. + /*! + * This function compute ideal angular position observation at a given time. The time argument can be either the + * reception or transmission time (defined by linkEndAssociatedWithTime input). + * Note that this observable does include e.g. light-time corrections, which represent physically true corrections. + * It does not include e.g. system-dependent measurement. + * The times and states of the link ends are also returned in full precision (determined by class template + * arguments). These states and times are returned by reference. + * \param time Time at which observation is to be simulated + * \param linkEndAssociatedWithTime Link end at which given time is valid, i.e. link end for which associated time + * is kept constant (to input value) + * \param linkEndTimes List of times at each link end during observation (returned by reference). + * \param linkEndStates List of states at each link end during observation (returned by reference). + * \return Calculated angular position observable values. + */ + Eigen::Matrix< ObservationScalarType, 2, 1 > computeIdealObservationsWithLinkEndData( + const TimeType time, + const LinkEndType linkEndAssociatedWithTime, + std::vector< TimeType >& linkEndTimes, + std::vector< Eigen::Matrix< StateScalarType, 6, 1 > >& linkEndStates ) + + { + // Check link end associated with input time and compute observable + bool isTimeAtReception; + if( linkEndAssociatedWithTime == receiver ) + { + isTimeAtReception = 1; + } + else if( linkEndAssociatedWithTime == transmitter ) + { + isTimeAtReception = 0; + } + else + { + isTimeAtReception = -1; + throw std::runtime_error( "Error when calculating angular position observation, link end is not transmitter or receiver" ); + } + + Eigen::Matrix< StateScalarType, 6, 1 > receiverState; + Eigen::Matrix< StateScalarType, 6, 1 > transmitterState; + + // Compute light-time and receiver/transmitter states. + ObservationScalarType lightTime = lightTimeCalculator_->calculateLightTimeWithLinkEndsStates( + receiverState, transmitterState, time, isTimeAtReception ); + + // Compute spherical relative position + Eigen::Matrix< ObservationScalarType, 3, 1 > sphericalRelativeCoordinates = + coordinate_conversions::convertCartesianToSpherical< StateScalarType >( + transmitterState.segment( 0, 3 ) - receiverState.segment( 0, 3 ) ). + template cast< ObservationScalarType >( ); + + // Set link end times and states. + linkEndTimes.clear( ); + linkEndStates.clear( ); + linkEndStates.push_back( transmitterState ); + linkEndStates.push_back( receiverState ); + + if( isTimeAtReception ) + { + linkEndTimes.push_back( time - lightTime ); + linkEndTimes.push_back( time ); + } + else + { + linkEndTimes.push_back( time ); + linkEndTimes.push_back( time + lightTime ); + } + + // Return observable + return ( Eigen::Matrix< ObservationScalarType, 2, 1 >( ) << sphericalRelativeCoordinates.z( ), + mathematical_constants::PI / 2.0 - sphericalRelativeCoordinates.y( ) ).finished( ); + } + + //! Function to get the object to calculate light time. + /*! + * Function to get the object to calculate light time. + * \return Object to calculate light time. + */ boost::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType, StateScalarType > > getLightTimeCalculator( ) + { + return lightTimeCalculator_; + } + +private: + + //! Object to calculate light time. + /*! + * Object to calculate light time, including possible corrections from troposphere, relativistic corrections, etc. + */ + boost::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType, StateScalarType > > lightTimeCalculator_; +}; + +} // namespace observation_models + +} // namespace tudat + +#endif // TUDAT_ANGULARPOSITIONOBSERVATIONMODEL_H diff --git a/Tudat/Astrodynamics/ObservationModels/lightTimeSolution.cpp b/Tudat/Astrodynamics/ObservationModels/lightTimeSolution.cpp index a5453ca77d..ce7ae97592 100644 --- a/Tudat/Astrodynamics/ObservationModels/lightTimeSolution.cpp +++ b/Tudat/Astrodynamics/ObservationModels/lightTimeSolution.cpp @@ -1,38 +1,14 @@ -/* Copyright (c) 2010-2015, Delft University of Technology - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, are - * permitted provided that the following conditions are met: - * - Redistributions of source code must retain the above copyright notice, this list of - * conditions and the following disclaimer. - * - Redistributions in binary form must reproduce the above copyright notice, this list of - * conditions and the following disclaimer in the documentation and/or other materials - * provided with the distribution. - * - Neither the name of the Delft University of Technology nor the names of its contributors - * may be used to endorse or promote products derived from this software without specific - * prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS - * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE - * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING - * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED - * OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Changelog - * YYMMDD Author Comment - * 130226 D. Dirkx Migrated from personal code. - * 130522 E.D. Brandon Minor changes during code check. - * - * References - * - * Notes +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. */ + #include #include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h" @@ -44,165 +20,33 @@ namespace tudat namespace observation_models { -//! Function to calculate the light time. -double LightTimeCalculator::calculateLightTime( const double ephemerisTime, - const bool isTimeAtReception, - const double tolerance ) -{ - // Declare and initialize variables for receiver and transmitter state (returned by reference). - basic_mathematics::Vector6d receiverState = basic_mathematics::Vector6d::Zero( ); - basic_mathematics::Vector6d transmitterState = basic_mathematics::Vector6d::Zero( ); - - // Calculate light time. - const double lightTime = calculateLightTimeWithLinkEndsStates( - receiverState, transmitterState, ephemerisTime, isTimeAtReception, tolerance ); - return lightTime; -} -//! Function to calculate the 'measured' vector from transmitter to receiver. -Eigen::Vector3d LightTimeCalculator::calculateRelativeRangeVector( const double ephemerisTime, - const bool isTimeAtReception, - const double tolerance ) +//! Function to retrieve the default tolerance for the light-time equation solution. +template< > +double getDefaultLightTimeTolerance< double, double >( ) { - // Declare and initialize variables for receiver and transmitter state (returned by reference). - basic_mathematics::Vector6d receiverState = basic_mathematics::Vector6d::Zero( ); - basic_mathematics::Vector6d transmitterState = basic_mathematics::Vector6d::Zero( ); - - // Calculate link end states and the determine range vector. - calculateLightTimeWithLinkEndsStates( receiverState, transmitterState, - ephemerisTime, isTimeAtReception, tolerance ); - return ( receiverState - transmitterState ).segment( 0, 3 ); + return 1.0E-12; } -//! Function to calculate the light time and link-ends states. -double LightTimeCalculator::calculateLightTimeWithLinkEndsStates( - basic_mathematics::Vector6d& receiverStateOutput, - basic_mathematics::Vector6d& transmitterStateOutput, - const double ephemerisTime, - const bool isTimeAtReception, - const double tolerance ) +//! Function to retrieve the default tolerance for the light-time equation solution. +template< > +long double getDefaultLightTimeTolerance< long double, long double >( ) { - using physical_constants::SPEED_OF_LIGHT; - using std::fabs; - - // Initialize reception and transmission times and states to initial guess (zero light time) - double receptionTime = ephemerisTime; - double transmissionTime = ephemerisTime; - basic_mathematics::Vector6d receiverState = stateFunctionOfReceivingBody_( receptionTime ); - basic_mathematics::Vector6d transmitterState = - stateFunctionOfTransmittingBody_( transmissionTime ); - - // Set initial light-time correction. - setTotalLightTimeCorrection( - transmitterState, receiverState, transmissionTime, receptionTime ); - - // Calculate light-time solution assuming infinte speed of signal as initial estimate. - double previousLightTimeCalculation = - calculateNewLightTimeEstime( receiverState, transmitterState ); - - // Set variables for iteration - double newLightTimeCalculation = 0.0; - bool isToleranceReached = false; - - // Recalculate light-time solution until tolerance is reached. - int counter = 0; - - // Set variable determining whether to update the light time each iteration. - bool updateLightTimeCorrections = false; - if( iterateCorrections_ ) - { - updateLightTimeCorrections = true; - } - - // Iterate until tolerance reached. - while( !isToleranceReached ) - { - // Update light-time corrections, if necessary. - if( updateLightTimeCorrections ) - { - setTotalLightTimeCorrection( - transmitterState, receiverState, transmissionTime, receptionTime ); - } - - // Update light-time estimate for this iteration. - if( isTimeAtReception ) - { - receptionTime = ephemerisTime; - transmissionTime = ephemerisTime - previousLightTimeCalculation; - transmitterState = ( stateFunctionOfTransmittingBody_( transmissionTime ) ); - } - else - { - receptionTime = ephemerisTime + previousLightTimeCalculation; - transmissionTime = ephemerisTime; - receiverState = ( stateFunctionOfReceivingBody_( receptionTime ) ); - } - newLightTimeCalculation = calculateNewLightTimeEstime( receiverState, transmitterState ); - - // Check for convergence. - if( fabs( newLightTimeCalculation - previousLightTimeCalculation ) < tolerance ) - { - // If convergence reached, but light-time corrections not iterated, - // perform 1 more iteration to check for change in correction. - if( !updateLightTimeCorrections ) - { - updateLightTimeCorrections = true; - } - else - { - isToleranceReached = true; - } - } - else - { - // Get out of infinite loop (for instance due to low accuracy state functions, - // to stringent tolerance or limit case for trop. corrections). - if( counter == 20 ) - { - isToleranceReached = true; - std::cerr << "Warning, light time unconverged at level " << - fabs( newLightTimeCalculation - previousLightTimeCalculation ) - << std::endl << "Current light-time corrections are: " << - currentCorrection_<< " and input time was " << ephemerisTime - << std::endl; - } - - // Update light time for new iteration. - previousLightTimeCalculation = newLightTimeCalculation; - } - - counter++; - } - - // Set output variables and return the light time. - receiverStateOutput = receiverState; - transmitterStateOutput = transmitterState; - return newLightTimeCalculation; + return 1.0E-15L; } -//! Function to reset the currentCorrection_ variable during current iteration. -void LightTimeCalculator::setTotalLightTimeCorrection( - const basic_mathematics::Vector6d& transmitterState, - const basic_mathematics::Vector6d& receiverState, - const double transmissionTime, - const double receptionTime ) +//! Function to retrieve the default tolerance for the light-time equation solution. +template< > +double getDefaultLightTimeTolerance< double, long double >( ) { - double totalLightTimeCorrections = 0.0; - for( unsigned int i = 0; i < correctionFunctions_.size( ); i++ ) - { - totalLightTimeCorrections += correctionFunctions_[ i ] - ( transmitterState, receiverState, transmissionTime, receptionTime ); - } - currentCorrection_ = totalLightTimeCorrections; + return 1.0E-12; } -//! Function to calculate a new light-time estimate from the link-ends states. -double LightTimeCalculator::calculateNewLightTimeEstime( - basic_mathematics::Vector6d receiverState, - basic_mathematics::Vector6d transmitterState ) const +//! Function to retrieve the default tolerance for the light-time equation solution. +template< > +long double getDefaultLightTimeTolerance< long double, double >( ) { - return ( receiverState - transmitterState ).segment( 0, 3 ).norm( ) / - physical_constants::SPEED_OF_LIGHT + currentCorrection_; + return 1.0E-12L; } } // namespace observation_models diff --git a/Tudat/Astrodynamics/ObservationModels/lightTimeSolution.h b/Tudat/Astrodynamics/ObservationModels/lightTimeSolution.h index 88905d0833..4e5143b719 100644 --- a/Tudat/Astrodynamics/ObservationModels/lightTimeSolution.h +++ b/Tudat/Astrodynamics/ObservationModels/lightTimeSolution.h @@ -1,59 +1,71 @@ -/* Copyright (c) 2010-2015, Delft University of Technology - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, are - * permitted provided that the following conditions are met: - * - Redistributions of source code must retain the above copyright notice, this list of - * conditions and the following disclaimer. - * - Redistributions in binary form must reproduce the above copyright notice, this list of - * conditions and the following disclaimer in the documentation and/or other materials - * provided with the distribution. - * - Neither the name of the Delft University of Technology nor the names of its contributors - * may be used to endorse or promote products derived from this software without specific - * prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS - * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE - * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING - * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED - * OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Changelog - * YYMMDD Author Comment - * 130226 D. Dirkx Migrated from personal code. - * 130522 E.D. Brandon Minor changes during code check. - * - * References - * - * Notes +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. */ #ifndef TUDAT_LIGHT_TIME_SOLUTIONS_H #define TUDAT_LIGHT_TIME_SOLUTIONS_H - +#include +#include #include +#include +#include #include #include #include "Tudat/Mathematics/BasicMathematics/linearAlgebraTypes.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h" +#include "Tudat/Astrodynamics/ObservationModels/ObservableCorrections/lightTimeCorrection.h" namespace tudat { namespace observation_models { +//! Function to retrieve the default tolerance for the light-time equation solution. +/*! + * Function to retrieve the default tolerance for the light-time equation solution. This tolerance denotes the + * difference between two subsequent light time solutions (in s) that is deemed acceptable for convergence/ + * \return Default light-time tolerance for given template arguments. + */ +template< typename ObservationScalarType = double, typename StateScalarType = ObservationScalarType > +ObservationScalarType getDefaultLightTimeTolerance( ); + + //! Typedef for function calculating light-time correction in light-time calculation loop. typedef boost::function< double( - const basic_mathematics::Vector6d, const basic_mathematics::Vector6d, + const basic_mathematics::Vector6d&, const basic_mathematics::Vector6d&, const double, const double ) > LightTimeCorrectionFunction; +class LightTimeCorrectionFunctionWrapper: public LightTimeCorrection +{ +public: + LightTimeCorrectionFunctionWrapper( + const LightTimeCorrectionFunction lightTimeCorrectionFunction ): + LightTimeCorrection( function_wrapper_light_time_correction ), + lightTimeCorrectionFunction_( lightTimeCorrectionFunction ){ } + + double calculateLightTimeCorrection( + const basic_mathematics::Vector6d& transmitterState, + const basic_mathematics::Vector6d& receiverState, + const double transmissionTime, + const double receptionTime ) + { + return lightTimeCorrectionFunction_( + transmitterState, receiverState, transmissionTime, receptionTime ); + } + +private: + LightTimeCorrectionFunction lightTimeCorrectionFunction_; +}; + //! Class to calculate the light time between two points. /*! * This class calculates the light time between two points, of which the state functions @@ -61,10 +73,16 @@ typedef boost::function< double( * relatvistic corrections) can be applied. The motion of the ends of the link during the * light time is taken into account in the calculations. */ +template< typename ObservationScalarType = double, + typename TimeType = double, + typename StateScalarType = ObservationScalarType > class LightTimeCalculator { public: + + typedef Eigen::Matrix< StateScalarType, 6, 1 > StateType; + typedef Eigen::Matrix< StateScalarType, 3, 1 > PositionType; //! Class constructor. /*! * This constructor is used to initialize the state functions and light-time correction @@ -76,12 +94,10 @@ class LightTimeCalculator * correction during each iteration. */ LightTimeCalculator( - const boost::function< basic_mathematics::Vector6d( const double ) > - positionFunctionOfTransmittingBody, - const boost::function< basic_mathematics::Vector6d( const double ) > - positionFunctionOfReceivingBody, - const std::vector< LightTimeCorrectionFunction > correctionFunctions = - std::vector< LightTimeCorrectionFunction >( ), + const boost::function< StateType( const TimeType ) > positionFunctionOfTransmittingBody, + const boost::function< StateType( const TimeType ) > positionFunctionOfReceivingBody, + const std::vector< boost::shared_ptr< LightTimeCorrection > > correctionFunctions = + std::vector< boost::shared_ptr< LightTimeCorrection > >( ), const bool iterateCorrections = false ): stateFunctionOfTransmittingBody_( positionFunctionOfTransmittingBody ), stateFunctionOfReceivingBody_( positionFunctionOfReceivingBody ), @@ -89,34 +105,74 @@ class LightTimeCalculator iterateCorrections_( iterateCorrections ), currentCorrection_( 0.0 ){ } + LightTimeCalculator( + const boost::function< StateType( const TimeType ) > positionFunctionOfTransmittingBody, + const boost::function< StateType( const TimeType ) > positionFunctionOfReceivingBody, + const std::vector< LightTimeCorrectionFunction > correctionFunctions, + const bool iterateCorrections = false ): + stateFunctionOfTransmittingBody_( positionFunctionOfTransmittingBody ), + stateFunctionOfReceivingBody_( positionFunctionOfReceivingBody ), + iterateCorrections_( iterateCorrections ), + currentCorrection_( 0.0 ) + { + for( unsigned int i = 0; i < correctionFunctions.size( ); i++ ) + { + correctionFunctions_.push_back( + boost::make_shared< LightTimeCorrectionFunctionWrapper >( + correctionFunctions.at( i ) ) ); + } + } + //! Function to calculate the light time. /*! * This function calculates the light time between the link ends defined in the constructor. * The input time can be either at transmission or at reception (default) time. - * \param ephemerisTime Time at reception or transmission. + * \param time Time at reception or transmission. * \param isTimeAtReception True if input time is at reception, false if at transmission. * \param tolerance Maximum allowed light-time difference between two subsequent iterations * for which solution is accepted. * \return The value of the light time between the link ends. */ - double calculateLightTime( const double ephemerisTime, + ObservationScalarType calculateLightTime( const TimeType time, const bool isTimeAtReception = true, - const double tolerance = 1.0E-12 ); + const ObservationScalarType tolerance = + getDefaultLightTimeTolerance< ObservationScalarType, StateScalarType >( ) ) + { + // Declare and initialize variables for receiver and transmitter state (returned by reference). + StateType receiverState; + StateType transmitterState; + + // Calculate light time. + ObservationScalarType lightTime = calculateLightTimeWithLinkEndsStates( + receiverState, transmitterState, time, isTimeAtReception, tolerance ); + return lightTime; + } //! Function to calculate the 'measured' vector from transmitter to receiver. /*! * Function to calculate the vector from transmitter at transmitter time to receiver at * reception time. * The input time can be either at transmission or reception (default) time. - * \param ephemerisTime Time at reception or transmission. + * \param time Time at reception or transmission. * \param isTimeAtReception True if input time is at reception, false if at transmission. * \param tolerance Maximum allowed light-time difference between two subsequent iterations * for which solution is accepted. * \return The vector from the transmitter to the reciever. */ - Eigen::Vector3d calculateRelativeRangeVector( const double ephemerisTime, - const bool isTimeAtReception = true, - const double tolerance = 1.0E-12 ); + PositionType calculateRelativeRangeVector( const TimeType time, + const bool isTimeAtReception = 1 , + const ObservationScalarType tolerance = + getDefaultLightTimeTolerance< ObservationScalarType, StateScalarType >( ) ) + { + // Declare and initialize variables for receiver and transmitter state (returned by reference). + StateType receiverState; + StateType transmitterState; + + // Calculate link end states and the determine range vector. + calculateLightTimeWithLinkEndsStates( receiverState, transmitterState, + time, isTimeAtReception, tolerance ); + return ( receiverState - transmitterState ).segment( 0, 3 ); + } //! Function to calculate the light time and link-ends states. /*! @@ -125,18 +181,126 @@ class LightTimeCalculator * The input time can be either at transmission or reception (default) time. * \param receiverStateOutput Output by reference of receiver state. * \param transmitterStateOutput Output by reference of transmitter state. - * \param ephemerisTime Time at reception or transmission. + * \param time Time at reception or transmission. * \param isTimeAtReception True if input time is at reception, false if at transmission. * \param tolerance Maximum allowed light-time difference between two subsequent iterations * for which solution is accepted. * \return The value of the light time between the reciever state and the transmitter state. */ - double calculateLightTimeWithLinkEndsStates( - basic_mathematics::Vector6d& receiverStateOutput, - basic_mathematics::Vector6d& transmitterStateOutput, - const double ephemerisTime, - const bool isTimeAtReception = true, - const double tolerance = 1.0E-12 ); + ObservationScalarType calculateLightTimeWithLinkEndsStates( + StateType& receiverStateOutput, + StateType& transmitterStateOutput, + const TimeType time, + const bool isTimeAtReception = 1, + const ObservationScalarType tolerance = + ( getDefaultLightTimeTolerance< ObservationScalarType, StateScalarType >( ) ) ) + { + using physical_constants::SPEED_OF_LIGHT; + using std::fabs; + + // Initialize reception and transmission times and states to initial guess (zero light time) + TimeType receptionTime = time; + TimeType transmissionTime = time; + StateType receiverState = stateFunctionOfReceivingBody_( receptionTime ); + StateType transmitterState = + stateFunctionOfTransmittingBody_( transmissionTime ); + + // Set initial light-time correction. + setTotalLightTimeCorrection( + transmitterState, receiverState, transmissionTime, receptionTime ); + + // Calculate light-time solution assuming infinte speed of signal as initial estimate. + ObservationScalarType previousLightTimeCalculation = + calculateNewLightTimeEstime( receiverState, transmitterState ); + + // Set variables for iteration + ObservationScalarType newLightTimeCalculation = 0.0; + bool isToleranceReached = false; + + // Recalculate light-time solution until tolerance is reached. + int counter = 0; + + // Set variable determining whether to update the light time each iteration. + bool updateLightTimeCorrections = false; + if( iterateCorrections_ ) + { + updateLightTimeCorrections = true; + } + + // Iterate until tolerance reached. + while( !isToleranceReached ) + { + // Update light-time corrections, if necessary. + if( updateLightTimeCorrections ) + { + setTotalLightTimeCorrection( + transmitterState, receiverState, transmissionTime, receptionTime ); + } + + // Update light-time estimate for this iteration. + if( isTimeAtReception ) + { + receptionTime = time; + transmissionTime = time - previousLightTimeCalculation; + transmitterState = ( stateFunctionOfTransmittingBody_( transmissionTime ) ); + } + else + { + receptionTime = time + previousLightTimeCalculation; + transmissionTime = time; + receiverState = ( stateFunctionOfReceivingBody_( receptionTime ) ); + } + newLightTimeCalculation = calculateNewLightTimeEstime( receiverState, transmitterState ); + + // Check for convergence. + if( fabs( newLightTimeCalculation - previousLightTimeCalculation ) < tolerance ) + { + // If convergence reached, but light-time corrections not iterated, + // perform 1 more iteration to check for change in correction. + if( !updateLightTimeCorrections ) + { + updateLightTimeCorrections = true; + } + else + { + isToleranceReached = true; + } + } + else + { + // Get out of infinite loop (for instance due to low accuracy state functions, + // to stringent tolerance or limit case for trop. corrections). + if( counter == 20 ) + { + isToleranceReached = true; + std::string errorMessage = + "Warning, light time unconverged at level " + + boost::lexical_cast< std::string >( + fabs( newLightTimeCalculation - previousLightTimeCalculation ) ) + + "; current light-time corrections are: " + + boost::lexical_cast< std::string >( currentCorrection_ ) + " and input time was " + + boost::lexical_cast< std::string >( time ); + throw std::runtime_error( errorMessage ); + } + + // Update light time for new iteration. + previousLightTimeCalculation = newLightTimeCalculation; + } + + counter++; + } + + // Set output variables and return the light time. + receiverStateOutput = receiverState; + transmitterStateOutput = transmitterState; + + return newLightTimeCalculation; + } + + std::vector< boost::shared_ptr< LightTimeCorrection > > getLightTimeCorrection( ) + { + return correctionFunctions_; + } protected: @@ -144,21 +308,21 @@ class LightTimeCalculator /*! * Transmitter state function. */ - boost::function< basic_mathematics::Vector6d( const double ) > + boost::function< StateType( const double ) > stateFunctionOfTransmittingBody_; //! Receiver state function. /*! * Receiver state function. */ - boost::function< basic_mathematics::Vector6d( const double ) > + boost::function< StateType( const double ) > stateFunctionOfReceivingBody_; //! List of light-time correction functions. /*! * List of light-time correction functions, i.e. tropospheric, relativistic, etc. */ - std::vector< LightTimeCorrectionFunction > correctionFunctions_; + std::vector< boost::shared_ptr< LightTimeCorrection > > correctionFunctions_; //! Boolean deciding whether to recalculate the correction during each iteration. /*! @@ -172,9 +336,6 @@ class LightTimeCalculator bool iterateCorrections_; //! Current light-time correction. - /*! - * Current light-time correction. - */ double currentCorrection_; //! Function to calculate a new light-time estimate from the link-ends states. @@ -186,8 +347,14 @@ class LightTimeCalculator * \param transmitterState Assumed state of transmitter. * \return New value of the light-time estimate. */ - double calculateNewLightTimeEstime( basic_mathematics::Vector6d receiverState, - basic_mathematics::Vector6d transmitterState ) const; + ObservationScalarType calculateNewLightTimeEstime( + const StateType& receiverState, + const StateType& transmitterState ) const + { + return ( ( ( receiverState - transmitterState ).segment( 0, 3 ) ). + template cast< ObservationScalarType >( ) ).norm( ) / + physical_constants::getSpeedOfLight< ObservationScalarType >( ) + currentCorrection_; + } //! Function to reset the currentCorrection_ variable during current iteration. /*! @@ -198,10 +365,21 @@ class LightTimeCalculator * \param transmissionTime Time at transmission. * \param receptionTime Time at reception. */ - void setTotalLightTimeCorrection( const basic_mathematics::Vector6d& transmitterState, - const basic_mathematics::Vector6d& receiverState, - const double transmissionTime, - const double receptionTime ); + void setTotalLightTimeCorrection( const StateType& transmitterState, + const StateType& receiverState, + const TimeType transmissionTime , + const TimeType receptionTime ) + { + ObservationScalarType totalLightTimeCorrections = mathematical_constants::getFloatingInteger< ObservationScalarType >( 0 ); + for( unsigned int i = 0; i < correctionFunctions_.size( ); i++ ) + { + totalLightTimeCorrections += static_cast< ObservationScalarType >( + correctionFunctions_[ i ]->calculateLightTimeCorrection( + transmitterState.template cast< double >( ), receiverState.template cast< double >( ), + static_cast< double >( transmissionTime ), static_cast< double >( receptionTime ) ) ); + } + currentCorrection_ = totalLightTimeCorrections; + } private: }; diff --git a/Tudat/Astrodynamics/ObservationModels/linkTypeDefs.cpp b/Tudat/Astrodynamics/ObservationModels/linkTypeDefs.cpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tudat/Astrodynamics/ObservationModels/linkTypeDefs.h b/Tudat/Astrodynamics/ObservationModels/linkTypeDefs.h new file mode 100644 index 0000000000..4548e3abfe --- /dev/null +++ b/Tudat/Astrodynamics/ObservationModels/linkTypeDefs.h @@ -0,0 +1,46 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_LINKTYPEDEFS_H +#define TUDAT_LINKTYPEDEFS_H + +#include +#include +#include + +namespace tudat +{ + +namespace observation_models +{ + +//! Enum defining different link end types. +/*! + * Enum defining different roles that a given link end can play in an observation model. + */ +enum LinkEndType +{ + transmitter = 0, + reflector = 1, + receiver = 2, + observed_body = 3 +}; + +//! Typedef for the identifier of a given link-end (body and reference points) +typedef std::pair< std::string, std::string > LinkEndId; + +//! Typedef for list of link ends, with associated role, used for a single observation (model). +typedef std::map< LinkEndType, LinkEndId > LinkEnds; + +} // namespace observation_models + +} // namespace tudat + +#endif // TUDAT_LINKTYPEDEFS_H diff --git a/Tudat/Astrodynamics/ObservationModels/observableTypes.cpp b/Tudat/Astrodynamics/ObservationModels/observableTypes.cpp new file mode 100644 index 0000000000..969e20353e --- /dev/null +++ b/Tudat/Astrodynamics/ObservationModels/observableTypes.cpp @@ -0,0 +1,75 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include + +#include "Tudat/Astrodynamics/ObservationModels/observableTypes.h" + +namespace tudat +{ + +namespace observation_models +{ + +//! Function to get the name (string) associated with a given observable type. +std::string getObservableName( const ObservableType observableType ) +{ + std::string observableName; + switch( observableType ) + { + case oneWayRange: + observableName = "OneWayRange"; + break; + case angular_position: + observableName = "AngularPosition"; + break; + case position_observable: + observableName = "CartesianPosition"; + break; + default: + std::string errorMessage = + "Error, could not find observable type "+ boost::lexical_cast< std::string >( observableType ) + + " when getting name from type"; + throw std::runtime_error( errorMessage ); + } + return observableName; +} + +//! Function to get the observable type.ssociated with the name (string) of observable. +ObservableType getObservableType( const std::string& observableName ) +{ + ObservableType observableType; + + if( observableName == "OneWayRange" ) + { + observableType = oneWayRange; + } + else if( observableName == "AngularPosition" ) + { + observableType = angular_position; + } + else if( observableName == "CartesianPosition" ) + { + observableType = position_observable; + } + else + { + std::string errorMessage = + "Error, could not find observable name "+ observableName + + " when getting type from name"; + throw std::runtime_error( errorMessage ); + } + + return observableType; +} + +} + +} diff --git a/Tudat/Astrodynamics/ObservationModels/observableTypes.h b/Tudat/Astrodynamics/ObservationModels/observableTypes.h new file mode 100644 index 0000000000..d7ea126767 --- /dev/null +++ b/Tudat/Astrodynamics/ObservationModels/observableTypes.h @@ -0,0 +1,55 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_OBSERVABLETYPES_H +#define TUDAT_OBSERVABLETYPES_H + +#include + +#include "Tudat/Astrodynamics/ObservationModels/linkTypeDefs.h" + +namespace tudat +{ + +namespace observation_models +{ + + +//! Enum for types of observations +enum ObservableType +{ + oneWayRange = 0, + angular_position = 1, + position_observable = 2 + +}; + +//! Function to get the name (string) associated with a given observable type. +/*! + * Function to get the name (string) associated with a given observable type. + * \param observableType Type of observable. + * \return Name of observable + */ +std::string getObservableName( const ObservableType observableType ); + +//! Function to get the observable type.ssociated with the name (string) of observable. +/*! + * Function to get the observable type.ssociated with the name (string) of observable. + * \param observableName of observable + * \return observableType Type of observable. + */ +ObservableType getObservableType( const std::string& observableName ); + + +} // namespace observation_models + +} // namespace tudat + +#endif // TUDAT_OBSERVABLETYPES_H diff --git a/Tudat/Astrodynamics/ObservationModels/observationBias.h b/Tudat/Astrodynamics/ObservationModels/observationBias.h new file mode 100644 index 0000000000..723192a654 --- /dev/null +++ b/Tudat/Astrodynamics/ObservationModels/observationBias.h @@ -0,0 +1,118 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_OBSERVATIONBIAS_H +#define TUDAT_OBSERVATIONBIAS_H + +#include + +#include +#include +#include + +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h" + +#include "Tudat/Mathematics/BasicMathematics/linearAlgebraTypes.h" + +#include "Tudat/Astrodynamics/ObservationModels/linkTypeDefs.h" +#include "Tudat/Astrodynamics/ObservationModels/observableTypes.h" + +namespace tudat +{ + +namespace observation_models +{ + + +//! Base class (non-functional) for describing observation biases +/*! + * Base class (non-functional) for describing observation biases. In this context, an observation bias denotes any deviation + * from the ideal observable between two reference points. + */ +template< int ObservationSize = 1 > +class ObservationBias +{ +public: + + //! Constructor + ObservationBias( ){ } + + //! Destructor + virtual ~ObservationBias( ){ } + + //! Pure virtual function to retrieve the observation bias. + /*! + * Pure virtual function to retrieve the observation bias as a function of the observation times and states (which are + * typically computed by the ObservationModel) + * \param linkEndTimes List of times at each link end during observation. + * \param linkEndStates List of states at each link end during observation. + * \return Observation bias at given times and states. + */ + virtual Eigen::Matrix< double, ObservationSize, 1 > getObservationBias( + const std::vector< double >& linkEndTimes, + const std::vector< Eigen::Matrix< double, 6, 1 > >& linkEndStates ) = 0; + + //! Function to return the size of the associated observation + /*! + * Function to return the size of the associated observation + * \return Size of the associated observation + */ + int getObservationSize( ) + { + return ObservationSize; + } +}; + +//! Class for a constant observation bias of a given size +template< int ObservationSize = 1 > +class ConstantObservationBias: public ObservationBias< ObservationSize > +{ +public: + + //! Constructor + /*! + * Constructor + * \param observationBias Constant (entry-wise) observation bias. + */ + ConstantObservationBias( const Eigen::Matrix< double, ObservationSize, 1 > observationBias ): + observationBias_( observationBias ){ } + + //! Destructor + ~ConstantObservationBias( ){ } + + //! Function to retrieve the constant observation bias. + /*! + * Function to retrieve the constant observation bias. + * \param linkEndTimes List of times at each link end during observation (unused). + * \param linkEndStates List of states at each link end during observation (unused). + * \return Constant observation bias. + */ + Eigen::Matrix< double, ObservationSize, 1 > getObservationBias( + const std::vector< double >& linkEndTimes, + const std::vector< Eigen::Matrix< double, 6, 1 > >& linkEndStates ) + { + return observationBias_; + } + + +private: + + //! Constant (entry-wise) observation bias. + Eigen::Matrix< double, ObservationSize, 1 > observationBias_; + +}; + +} // namespace observation_models + +} // namespace tudat + +#endif // TUDAT_OBSERVATIONMODEL_H diff --git a/Tudat/Astrodynamics/ObservationModels/observationModel.h b/Tudat/Astrodynamics/ObservationModels/observationModel.h new file mode 100644 index 0000000000..a847b2b083 --- /dev/null +++ b/Tudat/Astrodynamics/ObservationModels/observationModel.h @@ -0,0 +1,254 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_OBSERVATIONMODEL_H +#define TUDAT_OBSERVATIONMODEL_H + +#include + +#include +#include + +#include + +#include "Tudat/Mathematics/BasicMathematics/linearAlgebraTypes.h" + +#include "Tudat/Astrodynamics/ObservationModels/linkTypeDefs.h" +#include "Tudat/Astrodynamics/ObservationModels/observableTypes.h" +#include "Tudat/Astrodynamics/ObservationModels/observationBias.h" + +namespace tudat +{ + +namespace observation_models +{ + +//! Base class for models of observables (i.e. range, range-rate, etc.). +/*! + * Base class for models of observables to be used in (for instance) orbit determination. + * Each type of observables (1-way range, 2-way range, Doppler, VLBI, etc.) has its own + * derived class capable of simulating observables of the given type using given link ends. + * The functions to be used for computing the observables can be called with/without deviations from ideal observable + * (see base class member functions). Corrections are computed from an observationBiasCalculator member object, which is + * empty by default. Also, the observable may be a with/without returning (by reference) the times and states + * at each of the link ends. Returning these times/states prevents recomputations of these quantities in later calculations. + */ +template< int ObservationSize = Eigen::Dynamic, typename ObservationScalarType = double, typename TimeType = double, + typename StateScalarType = ObservationScalarType > +class ObservationModel +{ +public: + + //! Constructor + /*! + * Base class constructor. + * \param observableType Type of observable, used for derived class type identification without + * explicit casts. + * \param observationBiasCalculator Object for calculating system-dependent errors in the + * observable, i.e. deviations from the physically ideal observable between reference points (default none). + */ + ObservationModel( + const ObservableType observableType , + const boost::shared_ptr< ObservationBias< ObservationSize > > observationBiasCalculator = NULL ): + observableType_( observableType ), + observationBiasCalculator_( observationBiasCalculator ) + { + // Check if bias is empty + if( observationBiasCalculator_ != NULL ) + { + isBiasNull_ = 0; + if( observationBiasCalculator_->getObservationSize( ) != ObservationSize ) + { + throw std::runtime_error( "Error when making observation model, bias size is inconsistent" ); + } + } + else + { + isBiasNull_ = 1; + } + } + + //! Virtual destructor + virtual ~ObservationModel( ) { } + + //! Function to return the type of observable. + /*! + * Function to return the type of observable. + * \return Type of observable. + */ + ObservableType getObservableType( ) + { + return observableType_; + } + + //! Function to compute the observable without any corrections + /*! + * Function to compute the observable without any corrections, i.e. the ideal physical observable as computed + * from the defined link ends (in the derived class). Note that this observable does include e.g. light-time + * corrections, which represent physically true corrections. It does not include e.g. system-dependent measurement + * errors, such as biases or clock errors. + * The times and states of the link ends are also returned in full precision (determined by class template + * arguments). These states and times are returned by reference. + * \param time Time at which observable is to be evaluated. + * \param linkEndAssociatedWithTime Link end at which given time is valid, i.e. link end for which associated time + * is kept constant (to input value) + * \param linkEndTimes List of times at each link end during observation (returned by reference). + * \param linkEndStates List of states at each link end during observation (returned by reference). + * \return Ideal observable. + */ + virtual Eigen::Matrix< ObservationScalarType, ObservationSize, 1 > computeIdealObservationsWithLinkEndData( + const TimeType time, + const LinkEndType linkEndAssociatedWithTime, + std::vector< TimeType >& linkEndTimes, + std::vector< Eigen::Matrix< StateScalarType, 6, 1 > >& linkEndStates ) = 0; + + //! Function to compute full observation at given time. + /*! + * Function to compute observation at given time (include any defined non-ideal corrections). The + * times and states of the link ends are given in full precision (determined by class template + * arguments). These states and times are returned by reference. + * \param time Time at which observation is to be simulated + * \param linkEndAssociatedWithTime Link end at which current time is measured, i.e. reference + * link end for observable. + * \param linkEndTimes List of times at each link end during observation (returned by reference). + * \param linkEndStates List of states at each link end during observation (returned by reference). + * \return Calculated observable value. + */ + Eigen::Matrix< ObservationScalarType, ObservationSize, 1 > computeObservationsWithLinkEndData( + const TimeType time, + const LinkEndType linkEndAssociatedWithTime, + std::vector< TimeType >& linkEndTimes , + std::vector< Eigen::Matrix< StateScalarType, 6, 1 > >& linkEndStates ) + { + // Check if any non-ideal models are set. + if( isBiasNull_ ) + { + return computeIdealObservationsWithLinkEndData( + time, linkEndAssociatedWithTime, linkEndTimes, linkEndStates ); + } + else + { + // Compute ideal observable + Eigen::Matrix< ObservationScalarType, ObservationSize, 1 > currentObservation = + computeIdealObservationsWithLinkEndData( + time, linkEndAssociatedWithTime, linkEndTimes, linkEndStates ); + + // Add correction + return currentObservation + + this->observationBiasCalculator_->getObservationBias( linkEndTimes, linkEndStates ). + template cast< ObservationScalarType >( ); + } + } + + //! Function to compute the observable without any corrections. + /*! + * Function to compute the observable without any corrections, i.e. the ideal physical observable as computed + * from the defined link ends (in the derived class). Note that this observable does include e.g. light-time + * corrections, which represent physically true corrections. It does not include e.g. system-dependent measurement + * errors, such as biases or clock errors. This function may be redefined in derived class for improved efficiency. + * \param time Time at which observable is to be evaluated. + * \param linkEndAssociatedWithTime Link end at which given time is valid, i.e. link end for which associated time + * is kept constant (to input value) + * \return Ideal observable. + */ + virtual Eigen::Matrix< ObservationScalarType, ObservationSize, 1 > computeIdealObservations( + const TimeType time, + const LinkEndType linkEndAssociatedWithTime ) + { + // Compute ideal observable from derived class. + return this->computeIdealObservationsWithLinkEndData( + time, linkEndAssociatedWithTime, this->linkEndTimes_, this->linkEndStates_ ); + } + + //! Function to compute full observation at given time. + /*! + * Function to compute observation at given time (include any defined non-ideal corrections). + * \param time Time at which observable is to be evaluated. + * \param linkEndAssociatedWithTime Link end at which given time is valid, i.e. link end for which associated time + * is kept constant (to input value) + * \return Calculated (non-ideal) observable value. + */ + Eigen::Matrix< ObservationScalarType, ObservationSize, 1 > computeObservations( + const TimeType time, + const LinkEndType linkEndAssociatedWithTime ) + { + // Check if any non-ideal models are set. + if( isBiasNull_ ) + { + return computeObservations( time, linkEndAssociatedWithTime ); + } + else + { + // Compute ideal observable + Eigen::Matrix< ObservationScalarType, ObservationSize, 1 > currentObservation = + computeIdealObservationsWithLinkEndData( + time, linkEndAssociatedWithTime, linkEndTimes_, linkEndStates_ ); + + // Add correction + return currentObservation + + this->observationBiasCalculator_->getObservationBias( linkEndTimes_, linkEndStates_ ). + template cast< ObservationScalarType >( ); + } + } + + ObservationScalarType computeObservationEntry( + const TimeType time, + const LinkEndType linkEndAssociatedWithTime, + const int observationEntry ) + { + if( observationEntry < ObservationSize ) + { + return computeObservations( time, linkEndAssociatedWithTime )( observationEntry ); + } + else + { + throw std::runtime_error( "Error, requesting out-of-bounds index for observation model" ); + } + } + + //! Function to return the size of the observable + /*! + * Function to return the size of the observable + * \return Size of the observable + */ + int getObservationSize( ) + { + return ObservationSize; + } + +protected: + + //! Type of observable, used for derived class type identification without explicit casts. + ObservableType observableType_; + + //! Object for calculating system-dependent errors in the observable. + /*! + * Object for calculating system-dependent errors in the observable, i.e. deviations from the + * physically true observable + */ + boost::shared_ptr< ObservationBias< ObservationSize > > observationBiasCalculator_; + + //! Boolean set by constructor to denote whether observationBiasCalculator_ is NULL. + bool isBiasNull_; + + + //! Pre-define list of times used when calling function returning link-end states/times from interface function. + std::vector< TimeType > linkEndTimes_; + + //! Pre-define list of states used when calling function returning link-end states/times from interface function. + std::vector< Eigen::Matrix< StateScalarType, 6, 1 > > linkEndStates_; + +}; + + +} // namespace observation_models + +} // namespace tudat +#endif // TUDAT_OBSERVATIONMODEL_H diff --git a/Tudat/Astrodynamics/ObservationModels/oneWayRangeObservationModel.h b/Tudat/Astrodynamics/ObservationModels/oneWayRangeObservationModel.h new file mode 100644 index 0000000000..52f2ae92ef --- /dev/null +++ b/Tudat/Astrodynamics/ObservationModels/oneWayRangeObservationModel.h @@ -0,0 +1,194 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_ONEWAYRANGEOBSERVATIONMODEL_H +#define TUDAT_ONEWAYRANGEOBSERVATIONMODEL_H + +#include + +#include +#include + +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h" + +#include "Tudat/Astrodynamics/Ephemerides/simpleRotationalEphemeris.h" +#include "Tudat/Astrodynamics/ObservationModels/observationModel.h" +#include "Tudat/Astrodynamics/ObservationModels/lightTimeSolution.h" + +namespace tudat +{ + +namespace observation_models +{ + +//! Class for simulating one-way range observables. +/*! + * Class for simulating one-way range, based on light-time and light-time corrections. + * The one-way range is defined as the light time multiplied by speed of light. + * The user may add observation biases to model system-dependent deviations between measured and true observation. + */ +template< typename ObservationScalarType = double, + typename TimeType = double, + typename StateScalarType = ObservationScalarType > +class OneWayRangeObservationModel: public ObservationModel< 1, ObservationScalarType, TimeType, StateScalarType > +{ +public: + typedef Eigen::Matrix< StateScalarType, 6, 1 > StateType; + typedef Eigen::Matrix< StateScalarType, 3, 1 > PositionType; + + //! Constructor. + /*! + * Constructor, + * \param lightTimeCalculator Object to compute the light-time (including any corrections w.r.t. Euclidean case) + * \param observationBiasCalculator Object for calculating system-dependent errors in the + * observable, i.e. deviations from the physically ideal observable between reference points (default none). + */ + OneWayRangeObservationModel( + const boost::shared_ptr< observation_models::LightTimeCalculator + < ObservationScalarType, TimeType, StateScalarType > > lightTimeCalculator, + const boost::shared_ptr< ObservationBias< 1 > > observationBiasCalculator = NULL ): + ObservationModel< 1, ObservationScalarType, TimeType, StateScalarType >( oneWayRange, observationBiasCalculator ), + lightTimeCalculator_( lightTimeCalculator ){ } + + //! Destructor + ~OneWayRangeObservationModel( ){ } + + //! Function to compute ideal one-way range observation at given time. + /*! + * This function compute ideal the one-way observation at a given time. The time argument can be either the reception + * or transmission time (defined by linkEndAssociatedWithTime input) Note that this observable does include e.g. + * light-time corrections, which represent physically true corrections. + * It does not include e.g. system-dependent measurement. + * \param time Time at which observation is to be simulated + * \param linkEndAssociatedWithTime Link end at which given time is valid, i.e. link end for which associated time + * is kept constant (to input value) + * \return Calculated observed one-way range value. + */ + Eigen::Matrix< ObservationScalarType, 1, 1 > computeIdealObservations( + const TimeType time, + const LinkEndType linkEndAssociatedWithTime ) + + { + // Check link end associated with input time. + bool isTimeAtReception = -1; + if( linkEndAssociatedWithTime == receiver ) + { + isTimeAtReception = 1; + } + else if( linkEndAssociatedWithTime == transmitter ) + { + isTimeAtReception = 0; + } + else + { + throw std::runtime_error( + "Error when calculating one way range observation, link end is not transmitter or receiver" ); + } + + // Calculate light-time and multiply by speed of light in vacuum. + return ( Eigen::Matrix< ObservationScalarType, 1, 1 >( ) << + lightTimeCalculator_->calculateLightTime( time, isTimeAtReception ) * + physical_constants::getSpeedOfLight< ObservationScalarType >( ) ).finished( ); + } + + //! Function to compute one-way range observable without any corrections. + /*! + * Function to compute one-way range observable without any corrections, i.e. the true physical range as computed + * from the defined link ends. Note that this observable does include light-time + * corrections, which represent physically true corrections. It does not include e.g. system-dependent measurement + * errors, such as biases or clock errors. + * The times and states of the link ends are also returned in full precision (determined by class template + * arguments). These states and times are returned by reference. + * \param time Time at which observable is to be evaluated. + * \param linkEndAssociatedWithTime Link end at which given time is valid, i.e. link end for which associated time + * is kept constant (to input value) + * \param linkEndTimes List of times at each link end during observation. + * \param linkEndStates List of states at each link end during observation. + * \return Ideal one-way range observable. + */ + Eigen::Matrix< ObservationScalarType, 1, 1 > computeIdealObservationsWithLinkEndData( + const TimeType time, + const LinkEndType linkEndAssociatedWithTime, + std::vector< TimeType >& linkEndTimes, + std::vector< Eigen::Matrix< StateScalarType, 6, 1 > >& linkEndStates ) + { + ObservationScalarType observation = TUDAT_NAN; + TimeType transmissionTime = TUDAT_NAN, receptionTime = TUDAT_NAN; + + // Check link end associated with input time and compute observable + switch( linkEndAssociatedWithTime ) + { + case receiver: + observation = lightTimeCalculator_->calculateLightTimeWithLinkEndsStates( + receiverState, transmitterState, time, 1 ); + transmissionTime = time - observation; + receptionTime = time; + break; + + case transmitter: + observation = lightTimeCalculator_->calculateLightTimeWithLinkEndsStates( + receiverState, transmitterState, time, 0 ); + transmissionTime = time; + receptionTime = time + observation; + break; + default: + std::string errorMessage = "Error, cannot have link end type: " + + boost::lexical_cast< std::string >( linkEndAssociatedWithTime ) + "for one-way range"; + throw std::runtime_error( errorMessage ); + } + + // Convert light time to range. + observation *= physical_constants::getSpeedOfLight< ObservationScalarType >( ); + + // Set link end states and times. + linkEndTimes.push_back( transmissionTime ); + linkEndTimes.push_back( receptionTime ); + + linkEndStates.push_back( transmitterState); + linkEndStates.push_back( receiverState ); + + return ( Eigen::Matrix< ObservationScalarType, 1, 1 >( ) << observation ).finished( ); + } + + //! Function to get the object to calculate light time. + /*! + * Function to get the object to calculate light time. + * \return Object to calculate light time. + */ + boost::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType, StateScalarType > > + getLightTimeCalculator( ) + { + return lightTimeCalculator_; + } + +private: + + //! Object to calculate light time. + /*! + * Object to calculate light time, including possible corrections from troposphere, relativistic corrections, etc. + */ + boost::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType, StateScalarType > > + lightTimeCalculator_; + + //! Pre-declared receiver state, to prevent many (de-)allocations + StateType receiverState; + + //! Pre-declared transmitter state, to prevent many (de-)allocations + StateType transmitterState; + +}; + +} // namespace observation_models + +} // namespace tudat + +#endif // TUDAT_ONEWAYRANGEOBSERVATIONMODEL_H diff --git a/Tudat/Astrodynamics/ObservationModels/positionObservationModel.h b/Tudat/Astrodynamics/ObservationModels/positionObservationModel.h new file mode 100644 index 0000000000..d84a6557f4 --- /dev/null +++ b/Tudat/Astrodynamics/ObservationModels/positionObservationModel.h @@ -0,0 +1,123 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_POSITIONOBSERVATIONMODEL_H +#define TUDAT_POSITIONOBSERVATIONMODEL_H + +#include +#include + +#include "Tudat/Astrodynamics/Ephemerides/ephemeris.h" + +#include "Tudat/Astrodynamics/ObservationModels/observationModel.h" + +namespace tudat +{ + +namespace observation_models +{ + +//! Class for simulating observations of three-dimensional position. +/*! + * Class for simulating observations of three-dimensional position. This observable is typically not realized in + * practice, but its use can be very valuable in simulation studies + */ +template< typename ObservationScalarType = double, typename TimeType = double, typename StateScalarType = ObservationScalarType > +class PositionObservationModel: public ObservationModel< 3, ObservationScalarType, TimeType, StateScalarType > +{ +public: + + //! Constructor. + /*! + * Constructor, + * \param stateFunction Function that returns the Cartesian state of the observed body as a function of time. + * \param observationBiasCalculator Object for calculating system-dependent errors in the + * observable, i.e. deviations from the physically ideal observable (default none). + */ + PositionObservationModel( + const boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType& ) > stateFunction, + const boost::shared_ptr< ObservationBias< 3 > > observationBiasCalculator = NULL ): + ObservationModel< 3, ObservationScalarType, TimeType, StateScalarType >( + position_observable, observationBiasCalculator ), stateFunction_( stateFunction ){ } + + //! Destructor + ~PositionObservationModel( ) { } + + //! Function to compute ideal position observation at given time. + /*! + * This function computes the ideal position observation at a given time (without biases). + * \param time Time at which observation is to be simulated + * \param linkEndAssociatedWithTime Link end at which given time is valid (must be observed_body for this derived class) + * \return Calculated observed position of body. + */ + Eigen::Matrix< ObservationScalarType, 3, 1 > computeIdealObservations( + const TimeType time, + const LinkEndType linkEndAssociatedWithTime = observed_body ) + { + // Check link end + if( linkEndAssociatedWithTime != observed_body ) + { + throw std::runtime_error( + "Error when computing position observable, associated link end must be observed_body " ); + } + + // Compute and return state. + return stateFunction_( time ).segment( 0, 3 ); + } + + //! Function to compute ideal position observation at given time. + /*! + * This function computes the ideal position observation at a given time (without biases). + * The times and states of the link ends are also returned in full precision (determined by class template + * arguments). These states and times are returned by reference. + * \param time Time at which observable is to be evaluated. + * \param linkEndAssociatedWithTime Link end at which given time is valid (must be observed_body for this derived class) + * \param linkEndTimes List of times at each link end during observation. + * \param linkEndStates List of states at each link end during observation. + * \return Ideal position observable. + */ + Eigen::Matrix< StateScalarType, 3, 1 > computeIdealObservationsWithLinkEndData( + const TimeType time, + const LinkEndType linkEndAssociatedWithTime, + std::vector< TimeType >& linkEndTimes, + std::vector< Eigen::Matrix< StateScalarType, 6, 1 > >& linkEndStates ) + { + // Check link end + if( linkEndAssociatedWithTime != observed_body ) + { + throw std::runtime_error( + "Error when computing position observable, associated link end must be observed_body " ); + } + + // Set link end times and states. + linkEndTimes.clear( ); + linkEndTimes.push_back( static_cast< TimeType >( time ) ); + + linkEndStates.clear( ); + linkEndStates.push_back( stateFunction_( time ).template cast< StateScalarType >( ) ); + + // Retrieve position + Eigen::Matrix< ObservationScalarType, 3, 1 > observation = linkEndStates.at( 0 ).segment( 0, 3 ); + + return observation; + } + + +private: + + //! Function that returns the Cartesian state of the observed body as a function of time. + boost::function< Eigen::Matrix< ObservationScalarType, 6, 1 >( const TimeType& ) > stateFunction_; +}; + +} // namespace observation_models + +} // namespace tudat + +#endif // TUDAT_POSITIONOBSERVATIONMODEL_H diff --git a/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/CMakeLists.txt b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/CMakeLists.txt new file mode 100644 index 0000000000..94ebab9fdb --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/CMakeLists.txt @@ -0,0 +1,56 @@ + # Copyright (c) 2010-2016, Delft University of Technology + # All rigths reserved + # + # This file is part of the Tudat. Redistribution and use in source and + # binary forms, with or without modification, are permitted exclusively + # under the terms of the Modified BSD license. You should have received + # a copy of the license with this file. If not, please or visit: + # http://tudat.tudelft.nl/LICENSE. + # + # Changelog + # YYMMDD Author Comment + # 110820 S.M. Persson File created. + # 111025 K. Kumar Adapted file to work with Revision 194. + # 111026 K. Kumar Adapted file so all headers show in project tree in Qt Creator. + # + +# Set the source files. +set(ACCELERATION_PARTIALS_SOURCES + "${SRCROOT}${ACCELERATIONPARTIALSDIR}/accelerationPartial.cpp" + "${SRCROOT}${ACCELERATIONPARTIALSDIR}/centralGravityAccelerationPartial.cpp" + "${SRCROOT}${ACCELERATIONPARTIALSDIR}/numericalAccelerationPartial.cpp" + "${SRCROOT}${ACCELERATIONPARTIALSDIR}/radiationPressureAccelerationPartial.cpp" + "${SRCROOT}${ACCELERATIONPARTIALSDIR}/sphericalHarmonicPartialFunctions.cpp" + "${SRCROOT}${ACCELERATIONPARTIALSDIR}/sphericalHarmonicAccelerationPartial.cpp" +) + +# Set the header files. +set(ACCELERATION_PARTIALS_HEADERS + "${SRCROOT}${ACCELERATIONPARTIALSDIR}/accelerationPartial.h" + "${SRCROOT}${ACCELERATIONPARTIALSDIR}/thirdBodyGravityPartial.h" + "${SRCROOT}${ACCELERATIONPARTIALSDIR}/centralGravityAccelerationPartial.h" + "${SRCROOT}${ACCELERATIONPARTIALSDIR}/numericalAccelerationPartial.h" + "${SRCROOT}${ACCELERATIONPARTIALSDIR}/radiationPressureAccelerationPartial.h" + "${SRCROOT}${ACCELERATIONPARTIALSDIR}/sphericalHarmonicPartialFunctions.h" + "${SRCROOT}${ACCELERATIONPARTIALSDIR}/sphericalHarmonicAccelerationPartial.h" +) + +# Add static libraries. +add_library(tudat_acceleration_partials STATIC ${ACCELERATION_PARTIALS_SOURCES} ${ACCELERATION_PARTIALS_HEADERS}) +setup_tudat_library_target(tudat_acceleration_partials "${SRCROOT}{ACCELERATIONPARTIALSDIR}") + +if(USE_CSPICE) + +# Add unit tests +add_executable(test_AccelerationPartials "${SRCROOT}${ACCELERATIONPARTIALSDIR}/UnitTests/unitTestAccelerationPartials.cpp") +setup_custom_test_program(test_AccelerationPartials "${SRCROOT}${ACCELERATIONPARTIALSDIR}") +target_link_libraries(test_AccelerationPartials ${TUDAT_ESTIMATION_LIBRARIES} ${Boost_LIBRARIES}) + +# Add unit tests +add_executable(test_SphericalHarmonicPartials "${SRCROOT}${ACCELERATIONPARTIALSDIR}/UnitTests/unitTestSphericalHarmonicPartials.cpp") +setup_custom_test_program(test_SphericalHarmonicPartials "${SRCROOT}${ACCELERATIONPARTIALSDIR}") +target_link_libraries(test_SphericalHarmonicPartials ${TUDAT_ESTIMATION_LIBRARIES} ${Boost_LIBRARIES}) + +endif( ) + + diff --git a/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/UnitTests/unitTestAccelerationPartials.cpp b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/UnitTests/unitTestAccelerationPartials.cpp new file mode 100644 index 0000000000..36ba7ad14d --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/UnitTests/unitTestAccelerationPartials.cpp @@ -0,0 +1,432 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#define BOOST_TEST_MAIN + +#include +#include +#include "Tudat/Basics/testMacros.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h" + +#include +#include +#include + +#include "Tudat/Astrodynamics/Aerodynamics/exponentialAtmosphere.h" +#include "Tudat/Astrodynamics/Gravitation/centralGravityModel.h" +#include "Tudat/External/SpiceInterface/spiceInterface.h" +#include "Tudat/InputOutput/basicInputOutput.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/gravitationalParameter.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/initialTranslationalState.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/radiationPressureCoefficient.h" +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/numericalAccelerationPartial.h" +#include "Tudat/SimulationSetup/EstimationSetup/createAccelerationPartials.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h" +#include "Tudat/SimulationSetup/PropagationSetup/createAccelerationModels.h" +#include "Tudat/SimulationSetup/EstimationSetup/createEstimatableParameters.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h" + +namespace tudat +{ +namespace unit_tests +{ + +using namespace tudat::gravitation; +using namespace tudat::aerodynamics; +using namespace tudat::ephemerides; +using namespace tudat::simulation_setup; +using namespace tudat::orbital_element_conversions; +using namespace tudat::unit_conversions; +using namespace tudat::orbit_determination; +using namespace tudat::acceleration_partials; +using namespace tudat::spice_interface; +using namespace tudat::orbit_determination; +using namespace tudat::estimatable_parameters; +using namespace tudat::electro_magnetism; + +BOOST_AUTO_TEST_SUITE( test_acceleration_partials ) + +BOOST_AUTO_TEST_CASE( testCentralGravityPartials ) +{ + // Create empty bodies, earth and sun. + boost::shared_ptr< Body > earth = boost::make_shared< Body >( ); + boost::shared_ptr< Body > sun = boost::make_shared< Body >( ); + + NamedBodyMap bodyMap; + bodyMap[ "Earth" ] = earth; + bodyMap[ "Sun" ] = sun; + + // Load spice kernels. + std::string kernelsPath = input_output::getSpiceKernelPath( ); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de-403-masses.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de421.bsp"); + + // Set current state of sun and earth. + sun->setState( getBodyCartesianStateAtEpoch( "Sun", "Sun", "J2000", "NONE", 1.0E6 ) ); + earth->setState( getBodyCartesianStateAtEpoch( "Earth", "Sun", "J2000", "NONE", 1.0E6 ) ); + + // Get sun gravitational parameter and set gravity field model. + double sunsGravitationalParameter = getBodyGravitationalParameter( "Sun" ); + boost::shared_ptr< GravityFieldModel > sunGravityFieldModel = + boost::make_shared< GravityFieldModel >( sunsGravitationalParameter ); + sun->setGravityFieldModel( sunGravityFieldModel ); + double earthGravitationalParameter = getBodyGravitationalParameter( "Earth" ); + boost::shared_ptr< GravityFieldModel > earthGravityFieldModel = + boost::make_shared< GravityFieldModel >( earthGravitationalParameter ); + earth->setGravityFieldModel( earthGravityFieldModel ); + + // Create acceleration due to sun on earth. + boost::shared_ptr< CentralGravitationalAccelerationModel3d > gravitationalAcceleration =\ + createCentralGravityAcceleratioModel( earth, sun, "Earth", "Sun", 1 ); + + // Create central gravity partial. + boost::shared_ptr< AccelerationPartial > centralGravitationPartial = + createAnalyticalAccelerationPartial( gravitationalAcceleration, std::make_pair( "Earth", earth ), + std::make_pair( "Sun", sun ), bodyMap ); + + // Create gravitational parameter object. + boost::shared_ptr< EstimatableParameter< double > > sunGravitationalParameterParameter = boost::make_shared< + GravitationalParameter >( sunGravityFieldModel, "Sun" ); + boost::shared_ptr< EstimatableParameter< double > > earthGravitationalParameterParameter = boost::make_shared< + GravitationalParameter >( earthGravityFieldModel, "Earth" ); + + // Calculate analytical partials. + centralGravitationPartial->update( 0.0 ); + Eigen::MatrixXd partialWrtEarthPosition = Eigen::Matrix3d::Zero( ); + centralGravitationPartial->wrtPositionOfAcceleratedBody( partialWrtEarthPosition.block( 0, 0, 3, 3 ) ); + Eigen::MatrixXd partialWrtEarthVelocity = Eigen::Matrix3d::Zero( ); + centralGravitationPartial->wrtVelocityOfAcceleratedBody( partialWrtEarthVelocity.block( 0, 0, 3, 3 ), 1, 0, 0 ); + Eigen::MatrixXd partialWrtSunPosition = Eigen::Matrix3d::Zero( ); + centralGravitationPartial->wrtPositionOfAcceleratingBody( partialWrtSunPosition.block( 0, 0, 3, 3 ) ); + Eigen::MatrixXd partialWrtSunVelocity = Eigen::Matrix3d::Zero( ); + centralGravitationPartial->wrtVelocityOfAcceleratingBody( partialWrtSunVelocity.block( 0, 0, 3, 3 ), 1, 0, 0 ); + Eigen::Vector3d partialWrtSunGravitationalParameter = centralGravitationPartial->wrtParameter( + sunGravitationalParameterParameter ); + Eigen::Vector3d partialWrtEarthGravitationalParameter = centralGravitationPartial->wrtParameter( + earthGravitationalParameterParameter ); + + // Declare numerical partials. + Eigen::Matrix3d testPartialWrtEarthPosition = Eigen::Matrix3d::Zero( ); + Eigen::Matrix3d testPartialWrtEarthVelocity = Eigen::Matrix3d::Zero( ); + Eigen::Matrix3d testPartialWrtSunPosition = Eigen::Matrix3d::Zero( ); + Eigen::Matrix3d testPartialWrtSunVelocity = Eigen::Matrix3d::Zero( ); + + // Declare perturbations in position for numerical partial/ + Eigen::Vector3d positionPerturbation; + positionPerturbation << 10000.0, 10000.0, 10000.0; + Eigen::Vector3d velocityPerturbation; + velocityPerturbation << 1.0, 1.0, 1.0; + + // Create state access/modification functions for bodies. + boost::function< void( basic_mathematics::Vector6d ) > earthStateSetFunction = + boost::bind( &Body::setState, earth, _1 ); + boost::function< void( basic_mathematics::Vector6d ) > sunStateSetFunction = + boost::bind( &Body::setState, sun, _1 ); + boost::function< basic_mathematics::Vector6d ( ) > earthStateGetFunction = + boost::bind( &Body::getState, earth ); + boost::function< basic_mathematics::Vector6d ( ) > sunStateGetFunction = + boost::bind( &Body::getState, sun ); + + // Calculate numerical partials. + testPartialWrtEarthPosition = calculateAccelerationWrtStatePartials( + earthStateSetFunction, gravitationalAcceleration, earth->getState( ), positionPerturbation, 0 ); + testPartialWrtEarthVelocity = calculateAccelerationWrtStatePartials( + earthStateSetFunction, gravitationalAcceleration, earth->getState( ), velocityPerturbation, 3 ); + testPartialWrtSunPosition = calculateAccelerationWrtStatePartials( + sunStateSetFunction, gravitationalAcceleration, sun->getState( ), positionPerturbation, 0 ); + testPartialWrtSunVelocity = calculateAccelerationWrtStatePartials( + sunStateSetFunction, gravitationalAcceleration, sun->getState( ), velocityPerturbation, 3 ); + Eigen::Vector3d testPartialWrtSunGravitationalParameter = calculateAccelerationWrtParameterPartials( + sunGravitationalParameterParameter, gravitationalAcceleration, 1.0E12 ); + Eigen::Vector3d testPartialWrtEarthGravitationalParameter = calculateAccelerationWrtParameterPartials( + earthGravitationalParameterParameter, gravitationalAcceleration, 1.0E12 ); + + // Compare numerical and analytical results. + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtEarthPosition, + partialWrtEarthPosition, 1.0E-9 ); + + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtEarthVelocity, + partialWrtEarthVelocity, std::numeric_limits< double >::epsilon( ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtSunPosition, + partialWrtSunPosition, 1.0E-9 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtSunVelocity, + partialWrtSunVelocity, std::numeric_limits< double >::epsilon( ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtSunGravitationalParameter, + partialWrtSunGravitationalParameter, 1.0E-6 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtEarthGravitationalParameter, + testPartialWrtEarthGravitationalParameter, 1.0E-6 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtEarthGravitationalParameter, + partialWrtSunGravitationalParameter, std::numeric_limits< double >::epsilon( ) ); +} + +BOOST_AUTO_TEST_CASE( testRadiationPressureAccelerationPartials ) +{ + // Create empty bodies, earth and sun. + boost::shared_ptr< Body > vehicle = boost::make_shared< Body >( ); + vehicle->setConstantBodyMass( 400.0 ); + boost::shared_ptr< Body > sun = boost::make_shared< Body >( ); + + NamedBodyMap bodyMap; + bodyMap[ "Vehicle" ] = vehicle; + bodyMap[ "Sun" ] = sun; + + // Load spice kernels. + std::string kernelsPath = input_output::getSpiceKernelPath( ); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de-403-masses.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de421.bsp"); + + // Set current state of sun and earth. + sun->setState( getBodyCartesianStateAtEpoch( "Sun", "SSB", "J2000", "NONE", 1.0E6 ) ); + vehicle->setState( getBodyCartesianStateAtEpoch( "Earth", "SSB", "J2000", "NONE", 1.0E6 ) ); + + // Create links to set and get state functions of bodies. + boost::function< void( basic_mathematics::Vector6d ) > sunStateSetFunction = + boost::bind( &Body::setState, sun, _1 ); + boost::function< void( basic_mathematics::Vector6d ) > vehicleStateSetFunction = + boost::bind( &Body::setState, vehicle, _1 ); + boost::function< basic_mathematics::Vector6d( ) > sunStateGetFunction = + boost::bind( &Body::getState, sun ); + boost::function< basic_mathematics::Vector6d( ) > vehicleStateGetFunction = + boost::bind( &Body::getState, vehicle ); + + // Create radiation pressure properties of vehicle + boost::shared_ptr< RadiationPressureInterface > radiationPressureInterface = + createRadiationPressureInterface( boost::make_shared< CannonBallRadiationPressureInterfaceSettings >( + "Sun", mathematical_constants::PI * 0.3 * 0.3, 1.2 ), "Vehicle", bodyMap ); + radiationPressureInterface->updateInterface( 0.0 ); + vehicle->setRadiationPressureInterface( "Sun", radiationPressureInterface ); + + // Create acceleration model. + boost::shared_ptr< CannonBallRadiationPressureAcceleration > accelerationModel = + boost::make_shared< CannonBallRadiationPressureAcceleration >( + boost::bind( &Body::getPosition, sun ), + boost::bind( &Body::getPosition, vehicle ), + boost::bind( &RadiationPressureInterface::getCurrentRadiationPressure, + radiationPressureInterface ), + boost::bind( &RadiationPressureInterface::getRadiationPressureCoefficient, radiationPressureInterface ), + boost::bind( &RadiationPressureInterface::getArea, radiationPressureInterface ), + boost::bind( &Body::getBodyMass, vehicle ) ); + + // Create partial-calculating object. + boost::shared_ptr< AccelerationPartial > accelerationPartial = + createAnalyticalAccelerationPartial( accelerationModel, std::make_pair( "Vehicle", vehicle ), + std::make_pair( "Sun", sun ), bodyMap ); + + // Create parameter object + std::string vehicleName = "Vehicle"; + boost::shared_ptr< EstimatableParameter< double > > radiationPressureCoefficient = + boost::make_shared< RadiationPressureCoefficient >( radiationPressureInterface, vehicleName ); + + // Calculate analytical partials. + accelerationPartial->update( 0.0 ); + Eigen::MatrixXd partialWrtSunPosition = Eigen::Matrix3d::Zero( ); + accelerationPartial->wrtPositionOfAcceleratingBody( partialWrtSunPosition.block( 0, 0, 3, 3 ) ); + Eigen::MatrixXd partialWrtSunVelocity = Eigen::Matrix3d::Zero( ); + accelerationPartial->wrtVelocityOfAcceleratingBody( partialWrtSunVelocity.block( 0, 0, 3, 3 ), 1, 0, 0 ); + Eigen::MatrixXd partialWrtVehiclePosition = Eigen::Matrix3d::Zero( ); + accelerationPartial->wrtPositionOfAcceleratedBody( partialWrtVehiclePosition.block( 0, 0, 3, 3 ) ); + Eigen::MatrixXd partialWrtVehicleVelocity = Eigen::Matrix3d::Zero( ); + accelerationPartial->wrtVelocityOfAcceleratedBody( partialWrtVehicleVelocity.block( 0, 0, 3, 3 ), 1, 0, 0 ); + Eigen::Vector3d partialWrtRadiationPressureCoefficient = accelerationPartial->wrtParameter( + radiationPressureCoefficient ); + + // Declare numerical partials. + Eigen::Matrix3d testPartialWrtVehiclePosition = Eigen::Matrix3d::Zero( ); + Eigen::Matrix3d testPartialWrtVehicleVelocity = Eigen::Matrix3d::Zero( ); + Eigen::Matrix3d testPartialWrtSunPosition = Eigen::Matrix3d::Zero( ); + Eigen::Matrix3d testPartialWrtSunVelocity = Eigen::Matrix3d::Zero( ); + Eigen::Vector3d testPartialWrtRadiationPressureCoefficient = Eigen::Vector3d::Zero( ); + + // Declare perturbations in position for numerical partial/ + Eigen::Vector3d positionPerturbation; + positionPerturbation << 10000.0, 10000.0, 10000.0; + Eigen::Vector3d velocityPerturbation; + velocityPerturbation << 1.0, 1.0, 1.0; + + // Calculate numerical partials. + boost::function< void( ) > updateFunction = + boost::bind( &RadiationPressureInterface::updateInterface, radiationPressureInterface, 0.0 ); + testPartialWrtSunPosition = calculateAccelerationWrtStatePartials( + sunStateSetFunction, accelerationModel, sun->getState( ), positionPerturbation, 0, updateFunction ); + testPartialWrtVehiclePosition = calculateAccelerationWrtStatePartials( + vehicleStateSetFunction, accelerationModel, vehicle->getState( ), positionPerturbation, 0, updateFunction ); + testPartialWrtSunVelocity = calculateAccelerationWrtStatePartials( + sunStateSetFunction, accelerationModel, sun->getState( ),velocityPerturbation, 3, updateFunction ); + testPartialWrtVehicleVelocity = calculateAccelerationWrtStatePartials( + vehicleStateSetFunction, accelerationModel, vehicle->getState( ), velocityPerturbation, 3, updateFunction ); + testPartialWrtRadiationPressureCoefficient = calculateAccelerationWrtParameterPartials( + radiationPressureCoefficient, accelerationModel, 1.0E-2, updateFunction ); + + + // Compare numerical and analytical results. + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtSunPosition, + partialWrtSunPosition, 1.0E-8 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtSunVelocity, + partialWrtSunVelocity, std::numeric_limits< double >::epsilon( ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtVehiclePosition, + partialWrtVehiclePosition, 1.0E-8 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtVehicleVelocity, + partialWrtVehicleVelocity, std::numeric_limits< double >::epsilon( ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtRadiationPressureCoefficient, + partialWrtRadiationPressureCoefficient, 1.0E-12 ); +} + + +BOOST_AUTO_TEST_CASE( testThirdBodyGravityPartials ) +{ + // Create empty bodies, earth and sun. + boost::shared_ptr< Body > earth = boost::make_shared< Body >( ); + boost::shared_ptr< Body > sun = boost::make_shared< Body >( ); + boost::shared_ptr< Body > moon = boost::make_shared< Body >( ); + + NamedBodyMap bodyMap; + bodyMap[ "Earth" ] = earth; + bodyMap[ "Sun" ] = sun; + bodyMap[ "Moon" ] = moon; + + // Load spice kernels. + std::string kernelsPath = input_output::getSpiceKernelPath( ); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de-403-masses.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de421.bsp"); + + // Set current state of sun and earth. + sun->setState( getBodyCartesianStateAtEpoch( "Sun", "Sun", "J2000", "NONE", 1.0E6 ) ); + earth->setState( getBodyCartesianStateAtEpoch( "Earth", "Sun", "J2000", "NONE", 1.0E6 ) ); + moon->setState( getBodyCartesianStateAtEpoch( "Moon", "Sun", "J2000", "NONE", 1.0E6 ) ); + + // Get sun gravitational parameter and set gravity field model. + double sunsGravitationalParameter = getBodyGravitationalParameter( "Sun" ); + boost::shared_ptr< GravityFieldModel > sunGravityFieldModel = + boost::make_shared< GravityFieldModel >( sunsGravitationalParameter ); + sun->setGravityFieldModel( sunGravityFieldModel ); + + double moonsGravitationalParameter = getBodyGravitationalParameter( "Moon" ); + boost::shared_ptr< GravityFieldModel > moonGravityFieldModel = + boost::make_shared< GravityFieldModel >( moonsGravitationalParameter ); + moon->setGravityFieldModel( moonGravityFieldModel ); + + double earthGravitationalParameter = getBodyGravitationalParameter( "Earth" ); + boost::shared_ptr< GravityFieldModel > earthGravityFieldModel = + boost::make_shared< GravityFieldModel >( earthGravitationalParameter ); + earth->setGravityFieldModel( earthGravityFieldModel ); + + // Create acceleration due to moon on earth. + boost::shared_ptr< ThirdBodyCentralGravityAcceleration > gravitationalAcceleration = + createThirdBodyCentralGravityAccelerationModel( + moon, sun, earth, "Moon", "Sun", "Earth" ); + + // Create central gravity partial. + boost::shared_ptr< AccelerationPartial > thirdBodyGravitationPartial = + createAnalyticalAccelerationPartial( gravitationalAcceleration, std::make_pair( "Moon", moon ), + std::make_pair( "Sun", sun ), bodyMap ); + + // Create gravitational parameter object. + boost::shared_ptr< EstimatableParameter< double > > gravitationalParameterParameter = boost::make_shared< + GravitationalParameter >( sunGravityFieldModel, "Sun" ); + boost::shared_ptr< EstimatableParameter< double > > moonGravitationalParameterParameter = boost::make_shared< + GravitationalParameter >( moonGravityFieldModel, "Moon" ); + boost::shared_ptr< EstimatableParameter< double > > earthGravitationalParameterParameter = boost::make_shared< + GravitationalParameter >( earthGravityFieldModel, "Earth" ); + + // Calculate analytical partials. + thirdBodyGravitationPartial->update( 1.0E6 ); + Eigen::MatrixXd partialWrtMoonPosition = Eigen::Matrix3d::Zero( ); + thirdBodyGravitationPartial->wrtPositionOfAcceleratedBody( partialWrtMoonPosition.block( 0, 0, 3, 3 ) ); + Eigen::MatrixXd partialWrtMoonVelocity = Eigen::Matrix3d::Zero( ); + thirdBodyGravitationPartial->wrtVelocityOfAcceleratedBody( partialWrtMoonVelocity.block( 0, 0, 3, 3 ) ); + Eigen::MatrixXd partialWrtSunPosition = Eigen::Matrix3d::Zero( ); + thirdBodyGravitationPartial->wrtPositionOfAcceleratingBody( partialWrtSunPosition.block( 0, 0, 3, 3 ) ); + Eigen::MatrixXd partialWrtSunVelocity = Eigen::Matrix3d::Zero( ); + thirdBodyGravitationPartial->wrtVelocityOfAcceleratingBody( partialWrtSunVelocity.block( 0, 0, 3, 3 ) ); + Eigen::MatrixXd partialWrtEarthPosition = Eigen::Matrix3d::Zero( ); + thirdBodyGravitationPartial->wrtPositionOfAdditionalBody( "Earth", partialWrtEarthPosition.block( 0, 0, 3, 3 ) ); + Eigen::MatrixXd partialWrtEarthVelocity = Eigen::Matrix3d::Zero( ); + thirdBodyGravitationPartial->wrtVelocityOfAdditionalBody( "Earth", partialWrtEarthVelocity.block( 0, 0, 3, 3 ) ); + + Eigen::Vector3d partialWrtSunGravitationalParameter = thirdBodyGravitationPartial->wrtParameter( + gravitationalParameterParameter ); + Eigen::Vector3d partialWrtMoonGravitationalParameter = thirdBodyGravitationPartial->wrtParameter( + moonGravitationalParameterParameter ); + Eigen::Vector3d partialWrtEarthGravitationalParameter = thirdBodyGravitationPartial->wrtParameter( + earthGravitationalParameterParameter ); + + // Declare numerical partials. + Eigen::Matrix3d testPartialWrtMoonPosition = Eigen::Matrix3d::Zero( ); + Eigen::Matrix3d testPartialWrtMoonVelocity = Eigen::Matrix3d::Zero( ); + Eigen::Matrix3d testPartialWrtSunPosition = Eigen::Matrix3d::Zero( ); + Eigen::Matrix3d testPartialWrtSunVelocity = Eigen::Matrix3d::Zero( ); + Eigen::Matrix3d testPartialWrtEarthPosition = Eigen::Matrix3d::Zero( ); + Eigen::Matrix3d testPartialWrtEarthVelocity = Eigen::Matrix3d::Zero( ); + + // Declare perturbations in position for numerical partial/ + Eigen::Vector3d positionPerturbation; + positionPerturbation << 10000.0, 10000.0, 10000.0; + Eigen::Vector3d velocityPerturbation; + velocityPerturbation << 1.0, 1.0, 1.0; + + // Create state access/modification functions for bodies. + boost::function< void( basic_mathematics::Vector6d ) > moonStateSetFunction = + boost::bind( &Body::setState, moon, _1 ); + boost::function< void( basic_mathematics::Vector6d ) > sunStateSetFunction = + boost::bind( &Body::setState, sun, _1 ); + boost::function< void( basic_mathematics::Vector6d ) > earthStateSetFunction = + boost::bind( &Body::setState, earth, _1 ); + + // Calculate numerical partials. + testPartialWrtMoonPosition = calculateAccelerationWrtStatePartials( + moonStateSetFunction, gravitationalAcceleration, moon->getState( ), positionPerturbation, 0 ); + testPartialWrtMoonVelocity = calculateAccelerationWrtStatePartials( + moonStateSetFunction, gravitationalAcceleration, moon->getState( ), velocityPerturbation, 3 ); + testPartialWrtSunPosition = calculateAccelerationWrtStatePartials( + sunStateSetFunction, gravitationalAcceleration, sun->getState( ), positionPerturbation, 0 ); + testPartialWrtSunVelocity = calculateAccelerationWrtStatePartials( + sunStateSetFunction, gravitationalAcceleration, sun->getState( ), velocityPerturbation, 3 ); + testPartialWrtEarthPosition = calculateAccelerationWrtStatePartials( + earthStateSetFunction, gravitationalAcceleration, earth->getState( ), positionPerturbation, 0 ); + testPartialWrtEarthVelocity = calculateAccelerationWrtStatePartials( + earthStateSetFunction, gravitationalAcceleration, earth->getState( ), velocityPerturbation, 3 ); + Eigen::Vector3d testPartialWrtSunGravitationalParameter = calculateAccelerationWrtParameterPartials( + gravitationalParameterParameter, gravitationalAcceleration, 1.0E16 ); + Eigen::Vector3d testPartialWrtEarthGravitationalParameter = calculateAccelerationWrtParameterPartials( + earthGravitationalParameterParameter, gravitationalAcceleration, 1.0E16 ); + Eigen::Vector3d testPartialWrtMoonGravitationalParameter = calculateAccelerationWrtParameterPartials( + moonGravitationalParameterParameter, gravitationalAcceleration, 1.0E16 ); + + // Compare numerical and analytical results. + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtMoonPosition, + partialWrtMoonPosition, 1.0E-7 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtMoonVelocity, + partialWrtMoonVelocity, std::numeric_limits< double >::epsilon( ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtSunPosition, + partialWrtSunPosition, 1.0E-5 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtSunVelocity, + partialWrtSunVelocity, std::numeric_limits< double >::epsilon( ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtEarthPosition, + partialWrtEarthPosition, 1.0E-5 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtEarthVelocity, + partialWrtEarthVelocity, std::numeric_limits< double >::epsilon( ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtSunGravitationalParameter, + partialWrtSunGravitationalParameter, 1.0E-6 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtMoonGravitationalParameter, + partialWrtMoonGravitationalParameter, std::numeric_limits< double >::epsilon( ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtEarthGravitationalParameter, + partialWrtEarthGravitationalParameter, std::numeric_limits< double >::epsilon( ) ); +} + +BOOST_AUTO_TEST_SUITE_END( ) + +} // namespace unit_tests + +} // namespace tudat + + + + diff --git a/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/UnitTests/unitTestSphericalHarmonicPartials.cpp b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/UnitTests/unitTestSphericalHarmonicPartials.cpp new file mode 100644 index 0000000000..fa373e3c79 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/UnitTests/unitTestSphericalHarmonicPartials.cpp @@ -0,0 +1,654 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#define BOOST_TEST_MAIN + +#include +#include +#include "Tudat/Basics/testMacros.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h" +#include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h" + +#include +#include +#include + +#include "Tudat/Astrodynamics/Aerodynamics/exponentialAtmosphere.h" +#include "Tudat/Astrodynamics/Gravitation/centralGravityModel.h" +#include "Tudat/External/SpiceInterface/spiceInterface.h" +#include "Tudat/InputOutput/basicInputOutput.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/gravitationalParameter.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/initialTranslationalState.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/radiationPressureCoefficient.h" +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/numericalAccelerationPartial.h" +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/sphericalHarmonicPartialFunctions.h" +#include "Tudat/SimulationSetup/EstimationSetup/createAccelerationPartials.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h" +#include "Tudat/SimulationSetup/PropagationSetup/createAccelerationModels.h" +#include "Tudat/SimulationSetup/EstimationSetup/createEstimatableParameters.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h" + +namespace tudat +{ +namespace unit_tests +{ + +using namespace tudat::gravitation; +using namespace tudat::aerodynamics; +using namespace tudat::ephemerides; +using namespace tudat::simulation_setup; +using namespace tudat::orbital_element_conversions; +using namespace tudat::unit_conversions; +using namespace tudat::orbit_determination; +using namespace tudat::acceleration_partials; +using namespace tudat::spice_interface; +using namespace tudat::orbit_determination; +using namespace tudat::estimatable_parameters; +using namespace tudat::electro_magnetism; + +BOOST_AUTO_TEST_SUITE( test_spherical_harmonic_partials ) + +BOOST_AUTO_TEST_CASE( testSphericalHarmonicPartials ) +{ + // Short-cuts. + using namespace gravitation; + + const double gravitationalParameter = 3.986004418e14; + const double planetaryRadius = 6378137.0; + + const Eigen::MatrixXd cosineCoefficients = + ( Eigen::MatrixXd( 6, 6 ) << + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + -4.841651437908150e-4, -2.066155090741760e-10, 2.439383573283130e-6, 0.0, 0.0, 0.0, + 9.571612070934730e-7, 2.030462010478640e-6, 9.047878948095281e-7, + 7.213217571215680e-7, 0.0, 0.0, 5.399658666389910e-7, -5.361573893888670e-7, + 3.505016239626490e-7, 9.908567666723210e-7, -1.885196330230330e-7, 0.0, + 6.867029137366810e-8, -6.292119230425290e-8, 6.520780431761640e-7, + -4.518471523288430e-7, -2.953287611756290e-7, 1.748117954960020e-7 + ).finished( ); + const Eigen::MatrixXd sineCoefficients = + ( Eigen::MatrixXd( 6, 6 ) << + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.384413891379790e-9, -1.400273703859340e-6, 0.0, 0.0, 0.0, + 0.0, 2.482004158568720e-7, -6.190054751776180e-7, 1.414349261929410e-6, 0.0, 0.0, + 0.0, -4.735673465180860e-7, 6.624800262758290e-7, -2.009567235674520e-7, + 3.088038821491940e-7, 0.0, 0.0, -9.436980733957690e-8, -3.233531925405220e-7, + -2.149554083060460e-7, 4.980705501023510e-8, -6.693799351801650e-7 + ).finished( ); + + Eigen::Vector3d position( 7.0e6, 8.0e6, 9.0e6 ); + Eigen::Vector3d nominalSphericalPosition = coordinate_conversions:: + convertCartesianToSpherical( position ); + nominalSphericalPosition( 1 ) = mathematical_constants::PI / 2.0 - + nominalSphericalPosition( 1 ); + + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache + = boost::make_shared< basic_mathematics::SphericalHarmonicsCache >( 6, 6 ); + sphericalHarmonicsCache->update( + nominalSphericalPosition( 0 ), std::sin( nominalSphericalPosition( 1 ) ), nominalSphericalPosition( 2 ), + planetaryRadius ); + boost::shared_ptr< basic_mathematics::LegendreCache > legendreCache = sphericalHarmonicsCache->getLegendreCache( ); + + double currentLongitude = sphericalHarmonicsCache->getCurrentLongitude( ); + double currentPolynomialArgument = legendreCache->getCurrentPolynomialParameter( ); + + Eigen::MatrixXd upPerturbedLegendrePolynomials = Eigen::MatrixXd( 6, 6 ); + Eigen::MatrixXd downPerturbedLegendrePolynomials = Eigen::MatrixXd( 6, 6 ); + + Eigen::MatrixXd upPerturbedLegendrePolynomialPartials = Eigen::MatrixXd( 6, 6 ); + Eigen::MatrixXd downPerturbedLegendrePolynomialPartials = Eigen::MatrixXd( 6, 6 ); + + Eigen::MatrixXd analyticalLegendrePolynomialPartials = Eigen::MatrixXd( 6, 6 ); + Eigen::MatrixXd analyticalLegendrePolynomialSecondPartials = Eigen::MatrixXd( 6, 6 ); + + legendreCache->setComputeSecondDerivatives( 1 ); + legendreCache->update( currentPolynomialArgument + 0.1 ); + + legendreCache->update( currentPolynomialArgument ); + + for( unsigned int i = 0; i < 6; i++ ) + { + for( unsigned int j = 0; ( j <= i && j < 6 ); j++ ) + { + analyticalLegendrePolynomialPartials( i, j ) = legendreCache->getLegendrePolynomialDerivative( i, j ); + analyticalLegendrePolynomialSecondPartials( i, j ) = legendreCache->getLegendrePolynomialSecondDerivative( i, j ); + } + + } + + double polynomialArgumentPerturbation = 1.0E-6; + { + legendreCache->update( currentPolynomialArgument + polynomialArgumentPerturbation ); + for( unsigned int i = 0; i < 6; i++ ) + { + for( unsigned int j = 0; ( j <= i && j < 6 ); j++ ) + { + upPerturbedLegendrePolynomials( i, j ) = legendreCache->getLegendrePolynomial( i, j ); + upPerturbedLegendrePolynomialPartials( i, j ) = legendreCache->getLegendrePolynomialDerivative( i, j ); + } + } + + legendreCache->update( currentPolynomialArgument - polynomialArgumentPerturbation ); + for( unsigned int i = 0; i < 6; i++ ) + { + for( unsigned int j = 0; ( j <= i && j < 6 ); j++ ) + { + downPerturbedLegendrePolynomials( i, j ) = legendreCache->getLegendrePolynomial( i, j ); + downPerturbedLegendrePolynomialPartials( i, j ) = legendreCache->getLegendrePolynomialDerivative( i, j ); + } + } + } + + Eigen::MatrixXd numericalLegendrePolynomialPartials = + ( upPerturbedLegendrePolynomials - downPerturbedLegendrePolynomials ) / + ( 2.0 * polynomialArgumentPerturbation ); + Eigen::MatrixXd numericalLegendrePolynomialSecondPartials = + ( upPerturbedLegendrePolynomialPartials - downPerturbedLegendrePolynomialPartials ) / + ( 2.0 * polynomialArgumentPerturbation ); + + for( unsigned int i = 0; i < 6; i++ ) + { + for( unsigned int j = 0; ( j <= i && j < 6 ); j++ ) + { + BOOST_CHECK_SMALL( + std::fabs( numericalLegendrePolynomialPartials( i, j ) - analyticalLegendrePolynomialPartials( i, j ) ), 1.0E-8 ); + BOOST_CHECK_SMALL( + std::fabs( numericalLegendrePolynomialSecondPartials( i, j ) - analyticalLegendrePolynomialSecondPartials( i, j ) ), 1.0E-8 ); + } + } + + + std::vector< std::vector< Eigen::Vector3d > > sphericalPotentialGradients; + + std::vector< std::vector< Eigen::Matrix3d > > upPerturbedSphericalPotentialGradients; + std::vector< std::vector< Eigen::Matrix3d > > downPerturbedSphericalPotentialGradients; + + std::vector< std::vector< Eigen::Matrix3d > > numericalSphericalPotentialHessian; + std::vector< std::vector< Eigen::Matrix3d > > analyticalSphericalPotentialHessian; + + Eigen::Matrix3d normalization; + normalization << nominalSphericalPosition( 0 ) * nominalSphericalPosition( 0 ), nominalSphericalPosition( 0 ), nominalSphericalPosition( 0 ), + nominalSphericalPosition( 0 ), 1.0 , 1.0, + nominalSphericalPosition( 0 ), 1.0, 1.0; + for( unsigned int i = 0; i < 6; i++ ) + { + std::vector< Eigen::Vector3d > singleTermPotentialGradients; + singleTermPotentialGradients.resize( 6 ); + sphericalPotentialGradients.push_back( singleTermPotentialGradients ); + + std::vector< Eigen::Matrix3d > singleTermPotentialPartials; + singleTermPotentialPartials.resize( 6 ); + upPerturbedSphericalPotentialGradients.push_back( singleTermPotentialPartials ); + downPerturbedSphericalPotentialGradients.push_back( singleTermPotentialPartials ); + + numericalSphericalPotentialHessian.push_back( singleTermPotentialPartials ); + analyticalSphericalPotentialHessian.push_back( singleTermPotentialPartials ); + + + } + + sphericalHarmonicsCache->update( position.norm( ), currentPolynomialArgument, currentLongitude, planetaryRadius ); + + for( unsigned int i = 0; i < 6; i++ ) + { + for( unsigned int j = 0; ( j <= i && j < 6 ); j++ ) + { + sphericalPotentialGradients[ i ][ j ] = basic_mathematics::computePotentialGradient( + nominalSphericalPosition, gravitationalParameter / planetaryRadius, i, j, + cosineCoefficients( i, j ), sineCoefficients( i, j ), legendreCache->getLegendrePolynomial( i, j ), + legendreCache->getLegendrePolynomialDerivative( i, j ), sphericalHarmonicsCache ); + } + } + + + Eigen::Vector3d perturbedSphericalPosition; + Eigen::Vector3d sphericalStatePerturbation; + sphericalStatePerturbation<<10.0, 1.0E-7, 1.0E-8; + + for( unsigned parameter = 0; parameter < 3; parameter++ ) + { + perturbedSphericalPosition = nominalSphericalPosition; + perturbedSphericalPosition( parameter ) += sphericalStatePerturbation( parameter ); + + sphericalHarmonicsCache->update( + perturbedSphericalPosition( 0 ), std::sin( perturbedSphericalPosition( 1 ) ), + perturbedSphericalPosition( 2 ), planetaryRadius ); + + for( unsigned int i = 0; i < 6; i++ ) + { + for( unsigned int j = 0; ( j <= i && j < 6 ); j++ ) + { + upPerturbedSphericalPotentialGradients[ i ][ j ].block( 0, parameter, 3, 1 ) = basic_mathematics::computePotentialGradient( + perturbedSphericalPosition, gravitationalParameter / planetaryRadius, i, j, + cosineCoefficients( i, j ), sineCoefficients( i, j ), legendreCache->getLegendrePolynomial( i, j ), + legendreCache->getLegendrePolynomialDerivative( i, j ), sphericalHarmonicsCache ); + } + } + + perturbedSphericalPosition = nominalSphericalPosition; + perturbedSphericalPosition( parameter ) -= sphericalStatePerturbation( parameter ); + + sphericalHarmonicsCache->update( + perturbedSphericalPosition( 0 ), std::sin( perturbedSphericalPosition( 1 ) ), + perturbedSphericalPosition( 2 ), planetaryRadius ); + + for( unsigned int i = 0; i < 6; i++ ) + { + for( unsigned int j = 0; ( j <= i && j < 6 ); j++ ) + { + downPerturbedSphericalPotentialGradients[ i ][ j ].block( 0, parameter, 3, 1 ) = basic_mathematics::computePotentialGradient( + perturbedSphericalPosition, gravitationalParameter / planetaryRadius, i, j, + cosineCoefficients( i, j ), sineCoefficients( i, j ), legendreCache->getLegendrePolynomial( i, j ), + legendreCache->getLegendrePolynomialDerivative( i, j ), sphericalHarmonicsCache ); + } + } + + for( unsigned int i = 0; i < 6; i++ ) + { + for( unsigned int j = 0; ( j <= i && j < 6 ); j++ ) + { + numericalSphericalPotentialHessian[ i ][ j ].block( 0, parameter, 3, 1 ) = + ( ( upPerturbedSphericalPotentialGradients[ i ][ j ].block( 0, parameter, 3, 1 ) - + downPerturbedSphericalPotentialGradients[ i ][ j ].block( 0, parameter, 3, 1 ) ) / + ( 2.0 * sphericalStatePerturbation( parameter ) ) ); + + } + + } + } + + for( unsigned int i = 1; i < 6; i++ ) + { + for( unsigned int j = 0; ( j <= i && j < 6 ); j++ ) + { + computePotentialSphericalHessian( + nominalSphericalPosition, gravitationalParameter / planetaryRadius, i, j, + cosineCoefficients( i, j ), sineCoefficients( i, j ), sphericalHarmonicsCache, + analyticalSphericalPotentialHessian[ i ][ j ] ); + + analyticalSphericalPotentialHessian[ i ][ j ] = analyticalSphericalPotentialHessian[ i ][ j ].cwiseProduct( + normalization ); + numericalSphericalPotentialHessian[ i ][ j ] = numericalSphericalPotentialHessian[ i ][ j ].cwiseProduct( + normalization ); + for( unsigned int k = 0; k < 3; k++ ) + { + for( unsigned int l = 0; l < 3; l++ ) + { + BOOST_CHECK_SMALL( analyticalSphericalPotentialHessian[ i ][ j ]( k, l ) - + numericalSphericalPotentialHessian[ i ][ j ]( k, l ), 1.0E-5 ); + } + + } + + } + } + + Eigen::Matrix3d cumulativeSphericalHessian = computeCumulativeSphericalHessian( + nominalSphericalPosition, planetaryRadius, gravitationalParameter, cosineCoefficients, sineCoefficients, + sphericalHarmonicsCache ); + + Eigen::Matrix3d nominalGradientTransformationMatrix = + coordinate_conversions::getSphericalToCartesianGradientMatrix( position ); + + Eigen::Vector3d upPerturbedTotalGradient; + Eigen::Vector3d downPerturbedTotalGradient; + Eigen::Vector3d perturbedCartesianPosition; + + Eigen::Matrix3d numericalTotalSphericalGradient; + + sphericalStatePerturbation( 0 ) *= 10.0; + sphericalStatePerturbation( 1 ) *= 100.0; + sphericalStatePerturbation( 2 ) *= 1000.0; + + for( unsigned parameter = 0; parameter < 3; parameter++ ) + { + perturbedSphericalPosition = nominalSphericalPosition; + perturbedSphericalPosition( parameter ) += sphericalStatePerturbation( parameter ); + + perturbedSphericalPosition( 1 ) = mathematical_constants::PI / 2.0 - perturbedSphericalPosition( 1 ); + perturbedCartesianPosition = coordinate_conversions::convertSphericalToCartesian( + perturbedSphericalPosition ); + + upPerturbedTotalGradient = + coordinate_conversions::getSphericalToCartesianGradientMatrix( perturbedCartesianPosition ).inverse( ) * + computeGeodesyNormalizedGravitationalAccelerationSum( + perturbedCartesianPosition, gravitationalParameter, planetaryRadius, cosineCoefficients, sineCoefficients, + sphericalHarmonicsCache ); + + perturbedSphericalPosition = nominalSphericalPosition; + perturbedSphericalPosition( parameter ) -= sphericalStatePerturbation( parameter ); + + perturbedSphericalPosition( 1 ) = mathematical_constants::PI / 2.0 - perturbedSphericalPosition( 1 ); + perturbedCartesianPosition = coordinate_conversions::convertSphericalToCartesian( + perturbedSphericalPosition ); + + downPerturbedTotalGradient = + coordinate_conversions::getSphericalToCartesianGradientMatrix( perturbedCartesianPosition ).inverse( ) * + computeGeodesyNormalizedGravitationalAccelerationSum( + perturbedCartesianPosition, gravitationalParameter, planetaryRadius, cosineCoefficients, sineCoefficients, + sphericalHarmonicsCache ); + + numericalTotalSphericalGradient.block( 0, parameter, 3, 1 ) = + ( upPerturbedTotalGradient - downPerturbedTotalGradient ) / ( 2.0 * sphericalStatePerturbation( parameter ) ); + + } + + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + cumulativeSphericalHessian, numericalTotalSphericalGradient, 1.0E-6 ); + + Eigen::Vector3d cartesianStatePerturbation; + cartesianStatePerturbation<<10.0, 10.0, 10.0; + + for( unsigned parameter = 0; parameter < 3; parameter++ ) + { + perturbedCartesianPosition = position; + perturbedCartesianPosition( parameter ) += cartesianStatePerturbation( parameter ); + + upPerturbedTotalGradient = + nominalGradientTransformationMatrix * + coordinate_conversions::getSphericalToCartesianGradientMatrix( perturbedCartesianPosition ).inverse( ) * + computeGeodesyNormalizedGravitationalAccelerationSum( + perturbedCartesianPosition, gravitationalParameter, planetaryRadius, cosineCoefficients, sineCoefficients, + sphericalHarmonicsCache ); + + perturbedCartesianPosition = position; + perturbedCartesianPosition( parameter ) -= cartesianStatePerturbation( parameter ); + + downPerturbedTotalGradient = + nominalGradientTransformationMatrix * + coordinate_conversions::getSphericalToCartesianGradientMatrix( perturbedCartesianPosition ).inverse( ) * + computeGeodesyNormalizedGravitationalAccelerationSum( + perturbedCartesianPosition, gravitationalParameter, planetaryRadius, cosineCoefficients, sineCoefficients, + sphericalHarmonicsCache ); + + numericalTotalSphericalGradient.block( 0, parameter, 3, 1 ) = + ( upPerturbedTotalGradient - downPerturbedTotalGradient ) / ( 2.0 * cartesianStatePerturbation( parameter ) ); + + } + + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ( nominalGradientTransformationMatrix * cumulativeSphericalHessian * nominalGradientTransformationMatrix.transpose( ) ), + numericalTotalSphericalGradient, 1.0E-6 ); + + + for( unsigned parameter = 0; parameter < 3; parameter++ ) + { + perturbedCartesianPosition = position; + perturbedCartesianPosition( parameter ) += cartesianStatePerturbation( parameter ); + + upPerturbedTotalGradient = + computeGeodesyNormalizedGravitationalAccelerationSum( + perturbedCartesianPosition, gravitationalParameter, planetaryRadius, cosineCoefficients, sineCoefficients, + sphericalHarmonicsCache ); + + perturbedCartesianPosition = position; + perturbedCartesianPosition( parameter ) -= cartesianStatePerturbation( parameter ); + + downPerturbedTotalGradient = + computeGeodesyNormalizedGravitationalAccelerationSum( + perturbedCartesianPosition, gravitationalParameter, planetaryRadius, cosineCoefficients, sineCoefficients, + sphericalHarmonicsCache ); + + numericalTotalSphericalGradient.block( 0, parameter, 3, 1 ) = + ( upPerturbedTotalGradient - downPerturbedTotalGradient ) / ( 2.0 * cartesianStatePerturbation( parameter ) ); + + } + + + Eigen::Matrix3d totalGradientCartesianPartial = + computePartialDerivativeOfBodyFixedSphericalHarmonicAcceleration( + position, planetaryRadius, gravitationalParameter, cosineCoefficients, sineCoefficients, + sphericalHarmonicsCache ); + + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + totalGradientCartesianPartial, numericalTotalSphericalGradient, 1.0E-6 ); +} + +BOOST_AUTO_TEST_CASE( testSphericalHarmonicAccelerationpartial ) +{ + //Load spice kernels. + std::string kernelsPath = input_output::getSpiceKernelPath( ); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de-403-masses.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de421.bsp"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "naif0009.tls"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "pck00009.tpc"); + + // Create empty bodies, earth and vehicle. + boost::shared_ptr< Body > earth = boost::make_shared< Body >( ); + boost::shared_ptr< Body > vehicle = boost::make_shared< Body >( ); + + const double gravitationalParameter = 3.986004418e14; + const double planetaryRadius = 6378137.0; + + Eigen::MatrixXd cosineCoefficients = + ( Eigen::MatrixXd( 6, 6 ) << + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + -4.841651437908150e-4, -2.066155090741760e-10, 2.439383573283130e-6, 0.0, 0.0, 0.0, + 9.571612070934730e-7, 2.030462010478640e-6, 9.047878948095281e-7, + 7.213217571215680e-7, 0.0, 0.0, 5.399658666389910e-7, -5.361573893888670e-7, + 3.505016239626490e-7, 9.908567666723210e-7, -1.885196330230330e-7, 0.0, + 6.867029137366810e-8, -6.292119230425290e-8, 6.520780431761640e-7, + -4.518471523288430e-7, -2.953287611756290e-7, 1.748117954960020e-7 + ).finished( ); + + + Eigen::MatrixXd sineCoefficients = + ( Eigen::MatrixXd( 6, 6 ) << + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.384413891379790e-9, -1.400273703859340e-6, 0.0, 0.0, 0.0, + 0.0, 2.482004158568720e-7, -6.190054751776180e-7, 1.414349261929410e-6, 0.0, 0.0, + 0.0, -4.735673465180860e-7, 6.624800262758290e-7, -2.009567235674520e-7, + 3.088038821491940e-7, 0.0, 0.0, -9.436980733957690e-8, -3.233531925405220e-7, + -2.149554083060460e-7, 4.980705501023510e-8, -6.693799351801650e-7 + ).finished( ); + + NamedBodyMap bodyMap; + bodyMap[ "Earth" ] = earth; + bodyMap[ "Vehicle" ] = vehicle; + + boost::shared_ptr< gravitation::SphericalHarmonicsGravityField > earthGravityField = + boost::make_shared< gravitation::SphericalHarmonicsGravityField >( + gravitationalParameter, planetaryRadius, cosineCoefficients, sineCoefficients, "IAU_Earth" ); + + earth->setGravityFieldModel( earthGravityField ); + + boost::shared_ptr< ephemerides::SimpleRotationalEphemeris > simpleRotationalEphemeris = + boost::make_shared< ephemerides::SimpleRotationalEphemeris >( + spice_interface::computeRotationQuaternionBetweenFrames( "ECLIPJ2000" , "IAU_Earth", 0.0 ), + 2.0 * mathematical_constants::PI / 86400.0, + 1.0E7, basic_astrodynamics::JULIAN_DAY_ON_J2000, + "ECLIPJ2000" , "IAU_Earth" ); + + earth->setRotationalEphemeris( simpleRotationalEphemeris ); + + // Set current state of vehicle and earth. + earth->setState( basic_mathematics::Vector6d::Zero( ) ); + + // Set Keplerian elements for Asterix. + basic_mathematics::Vector6d asterixInitialStateInKeplerianElements; + asterixInitialStateInKeplerianElements( semiMajorAxisIndex ) = 7500.0E3; + asterixInitialStateInKeplerianElements( eccentricityIndex ) = 0.1; + asterixInitialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 85.3 ); + asterixInitialStateInKeplerianElements( argumentOfPeriapsisIndex ) + = unit_conversions::convertDegreesToRadians( 235.7 ); + asterixInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex ) + = unit_conversions::convertDegreesToRadians( 23.4 ); + asterixInitialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 139.87 ); + + basic_mathematics::Vector6d asterixInitialState = orbital_element_conversions::convertKeplerianToCartesianElements( + asterixInitialStateInKeplerianElements, gravitationalParameter ); + + + vehicle->setState( asterixInitialState ); + + + + // Create acceleration due to vehicle on earth. + boost::shared_ptr< SphericalHarmonicAccelerationSettings > accelerationSettings = + boost::make_shared< SphericalHarmonicAccelerationSettings >( 5, 5 ); + boost::shared_ptr< SphericalHarmonicsGravitationalAccelerationModel > gravitationalAcceleration = + boost::dynamic_pointer_cast< SphericalHarmonicsGravitationalAccelerationModel >( + createAccelerationModel( vehicle, earth, accelerationSettings, "Vehicle", "Earth" ) ); + + gravitationalAcceleration->updateMembers( 0.0 ); + gravitationalAcceleration->getAcceleration( ); + + // Declare numerical partials. + Eigen::Matrix3d testPartialWrtEarthPosition = Eigen::Matrix3d::Zero( ); + Eigen::Matrix3d testPartialWrtEarthVelocity = Eigen::Matrix3d::Zero( ); + Eigen::Matrix3d testPartialWrtVehiclePosition = Eigen::Matrix3d::Zero( ); + Eigen::Matrix3d testPartialWrtVehicleVelocity = Eigen::Matrix3d::Zero( ); + + // Declare perturbations in position for numerical partial/ + Eigen::Vector3d positionPerturbation; + positionPerturbation<<10.0, 10.0, 10.0; + Eigen::Vector3d velocityPerturbation; + velocityPerturbation<< 1.0E-3, 1.0E-3, 1.0E-3; + + // Create state access/modification functions for bodies. + boost::function< void( basic_mathematics::Vector6d ) > earthStateSetFunction = + boost::bind( &Body::setState, earth, _1 ); + boost::function< void( basic_mathematics::Vector6d ) > vehicleStateSetFunction = + boost::bind( &Body::setState, vehicle, _1 ); + boost::function< basic_mathematics::Vector6d ( ) > earthStateGetFunction = + boost::bind( &Body::getState, earth ); + boost::function< basic_mathematics::Vector6d ( ) > vehicleStateGetFunction = + boost::bind( &Body::getState, vehicle ); + + + + std::vector< boost::shared_ptr< EstimatableParameterSettings > > parameterNames; + parameterNames.push_back( boost::make_shared< EstimatableParameterSettings >( "Earth", gravitational_parameter) ); + parameterNames.push_back( boost::make_shared< EstimatableParameterSettings >( "Earth", constant_rotation_rate ) ); + parameterNames.push_back( boost::make_shared< EstimatableParameterSettings >( "Earth", rotation_pole_position ) ); + + parameterNames.push_back( boost::make_shared< SphericalHarmonicEstimatableParameterSettings >( + 2, 0, 5, 4, "Earth", spherical_harmonics_cosine_coefficient_block ) ); + parameterNames.push_back( boost::make_shared< SphericalHarmonicEstimatableParameterSettings >( + 2, 1, 5, 4, "Earth", spherical_harmonics_sine_coefficient_block ) ); + + boost::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parameterSet = + createParametersToEstimate( parameterNames, bodyMap ); + + // Create acceleration partial object. + boost::shared_ptr< SphericalHarmonicsGravityPartial > accelerationPartial = + boost::dynamic_pointer_cast< SphericalHarmonicsGravityPartial > ( + createAnalyticalAccelerationPartial( + gravitationalAcceleration, std::make_pair( "Vehicle", vehicle ), std::make_pair( "Earth", earth ), + bodyMap, parameterSet ) ); + + double testTime = 1.0E6; + earth->setCurrentRotationToLocalFrameFromEphemeris( testTime ); + + accelerationPartial->update( testTime ); + + Eigen::MatrixXd partialWrtVehiclePosition = Eigen::Matrix3d::Zero( ); + accelerationPartial->wrtPositionOfAcceleratedBody( partialWrtVehiclePosition.block( 0, 0, 3, 3 ) ); + Eigen::MatrixXd partialWrtVehicleVelocity = Eigen::Matrix3d::Zero( ); + accelerationPartial->wrtVelocityOfAcceleratedBody( partialWrtVehicleVelocity.block( 0, 0, 3, 3 ), 1, 0, 0 ); + Eigen::MatrixXd partialWrtEarthPosition = Eigen::Matrix3d::Zero( ); + accelerationPartial->wrtPositionOfAcceleratingBody( partialWrtEarthPosition.block( 0, 0, 3, 3 ) ); + Eigen::MatrixXd partialWrtEarthVelocity = Eigen::Matrix3d::Zero( ); + accelerationPartial->wrtVelocityOfAcceleratingBody( partialWrtEarthVelocity.block( 0, 0, 3, 3 ), 1, 0, 0 ); + + + + + + // Calculate numerical partials. + testPartialWrtVehiclePosition = calculateAccelerationWrtStatePartials( + vehicleStateSetFunction, gravitationalAcceleration, vehicle->getState( ), positionPerturbation, 0 ); + testPartialWrtVehicleVelocity = calculateAccelerationWrtStatePartials( + vehicleStateSetFunction, gravitationalAcceleration, vehicle->getState( ), velocityPerturbation, 3 ); + + testPartialWrtEarthPosition = calculateAccelerationWrtStatePartials( + earthStateSetFunction, gravitationalAcceleration, earth->getState( ), positionPerturbation, 0 ); + testPartialWrtEarthVelocity = calculateAccelerationWrtStatePartials( + earthStateSetFunction, gravitationalAcceleration, earth->getState( ), velocityPerturbation, 3 ); + + + + // Calculate numerical partials. + std::map< int, boost::shared_ptr< EstimatableParameter< double > > > doubleParameters = + parameterSet->getDoubleParameters( ); + std::map< int, boost::shared_ptr< EstimatableParameter< double > > >::iterator doubleParametersIterator = + doubleParameters.begin( ); + + Eigen::Vector3d testPartialWrtEarthGravitationalParameter = calculateAccelerationWrtParameterPartials( + doubleParametersIterator->second, gravitationalAcceleration, 1.0E12 ); + Eigen::Vector3d partialWrtEarthGravitationalParameter = accelerationPartial->wrtParameter( + doubleParametersIterator->second ); + doubleParametersIterator++; + + Eigen::Vector3d partialWrtEarthRotationRate = accelerationPartial->wrtParameter( + doubleParametersIterator->second ); + Eigen::Vector3d testPartialWrtEarthRotationRate = calculateAccelerationWrtParameterPartials( + doubleParametersIterator->second, gravitationalAcceleration, 1.0E-12, &emptyFunction, testTime, boost::bind( + &Body::setCurrentRotationToLocalFrameFromEphemeris, earth, _1 ) ); + + + + + std::map< int, boost::shared_ptr< EstimatableParameter< Eigen::VectorXd > > > vectorParameters = + parameterSet->getVectorParameters( ); + std::map< int, boost::shared_ptr< EstimatableParameter< Eigen::VectorXd > > >::iterator vectorParametersIterator = + vectorParameters.begin( ); + Eigen::MatrixXd partialWrtPolePosition = accelerationPartial->wrtParameter( + vectorParametersIterator->second ); + Eigen::MatrixXd testPartialWrtPosition = calculateAccelerationWrtParameterPartials( + vectorParametersIterator->second, gravitationalAcceleration, Eigen::Vector2d::Constant( 1.0E-6 ), + &emptyFunction, testTime, boost::bind( + &Body::setCurrentRotationToLocalFrameFromEphemeris, earth, _1 ) ); + vectorParametersIterator++; + + Eigen::MatrixXd partialWrtCosineCoefficients = accelerationPartial->wrtParameter( + vectorParametersIterator->second ); + Eigen::MatrixXd testPartialWrtCosineCoefficients = calculateAccelerationWrtParameterPartials( + vectorParametersIterator->second, gravitationalAcceleration, vectorParametersIterator->second->getParameterValue( ) * 1.0E-2 ); + vectorParametersIterator++; + + Eigen::MatrixXd partialWrtSineCoefficients = accelerationPartial->wrtParameter( + vectorParametersIterator->second ); + Eigen::MatrixXd testPartialWrtSineCoefficients = calculateAccelerationWrtParameterPartials( + vectorParametersIterator->second, gravitationalAcceleration, vectorParametersIterator->second->getParameterValue( ) * 1.0E-2 ); + + + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtVehiclePosition, partialWrtVehiclePosition, 1.0E-6 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtVehicleVelocity, partialWrtVehicleVelocity, 1.0E-6 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtEarthPosition, partialWrtEarthPosition, 1.0E-6 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtEarthVelocity, partialWrtEarthVelocity, 1.0E-6 ); + + + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtEarthGravitationalParameter, partialWrtEarthGravitationalParameter, 1.0E-12 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtEarthRotationRate, partialWrtEarthRotationRate, 1.0E-6 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtPosition, partialWrtPolePosition, 1.0E-6 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtCosineCoefficients, partialWrtCosineCoefficients, 1.0E-6 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtSineCoefficients, partialWrtSineCoefficients, 1.0E-6 ); + + BOOST_CHECK_EQUAL( testPartialWrtCosineCoefficients.cols( ), 17 ); + BOOST_CHECK_EQUAL( testPartialWrtSineCoefficients.cols( ), 13 ); + +} + +BOOST_AUTO_TEST_SUITE_END( ) + +} // namespace unit_tests + +} // namespace tudat + + + + + diff --git a/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/accelerationPartial.cpp b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/accelerationPartial.cpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/accelerationPartial.h b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/accelerationPartial.h new file mode 100644 index 0000000000..9a3a88115b --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/accelerationPartial.h @@ -0,0 +1,375 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_ACCELERATIONPARTIALS_H +#define TUDAT_ACCELERATIONPARTIALS_H + +#include +#include +#include + +#include +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" + +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h" +#include "Tudat/Astrodynamics/OrbitDetermination/stateDerivativePartial.h" + +namespace tudat +{ + +namespace acceleration_partials +{ + +//! Base class for objects calculating partial derivatives of accelerations w.r.t. states, model parameters. +/*! + * Base class for objects calculating partial derivatives of accelerations w.r.t. states, model parameters. Such + * calculations are used in orbit determination, for the computation of the state transition; sensitivity matrices. + * Derived classes implement derivative-calculating models for specific acceleration models, so that the calculation + * of all partials of a single type acceleration model is encompassed in a single derived class. + */ +class AccelerationPartial: public orbit_determination::StateDerivativePartial +{ + +public: + //! Base class constructor. + /*! + * Constructor of base class, sets the base class member variables identifying the body undergoing and exerting the + * acceleration. + * \param acceleratedBody Body undergoing acceleration. + * \param acceleratingBody Body exerting acceleration. + * \param accelerationType Type of acceleration w.r.t. which partial is taken. + */ + AccelerationPartial( const std::string& acceleratedBody, const std::string& acceleratingBody, + const basic_astrodynamics::AvailableAcceleration accelerationType ): + StateDerivativePartial( propagators::transational_state, std::make_pair( acceleratedBody, "" ) ), + acceleratedBody_( acceleratedBody ), acceleratingBody_( acceleratingBody ),accelerationType_( accelerationType ) { } + + //! Virtual destructor. + virtual ~AccelerationPartial( ) { } + + //! Function to retrieve the function that returns the partial derivative w.r.t. a propagated state. + /*! + * Function to retrieve the function that returns the partial derivative w.r.t. a propagated state. + * \param stateReferencePoint Reference point (id) for propagated state (i.e. body name for translational dynamics). + * \param integratedStateType Type of propagated state. + * \return Pair with function, returning partial derivative, and number of columns in partial vector, + */ + std::pair< boost::function< void( Eigen::Block< Eigen::MatrixXd > ) >, int > + getDerivativeFunctionWrtStateOfIntegratedBody( + const std::pair< std::string, std::string >& stateReferencePoint, + const propagators::IntegratedStateType integratedStateType ) + { + // Initialize to empty function; 0 parameter size. + std::pair< boost::function< void( Eigen::Block< Eigen::MatrixXd > ) >, int > + partialFunction = std::make_pair( boost::function< void( Eigen::Block< Eigen::MatrixXd > ) >( ), 0 ); + + // Check if state dependency exists + switch( integratedStateType ) + { + case propagators::transational_state: + { + // Check if reference id is consistent. + if( stateReferencePoint.second != "" ) + { + throw std::runtime_error( "Error when getting state derivative partial acceleration model, cannot have reference point on body for dynamics" ); + } + // Check if propagated body corresponds to accelerated, accelerating, ro relevant third body. + else if( stateReferencePoint.first == acceleratedBody_ ) + { + partialFunction = std::make_pair( boost::bind( &AccelerationPartial::wrtStateOfAcceleratedBody, this, _1 ), 3 ); + } + else if( stateReferencePoint.first == acceleratingBody_ ) + { + partialFunction = std::make_pair( boost::bind( &AccelerationPartial::wrtStateOfAcceleratingBody, this, _1 ), 3 ); + } + else if( isAccelerationPartialWrtAdditionalBodyNonNull( stateReferencePoint.first ) ) + { + partialFunction = std::make_pair( boost::bind( &AccelerationPartial::wrtStateOfAdditionalBody, + this, _1, stateReferencePoint.first ), 3 ); + } + break; + } + case propagators::body_mass_state: + { + // Check if reference id is consistent. + if( stateReferencePoint.second != "" ) + { + throw std::runtime_error( "Error when getting state derivative partial acceleration model, cannot have reference point on body for body mass" ); + } + else if( isStateDerivativeDependentOnIntegratedNonTranslationalState( stateReferencePoint, integratedStateType ) ) + { + partialFunction = std::make_pair( boost::bind( &AccelerationPartial::wrtNonTranslationalStateOfAdditionalBody, + this, _1, stateReferencePoint, integratedStateType ), 1 ); + } + } + default: + std::string errorMessage = + "Error when getting state derivative partial acceleration model, dynamics type " + + boost::lexical_cast< std::string >( integratedStateType ) + "not recognized" ; + throw std::runtime_error( errorMessage ); + break; + } + + + return partialFunction; + } + + //! Function for determining if the acceleration is dependent on a non-translational integrated state (default none). + /*! + * Function for determining if the acceleration is dependent on a non-translational integrated state. + * No dependency is implemented is returned in this base class function, but may be overriden by derived class. + * \param stateReferencePoint Reference point id of propagated state + * \param integratedStateType Type of propagated state for which dependency is to be determined. + * \return True if dependency exists (non-zero partial), false otherwise. + */ + virtual bool isStateDerivativeDependentOnIntegratedNonTranslationalState( + const std::pair< std::string, std::string >& stateReferencePoint, + const propagators::IntegratedStateType integratedStateType ) + { + return false; + } + + //! Function to check whether a partial w.r.t. some integrated state is non-zero. + /*! + * Function to check whether a partial w.r.t. some integrated state is non-zero. + * \param stateReferencePoint Reference point (id) for propagated state (i.e. body name for translational dynamics). + * \param integratedStateType Type of propagated state. + * \return True if dependency exists, false otherwise. + */ + bool isStateDerivativeDependentOnIntegratedState( + const std::pair< std::string, std::string >& stateReferencePoint, + const propagators::IntegratedStateType integratedStateType ) + { + bool isDependent = 0; + + // Check if state is translational. + switch( integratedStateType ) + { + case propagators::transational_state: + { + // Check if reference id is consistent. + if( stateReferencePoint.second != "" ) + { + throw std::runtime_error( "Error when checking state derivative partial dependency of acceleration model, cannot have reference point on body for dynamics" ); + } + + // Check if propagated body corresponds to accelerated, accelerating, ro relevant third body. + else if( stateReferencePoint.first == acceleratedBody_ || stateReferencePoint.first == acceleratingBody_ || + isAccelerationPartialWrtAdditionalBodyNonNull( stateReferencePoint.first ) ) + { + isDependent = 1; + } + break; + } + default: + isDependent = isStateDerivativeDependentOnIntegratedNonTranslationalState( stateReferencePoint, integratedStateType ); + break; + } + return isDependent; + } + + //! Pure virtual function for calculating the partial of the acceleration w.r.t. the position of the accelerated body. + /*! + * Pure virtual function for calculating the partial of the acceleration w.r.t. the position of the accelerated body and + * adding it to the existing partial block. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian position of body + * undergoing acceleration where current partial is to be added. + * \param addContribution Variable denoting whether to return the partial itself (true) or the negative partial (false). + * \param startRow First row in partialMatrix block where the computed partial is to be added. + * \param startColumn First column in partialMatrix block where the computed partial is to be added. + */ + virtual void wrtPositionOfAcceleratedBody( + Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 0 ) = 0; + + //! Function for calculating the partial of the acceleration w.r.t. the velocity of the accelerated body. + /*! + * Function for calculating the partial of the acceleration w.r.t. the velocity of the accelerated body and + * adding it to the existing partial block. Function may be overridden in derived class, default dependency is none. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian velocity of body + * undergoing acceleration where current partial is to be added. + * \param addContribution Variable denoting whether to return the partial itself (true) or the negative partial (false). + * \param startRow First row in partialMatrix block where the computed partial is to be added. + * \param startColumn First column in partialMatrix block where the computed partial is to be added. + */ + virtual void wrtVelocityOfAcceleratedBody( + Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 3 ) + { } + + //! Function for calculating the partial of the acceleration w.r.t. the Cartesian state of the body undergoing acceleration. + /*! + * Function for calculating the partial of the acceleration w.r.t. the Cartesian state of the body + * undergoing acceleration and adding it to the existing partial block. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian state of body + * undergoing acceleration where current partial is to be added. + */ + void wrtStateOfAcceleratedBody( Eigen::Block< Eigen::MatrixXd > partialMatrix ) + { + wrtPositionOfAcceleratedBody( partialMatrix, true, 0, 0 ); + wrtVelocityOfAcceleratedBody( partialMatrix, true, 0, 3 ); + } + + //! Pure virtual function for calculating the partial of the acceleration w.r.t. the position of the body exerting acceleration. + /*! + * Pure virtual function for calculating the partial of the acceleration w.r.t. the position of the body exerting + * acceleration and adding it to the existing partial block. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian position of body + * exerting acceleration where current partial is to be added. + * \param addContribution Variable denoting whether to return the partial itself (true) or the negative partial (false). + * \param startRow First row in partialMatrix block where the computed partial is to be added. + * \param startColumn First column in partialMatrix block where the computed partial is to be added. + */ + virtual void wrtPositionOfAcceleratingBody( + Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 0 ) = 0; + + //! Function for calculating the partial of the acceleration w.r.t. the velocity of the body exerting acceleration. + /*! + * Function for calculating the partial of the acceleration w.r.t. the velocity of the body exerting a + * acceleration. . Function may be overridden in derived class, default dependency is none. + * \param addContribution Variable denoting whether to return the partial itself (true) or the negative partial (false). + * \param startRow First row in partialMatrix block where the computed partial is to be added. + * \param startColumn First column in partialMatrix block where the computed partial is to be added. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian velocity of body + * exerting acceleration where current partial is to be added. + */ + virtual void wrtVelocityOfAcceleratingBody( + Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 3 ){ } + + //! Function for calculating the partial of the acceleration w.r.t. the Cartesian state of the body exerting acceleration. + /*! + * Function for calculating the partial of the acceleration w.r.t. the Cartesian state of the body exerting + * acceleration and adding it to the existing partial block. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian state of body + * exerting acceleration where current partial is to be added. + */ + void wrtStateOfAcceleratingBody( Eigen::Block< Eigen::MatrixXd > partialMatrix ) + { + wrtPositionOfAcceleratingBody( partialMatrix, true, 0, 0 ); + wrtVelocityOfAcceleratingBody( partialMatrix, true, 0, 3 ); + } + + //! Function for calculating the partial of the acceleration w.r.t. the position of the third body. + /*! + * Function for calculating the partial of the acceleration w.r.t. the position of the third body + * and adding it to the existing partial block. Function may be overridden in derived class, default dependency is none. + * \param bodyName Name of third body. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian position of third body where + * current partial is to be added. + * \param addContribution Variable denoting whether to return the partial itself (true) or the negative partial (false). + * \param startRow First row in partialMatrix block where the computed partial is to be added. + * \param startColumn First column in partialMatrix block where the computed partial is to be added. + */ + virtual void wrtPositionOfAdditionalBody( + const std::string& bodyName, Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 0 ){ } + + //! Function for calculating the partial of the acceleration w.r.t. the velocity of the third body. + /*! + * Function for calculating the partial of the acceleration w.r.t. the velocity of the third body + * and adding it to the existing partial block. . Function may be overridden in derived class, default dependency is + * none. + * \param bodyName Name of third body. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian velocity of third body where + * current partial is to be added. + * \param addContribution Variable denoting whether to return the partial itself (true) or the negative partial (false). + * \param startRow First row in partialMatrix block where the computed partial is to be added. + * \param startColumn First column in partialMatrix block where the computed partial is to be added. + */ + virtual void wrtVelocityOfAdditionalBody( + const std::string& bodyName, Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 3 ){ } + + //! Function for calculating the partial of the acceleration w.r.t. the Cartesian state of the third body. + /*! + * Function for calculating the partial of the acceleration w.r.t. the Cartesian state of the third body + * and adding it to the existing partial block. + * \param bodyName Name of third body. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian state of third body where + * current partial is to be added. + */ + void wrtStateOfAdditionalBody( Eigen::Block< Eigen::MatrixXd > partialMatrix, const std::string& bodyName ) + { + wrtPositionOfAdditionalBody( bodyName, partialMatrix, true, 0, 0 ); + wrtVelocityOfAdditionalBody( bodyName, partialMatrix, true, 0, 3 ); + } + + //! Function for calculating the partial of the acceleration w.r.t. a non-translational integrated state + /*! + * Function for calculating the partial of the acceleration w.r.t. a non-translational integrated state + * and adding it to the existing partial block. Function may be overridden in derived class, default dependency is + * none. + * \param partialMatrix Block of partial derivatives of where current partial is to be added. + * \param stateReferencePoint Reference point id of propagated state + * \param integratedStateType Type of propagated state for which partial is to be computed. + */ + virtual void wrtNonTranslationalStateOfAdditionalBody( + Eigen::Block< Eigen::MatrixXd > partialMatrix, + const std::pair< std::string, std::string >& stateReferencePoint, + const propagators::IntegratedStateType integratedStateType ){ } + + //! Function to check whether the partial derivative w.r.t. the translational state of a third body is non-zero. + /*! + * Function to check whether the partial derivative w.r.t. the translational state of a third body is non-zero. This + * function returns false by default, should be redefined in derived class if any third-bodyd dependencies exist. + * \param bodyName Name of third body. + * \return True if third body dependency exists, false otherwise. + */ + virtual bool isAccelerationPartialWrtAdditionalBodyNonNull( const std::string& bodyName ) + { + return 0; + } + + //! Function to retrieve the name of the body undergoing acceleration. + /*! + * Function to retrieve the name of the body undergoing acceleration. + * \return Name of the body undergoing acceleration. + */ + std::string getAcceleratedBody( ) { return acceleratedBody_; } + + //! Function to retrieve the name of the body exerting acceleration. + /*! + * Function to retrieve the name of the body exerting acceleration. + * \return Name of the body exerting acceleration. + */ + std::string getAcceleratingBody( ) { return acceleratingBody_; } + + //! Function to retrieve the type of acceleration w.r.t. which partial is taken.. + /*! + * Function to retrieve the type of acceleration w.r.t. which partial is taken.. + * \return Type of acceleration w.r.t. which partial is taken.. + */ + basic_astrodynamics::AvailableAcceleration getAccelerationType( ) + { + return accelerationType_; + } + +protected: + //! Name of the body undergoing acceleration. + std::string acceleratedBody_; + + //! Name of the body exerting acceleration. + std::string acceleratingBody_; + + //! Type of acceleration w.r.t. which partial is taken.. + basic_astrodynamics::AvailableAcceleration accelerationType_; +}; + + +} // namespace acceleration_partials + +} // namespace tudat + +#endif // TUDAT_ACCELERATIONPARTIALS_H diff --git a/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/centralGravityAccelerationPartial.cpp b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/centralGravityAccelerationPartial.cpp new file mode 100644 index 0000000000..e80e0b43fd --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/centralGravityAccelerationPartial.cpp @@ -0,0 +1,144 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/centralGravityAccelerationPartial.h" + + +namespace tudat +{ + +namespace acceleration_partials +{ + +//! Calculates partial derivative of point mass gravitational acceleration wrt the position of body undergoing acceleration. +Eigen::Matrix3d calculatePartialOfPointMassGravityWrtPositionOfAcceleratedBody( + const Eigen::Vector3d& acceleratedBodyPosition, + const Eigen::Vector3d& acceleratingBodyPositions, + double gravitationalParameter ) +{ + // Calculate relative position + Eigen::Vector3d relativePosition = acceleratedBodyPosition - acceleratingBodyPositions; + + // Calculate partial (Montenbruck & Gill, Eq. 7.56) + double relativePositionNorm = relativePosition.norm( ); + double invSquareOfPositionNorm = 1.0 / ( relativePositionNorm * relativePositionNorm ); + double invCubeOfPositionNorm = invSquareOfPositionNorm / relativePositionNorm; + Eigen::Matrix3d partialMatrix = -gravitationalParameter * + ( Eigen::Matrix3d::Identity( ) * invCubeOfPositionNorm - + ( 3.0 * invSquareOfPositionNorm * invCubeOfPositionNorm ) * relativePosition * relativePosition.transpose( ) ); + + return partialMatrix; +} + +//! Calculates partial derivative of point mass gravitational acceleration wrt gravitational parameter of the central body. +Eigen::Vector3d computePartialOfCentralGravityWrtGravitationalParameter( const Eigen::Vector3d& acceleratedBodyPosition, + const Eigen::Vector3d& acceleratingBodyPosition) +{ + // Calculate relative position + Eigen::Vector3d relativePosition = acceleratingBodyPosition - acceleratedBodyPosition; + + // Calculate partial (Montenbruck & Gill, Eq. 7.76) + double positionNorm = relativePosition.norm( ); + Eigen::Vector3d partialMatrix = relativePosition / ( positionNorm * positionNorm * positionNorm ); + return partialMatrix; +} + +//! Calculates partial derivative of point mass gravitational acceleration wrt gravitational parameter of the central body. +Eigen::Vector3d computePartialOfCentralGravityWrtGravitationalParameter( const Eigen::Vector3d& gravitationalAcceleration, + const double gravitationalParameter ) +{ + return gravitationalAcceleration / gravitationalParameter; +} + +//! Constructor +CentralGravitationPartial::CentralGravitationPartial( + const boost::shared_ptr< gravitation::CentralGravitationalAccelerationModel3d > gravitationalAcceleration, + const std::string acceleratedBody, + const std::string acceleratingBody ): + AccelerationPartial( acceleratedBody, acceleratingBody, basic_astrodynamics::central_gravity ) +{ + accelerationUpdateFunction_ = + boost::bind( &basic_astrodynamics::AccelerationModel< Eigen::Vector3d>::updateMembers, gravitationalAcceleration, _1 ); + + gravitationalParameterFunction_ = gravitationalAcceleration->getGravitationalParameterFunction( ); + centralBodyState_ = gravitationalAcceleration->getStateFunctionOfBodyExertingAcceleration( ); + acceleratedBodyState_ = gravitationalAcceleration->getStateFunctionOfBodyUndergoingAcceleration( ); + accelerationUsesMutualAttraction_ = gravitationalAcceleration->getIsMutualAttractionUsed( ); +} + +//! Function for setting up and retrieving a function returning a partial w.r.t. a double parameter. +std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > +CentralGravitationPartial::getParameterPartialFunction( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameter ) + +{ + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > partialFunctionPair; + + // Check dependencies. + if( parameter->getParameterName( ).first == estimatable_parameters::gravitational_parameter ) + { + // If parameter is gravitational parameter, check and create dependency function . + partialFunctionPair = getGravitationalParameterPartialFunction( parameter->getParameterName( ) ); + } + else + { + partialFunctionPair = std::make_pair( boost::function< void( Eigen::MatrixXd& ) >( ), 0 ); + } + + return partialFunctionPair; +} + +//! Function to create a function returning the current partial w.r.t. a gravitational parameter. +std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > +CentralGravitationPartial::getGravitationalParameterPartialFunction( + const estimatable_parameters::EstimatebleParameterIdentifier& parameterId ) +{ + boost::function< void( Eigen::MatrixXd& ) > partialFunction; + int numberOfColumns = 0; + + // Check if parameter is gravitational parameter. + if( parameterId.first == estimatable_parameters::gravitational_parameter ) + { + // Check if parameter body is central body. + if( parameterId.second.first == acceleratingBody_ ) + { + partialFunction = boost::bind( &CentralGravitationPartial::wrtGravitationalParameterOfCentralBody, + this, _1 ); + numberOfColumns = 1; + + } + + // Check if parameter body is accelerated body, and if the mutual acceleration is used. + if( parameterId.second.first == acceleratedBody_ ) + { + if( accelerationUsesMutualAttraction_ ) + { + partialFunction = boost::bind( &CentralGravitationPartial::wrtGravitationalParameterOfCentralBody, + this, _1 ); + numberOfColumns = 1; + } + } + } + + return std::make_pair( partialFunction, numberOfColumns ); +} + +//! Function to calculate central gravity partial w.r.t. central body gravitational parameter +void CentralGravitationPartial::wrtGravitationalParameterOfCentralBody( Eigen::MatrixXd& gravitationalParameterPartial ) +{ + gravitationalParameterPartial = computePartialOfCentralGravityWrtGravitationalParameter( + currentAcceleratedBodyState_, currentCentralBodyState_ ); +} + + + +} + +} diff --git a/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/centralGravityAccelerationPartial.h b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/centralGravityAccelerationPartial.h new file mode 100644 index 0000000000..df9cdbc4d9 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/centralGravityAccelerationPartial.h @@ -0,0 +1,250 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_CENTRALGRAVITYACCELERATIONPARTIALS_H +#define TUDAT_CENTRALGRAVITYACCELERATIONPARTIALS_H + +#include "Tudat/Astrodynamics/Gravitation/centralGravityModel.h" + +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/accelerationPartial.h" + +namespace tudat +{ + +namespace acceleration_partials +{ + +//! Calculates partial derivative of point mass gravitational acceleration wrt the position of body undergoing acceleration. +/*! + * Calculates partial derivative of point mass gravitational acceleration wrt the position of body undergoing acceleration. + * \param acceleratedBodyPosition Cartesian state of body being accelerated. + * \param acceleratingBodyPosition Cartesian state of body exerting acceleration. + * \param gravitationalParameter Gravitational parameter of gravitating body. + * \return Matrix with the Jacobian of the acceleration vector w.r.t. the position vector. + */ +Eigen::Matrix3d calculatePartialOfPointMassGravityWrtPositionOfAcceleratedBody( + const Eigen::Vector3d& acceleratedBodyPosition, + const Eigen::Vector3d& acceleratingBodyPosition, + const double gravitationalParameter ); + +//! Calculates partial derivative of point mass gravitational acceleration wrt gravitational parameter of the central body. +/*! + * Calculates partial derivative of point mass gravitational acceleration wrt gravitational parameter of the central body. + * \param acceleratedBodyPosition Cartesian state of body being accelerated. + * \param acceleratingBodyPosition Cartesian state of body exerting acceleration. + * \return Vector with the partial of the acceleration vector w.r.t. ational parameter of the central body. + */ +Eigen::Vector3d computePartialOfCentralGravityWrtGravitationalParameter( const Eigen::Vector3d& acceleratedBodyPosition, + const Eigen::Vector3d& acceleratingBodyPosition); + + +//! Calculates partial derivative of point mass gravitational acceleration wrt gravitational parameter of the central body. +/*! + * Calculates partial derivative of point mass gravitational acceleration wrt gravitational parameter of the central body. + * \param gravitationalAcceleration Gravitational acceleration vector for which partial is to be computed. + * \param gravitationalParameter Gravitational parameter of gravitating body. + * \return Vector with the partial of the acceleration vector w.r.t. ational parameter of the central body. + */ +Eigen::Vector3d computePartialOfCentralGravityWrtGravitationalParameter( const Eigen::Vector3d& gravitationalAcceleration, + const double gravitationalParameter ); + + +//! Class to calculate the partials of the central gravitational acceleration w.r.t. parameters and states. +class CentralGravitationPartial: public AccelerationPartial +{ +public: + + //! Default constructor. + /*! + * Default constructor. + * \param gravitationalAcceleration Central gravitational acceleration w.r.t. which partials are to be taken. + * \param acceleratedBody Body undergoing acceleration. + * \param acceleratingBody Body exerting acceleration. + */ + CentralGravitationPartial( + const boost::shared_ptr< gravitation::CentralGravitationalAccelerationModel3d > gravitationalAcceleration, + const std::string acceleratedBody, + const std::string acceleratingBody ); + + //! Function for calculating the partial of the acceleration w.r.t. the position of body undergoing acceleration.. + /*! + * Function for calculating the partial of the acceleration w.r.t. the position of body undergoing acceleration + * and adding it to the existing partial block + * Update( ) function must have been called during current time step before calling this function. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian position of body + * undergoing acceleration where current partial is to be added. + * \param addContribution Variable denoting whether to return the partial itself (true) or the negative partial (false). + * \param startRow First row in partialMatrix block where the computed partial is to be added. + * \param startColumn First column in partialMatrix block where the computed partial is to be added. + */ + void wrtPositionOfAcceleratedBody( + Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 0 ) + { + if( addContribution ) + { + partialMatrix.block( startRow, startColumn, 3, 3 ) += currentPartialWrtPosition_; + } + else + { + partialMatrix.block( startRow, startColumn, 3, 3 ) -= currentPartialWrtPosition_; + } + } + + //! Function for calculating the partial of the acceleration w.r.t. the velocity of body undergoing acceleration.. + /*! + * Function for calculating the partial of the acceleration w.r.t. the velocity of body undergoing acceleration and + * adding it to the existing partial block. + * The update( ) function must have been called during current time step before calling this function. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian position of body + * exerting acceleration where current partial is to be added. + * \param addContribution Variable denoting whether to return the partial itself (true) or the negative partial (false). + * \param startRow First row in partialMatrix block where the computed partial is to be added. + * \param startColumn First column in partialMatrix block where the computed partial is to be added. + */ + void wrtPositionOfAcceleratingBody( Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 0 ) + { + if( addContribution ) + { + partialMatrix.block( startRow, startColumn, 3, 3 ) -= currentPartialWrtPosition_; + } + else + { + partialMatrix.block( startRow, startColumn, 3, 3 ) += currentPartialWrtPosition_; + } + } + + //! Function for determining if the acceleration is dependent on a non-translational integrated state. + /*! + * Function for determining if the acceleration is dependent on a non-translational integrated state. + * No dependency is implemented, but a warning is provided if partial w.r.t. mass of body exerting acceleration + * (and undergoing acceleration if mutual attraction is used) is requested. + * \param stateReferencePoint Reference point id of propagated state + * \param integratedStateType Type of propagated state for which dependency is to be determined. + * \return True if dependency exists (non-zero partial), false otherwise. + */ + bool isStateDerivativeDependentOnIntegratedNonTranslationalState( + const std::pair< std::string, std::string >& stateReferencePoint, + const propagators::IntegratedStateType integratedStateType ) + { + if( ( ( stateReferencePoint.first == acceleratingBody_ || + ( stateReferencePoint.first == acceleratedBody_ && accelerationUsesMutualAttraction_ ) ) + && integratedStateType == propagators::body_mass_state ) ) + { + throw std::runtime_error( "Warning, dependency of central gravity on body masses not yet implemented" ); + } + return 0; + } + + //! Function for setting up and retrieving a function returning a partial w.r.t. a double parameter. + /*! + * Function for setting up and retrieving a function returning a partial w.r.t. a double parameter. + * Function returns empty function and zero size indicator for parameters with no dependency for current acceleration. + * \param parameter Parameter w.r.t. which partial is to be taken. + * \return Pair of parameter partial function and number of columns in partial (0 for no dependency, 1 otherwise). + */ + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > + getParameterPartialFunction( boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameter ); + + //! Function for setting up and retrieving a function returning a partial w.r.t. a vector parameter. + /*! + * Function for setting up and retrieving a function returning a partial w.r.t. a vector parameter. + * Function returns empty function and zero size indicator for parameters with no dependency for current acceleration. + * \param parameter Parameter w.r.t. which partial is to be taken. + * \return Pair of parameter partial function and number of columns in partial (0 for no dependency). + */ + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > getParameterPartialFunction( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > parameter ) + { + boost::function< void( Eigen::MatrixXd& ) > partialFunction; + return std::make_pair( partialFunction, 0 ); + } + + //! Function for updating partial w.r.t. the bodies' positions + /*! + * Function for updating common blocks of partial to current state. For the central gravitational acceleration, + * position partial is computed and set. + * \param currentTime Time at which partials are to be calculated + */ + void update( const double currentTime = TUDAT_NAN ) + { + accelerationUpdateFunction_( currentTime ); + + if( !( currentTime_ == currentTime ) ) + { + currentAcceleratedBodyState_ = acceleratedBodyState_( ); + currentCentralBodyState_ = centralBodyState_( ); + currentGravitationalParameter_ = gravitationalParameterFunction_( ); + + currentPartialWrtPosition_ = calculatePartialOfPointMassGravityWrtPositionOfAcceleratedBody( + currentAcceleratedBodyState_, + currentCentralBodyState_, + currentGravitationalParameter_ ); + + currentTime_ = currentTime; + } + } + +protected: + + //! Function to create a function returning the current partial w.r.t. a gravitational parameter. + /*! + * Function to create a function returning the current partial w.r.t. a gravitational parameter. + * \param parameterId Identifier of parameter for which the partial is to be created. + * \return Pair with partial function and paramater partial size. The partial function is non-empty only + * if the parameterId input represents the gravitational parameter of acceleratingBody_ (or acceleratedBody_ if + * accelerationUsesMutualAttraction_ is true). + */ + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > getGravitationalParameterPartialFunction( + const estimatable_parameters::EstimatebleParameterIdentifier& parameterId ); + + //! Function to calculate central gravity partial w.r.t. central body gravitational parameter. + void wrtGravitationalParameterOfCentralBody( Eigen::MatrixXd& gravitationalParameterPartial ); + + //! Function to retrieve current gravitational parameter of central body. + boost::function< double( ) > gravitationalParameterFunction_; + + //! Function to retrieve current state of body exerting acceleration. + boost::function< Eigen::Vector3d( ) > centralBodyState_; + + //! Function to retrieve current state of body undergoing acceleration. + boost::function< Eigen::Vector3d( ) > acceleratedBodyState_; + + //! Boolean denoting whether the gravitational attraction of the central body on the accelerated body is included. + bool accelerationUsesMutualAttraction_; + + //! Current state of the body undergoing the acceleration (as set by update function). + Eigen::Vector3d currentAcceleratedBodyState_; + + //! Current state of the body exerting the acceleration (as set by update function). + Eigen::Vector3d currentCentralBodyState_; + + //! Current gravitational parameetr of the body exerting the acceleration (as set by update function). + double currentGravitationalParameter_; + + //! Current partial of central gravity acceleration w.r.t. position of body undergoing acceleration + /*! + * Current partial of central gravity acceleration w.r.t. position of body undergoing acceleration + * ( = -partial of central gravity acceleration w.r.t. position of body exerting acceleration), + * calculated and set by update( ) function. + */ + Eigen::Matrix3d currentPartialWrtPosition_; + + //! Function to update the gravitational acceleration model. + boost::function< void( const double ) > accelerationUpdateFunction_; + +}; + +} // namespace acceleration_partials + +} // namespace tudat + +#endif // TUDAT_CENTRALGRAVITYACCELERATIONPARTIALS_H diff --git a/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/numericalAccelerationPartial.cpp b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/numericalAccelerationPartial.cpp new file mode 100644 index 0000000000..41c2cf3052 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/numericalAccelerationPartial.cpp @@ -0,0 +1,196 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/numericalAccelerationPartial.h" + +namespace tudat +{ + +namespace acceleration_partials +{ + +//! Dummy function used for update, performs no calculations. +void emptyFunction( ){ } + +//! Dummy function used for update, performs no calculations. +void emptyTimeFunction( const double time ){ } + +//! Function to numerical compute the partial derivative of an acceleration w.r.t. a body state. +Eigen::Matrix3d calculateAccelerationWrtStatePartials( + boost::function< void( basic_mathematics::Vector6d ) > setBodyState, + boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > accelerationModel, + basic_mathematics::Vector6d originalState, + Eigen::Vector3d statePerturbation, + int startIndex, + boost::function< void( ) > updateFunction ) +{ + Eigen::Matrix3d upAccelerations = Eigen::Matrix3d::Zero( ); + Eigen::Matrix3d downAccelerations = Eigen::Matrix3d::Zero( ); + + basic_mathematics::Vector6d perturbedState = originalState; + + // Calculate perturbed accelerations for up-perturbed state entries. + for( int i = 0; i < 3; i++ ) + { + perturbedState( i + startIndex ) += statePerturbation( i ); + setBodyState( perturbedState ); + updateFunction( ); + upAccelerations.block( 0, i, 3, 1 ) = basic_astrodynamics::updateAndGetAcceleration< Eigen::Vector3d >( + accelerationModel ); + accelerationModel->resetTime( TUDAT_NAN ); + perturbedState = originalState; + } + + // Calculate perturbed accelerations for down-perturbed state entries. + for( int i = 0; i < 3; i++ ) + { + perturbedState( i + startIndex ) -= statePerturbation( i ); + setBodyState( perturbedState ); + updateFunction( ); + downAccelerations.block( 0, i, 3, 1 ) = basic_astrodynamics::updateAndGetAcceleration< Eigen::Vector3d >( + accelerationModel ); + accelerationModel->resetTime( TUDAT_NAN ); + perturbedState = originalState; + } + + // Reset state/environment to original state. + setBodyState( perturbedState ); + updateFunction( ); + + basic_astrodynamics::updateAndGetAcceleration< Eigen::Vector3d >( + accelerationModel ); + + // Numerically compute partial derivatives. + Eigen::Matrix3d accelerationPartials = upAccelerations - downAccelerations; + for( int i = 0; i < 3; i++ ) + { + accelerationPartials.block( 0, i, 3, 1 ) /= ( 2.0 * statePerturbation( i ) ); + } + + return accelerationPartials; +} + +//! Function to numerical compute the partial derivative of an acceleration w.r.t. a double parameter +Eigen::Vector3d calculateAccelerationWrtParameterPartials( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameter, + boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > accelerationModel, + double parameterPerturbation, + boost::function< void( ) > updateDependentVariables, + const double currentTime, + boost::function< void( const double ) > timeDependentUpdateDependentVariables ) +{ + // Store uperturbed value. + double unperturbedParameterValue = parameter->getParameterValue( ); + + // Calculate up-perturbation + parameter->setParameterValue( + unperturbedParameterValue + parameterPerturbation ); + updateDependentVariables( ); + timeDependentUpdateDependentVariables( currentTime ); + + Eigen::Vector3d upPerturbedAcceleration = basic_astrodynamics::updateAndGetAcceleration< Eigen::Vector3d >( + accelerationModel, currentTime ); + accelerationModel->resetTime( TUDAT_NAN ); + + // Calculate down-perturbation. + parameter->setParameterValue( + unperturbedParameterValue - parameterPerturbation ); + updateDependentVariables( ); + timeDependentUpdateDependentVariables( currentTime ); + + Eigen::Vector3d downPerturbedAcceleration = basic_astrodynamics::updateAndGetAcceleration< Eigen::Vector3d >( + accelerationModel, currentTime ); + accelerationModel->resetTime( TUDAT_NAN ); + + // Reset to original value. + parameter->setParameterValue( + unperturbedParameterValue ) ; + updateDependentVariables( ); + timeDependentUpdateDependentVariables( currentTime ); + + accelerationModel->resetTime( TUDAT_NAN ); + basic_astrodynamics::updateAndGetAcceleration< Eigen::Vector3d >( + accelerationModel, currentTime ); + + // Calculate partial using central difference. + return ( upPerturbedAcceleration - downPerturbedAcceleration ) / ( 2.0 * parameterPerturbation ); + +} + +//! Function to numerical compute the partial derivative of an acceleration w.r.t. a vector parameter +Eigen::Matrix< double, 3, Eigen::Dynamic > calculateAccelerationWrtParameterPartials( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > parameter, + boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > accelerationModel, + Eigen::VectorXd parameterPerturbation, + boost::function< void( ) > updateDependentVariables, + const double currentTime, + boost::function< void( const double ) > timeDependentUpdateDependentVariables ) +{ + // Store uperturbed value. + + Eigen::VectorXd unperturbedParameterValue = parameter->getParameterValue( ); + + + if( unperturbedParameterValue.size( ) != parameterPerturbation.size( ) ) + { + throw std::runtime_error( "Error when calculating numerical parameter partial of acceleration, parameter and perturbations are not the same size" ); + } + + Eigen::Matrix< double, 3, Eigen::Dynamic > partialMatrix = Eigen::MatrixXd::Zero( 3, unperturbedParameterValue.size( ) ); + + Eigen::VectorXd perturbedParameterValue; + for( int i = 0; i < unperturbedParameterValue.size( ); i++ ) + { + + perturbedParameterValue = unperturbedParameterValue; + perturbedParameterValue( i ) += parameterPerturbation( i ); + + // Calculate up-perturbation + parameter->setParameterValue( perturbedParameterValue ); + updateDependentVariables( ); + timeDependentUpdateDependentVariables( currentTime ); + Eigen::Vector3d upPerturbedAcceleration = basic_astrodynamics::updateAndGetAcceleration< Eigen::Vector3d >( + accelerationModel, currentTime ); + accelerationModel->resetTime( TUDAT_NAN ); + + // Calculate down-perturbation. + perturbedParameterValue = unperturbedParameterValue; + perturbedParameterValue( i ) -= parameterPerturbation( i ); + parameter->setParameterValue( perturbedParameterValue ); + updateDependentVariables( ); + timeDependentUpdateDependentVariables( currentTime ); + Eigen::Vector3d downPerturbedAcceleration = basic_astrodynamics::updateAndGetAcceleration< Eigen::Vector3d >( + accelerationModel, currentTime ); + accelerationModel->resetTime( TUDAT_NAN ); + + // Compute partial entry. + partialMatrix.block( 0, i, 3, 1 ) = + ( upPerturbedAcceleration - downPerturbedAcceleration ) / ( 2.0 * parameterPerturbation( i ) ); + + } + + // Reset to original value. + parameter->setParameterValue( + unperturbedParameterValue ) ; + updateDependentVariables( ); + timeDependentUpdateDependentVariables( currentTime ); + accelerationModel->resetTime( TUDAT_NAN ); + + basic_astrodynamics::updateAndGetAcceleration< Eigen::Vector3d >( + accelerationModel, currentTime ); + + // Calculate partial using central difference. + return partialMatrix; +} + + +} + +} diff --git a/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/numericalAccelerationPartial.h b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/numericalAccelerationPartial.h new file mode 100644 index 0000000000..d6738950d5 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/numericalAccelerationPartial.h @@ -0,0 +1,111 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_NUMERICALACCELERATIONPARTIAL_H +#define TUDAT_NUMERICALACCELERATIONPARTIAL_H + +#include + +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h" +#include "Tudat/Mathematics/BasicMathematics/linearAlgebraTypes.h" + +namespace tudat +{ + +namespace acceleration_partials +{ + +//! Dummy function used for update, performs no calculations. +/*! + * Dummy function used for update, performs no calculations. + */ +void emptyFunction( ); + +//! Dummy function used for update, performs no calculations. +/*! + * Dummy function used for update, performs no calculations. + * \param time Input parameter, not used by function. + */ +void emptyTimeFunction( const double time ); + +//! Function to numerical compute the partial derivative of an acceleration w.r.t. a body state. +/*! + * Function to numerical compute the partial derivative of an acceleration w.r.t. a body state (position or velocity), + * using a first-order central difference method. + * \param setBodyState Function to reset the current state w.r.t. which the partial is to be computed. + * \param accelerationModel Acceleration model for which the partial derivative is to be computed. + * \param originalState Nominal state at which the partial derivative is to be computed. + * \param statePerturbation Perturbation to the position or velocity that is to be used + * \param startIndex Start index in the state vector from where the statePerturbation is to be added (i.e. 0 if + * function is to compute partial w.r.t. position, 3 if w.r.t velocity). + * \param updateFunction Function to update the required environment models following the change of the body state. + * \return Numerical partial of the acceleration w.r.t. position or velocity (depending on function input). + */ +Eigen::Matrix3d calculateAccelerationWrtStatePartials( + boost::function< void( basic_mathematics::Vector6d ) > setBodyState, + boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > accelerationModel, + basic_mathematics::Vector6d originalState, + Eigen::Vector3d statePerturbation, + int startIndex, + boost::function< void( ) > updateFunction = emptyFunction ); + +//! Function to numerical compute the partial derivative of an acceleration w.r.t. a double parameter +/*! + * Function to numerical compute the partial derivative of an acceleration w.r.t. a double parameter, + * using a first-order central difference method. + * \param parameter Object describing the parameter w.r.t. which teh partial is to be taken. + * \param accelerationModel Acceleration model for which the partial derivative is to be computed. + * \param parameterPerturbation Perturbation to be used for parameter value. + * \param updateDependentVariables Function to update the required environment models following the change in parameter, + * for models that do not explicitly depend on the current time. + * \param currentTime Time at which partial is to be computed. + * \param timeDependentUpdateDependentVariables Function to update the required environment models following the change in + * parameters for models that do explicitly depend on the current time. + * \return Numerical partial of the acceleration w.r.t. given parameter. + */ +Eigen::Vector3d calculateAccelerationWrtParameterPartials( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameter, + boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > accelerationModel, + double parameterPerturbation, + boost::function< void( ) > updateDependentVariables = emptyFunction, + const double currentTime = 0.0, + boost::function< void( const double ) > timeDependentUpdateDependentVariables = emptyTimeFunction ); + +//! Function to numerical compute the partial derivative of an acceleration w.r.t. a vector parameter +/*! + * Function to numerical compute the partial derivative of an acceleration w.r.t. a vector parameter, + * using a first-order central difference method. + * \param parameter Object describing the parameter w.r.t. which teh partial is to be taken. + * \param accelerationModel Acceleration model for which the partial derivative is to be computed. + * \param parameterPerturbation Perturbations to be used for parameter value. + * \param updateDependentVariables Function to update the required environment models following the change in parameter, + * for models that do not explicitly depend on the current time. + * \param currentTime Time at which partial is to be computed. + * \param timeDependentUpdateDependentVariables Function to update the required environment models following the change in + * parameters for models that do explicitly depend on the current time. + * \return Numerical partial of the acceleration w.r.t. given parameter. + */ +Eigen::Matrix< double, 3, Eigen::Dynamic > calculateAccelerationWrtParameterPartials( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > parameter, + boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > accelerationModel, + Eigen::VectorXd parameterPerturbation, + boost::function< void( ) > updateDependentVariables = emptyFunction, + const double currentTime = 0.0, + boost::function< void( const double ) > timeDependentUpdateDependentVariables = emptyTimeFunction ); + + +} // namespace acceleration_partials + +} // namespace tudat + +#endif // TUDAT_NUMERICALACCELERATIONPARTIAL_H diff --git a/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/radiationPressureAccelerationPartial.cpp b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/radiationPressureAccelerationPartial.cpp new file mode 100644 index 0000000000..2aa07a2256 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/radiationPressureAccelerationPartial.cpp @@ -0,0 +1,82 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/radiationPressureCoefficient.h" +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/radiationPressureAccelerationPartial.h" + +namespace tudat +{ + +namespace acceleration_partials +{ + +//! Calculates partial derivative of cannon ball radiation pressure acceleration wrt radiation pressure coefficient. +Eigen::Vector3d computePartialOfCannonBallRadiationPressureAccelerationWrtRadiationPressureCoefficient( + const double radiationPressure, + const double area, + const double bodyMass, + const Eigen::Vector3d& vectorToSource ) +{ + return -radiationPressure * area / bodyMass * vectorToSource; +} + +//! Function for setting up and retrieving a function returning a partial w.r.t. a double parameter. +std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > CannonBallRadiationPressurePartial::getParameterPartialFunction( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameter ) +{ + boost::function< void( Eigen::MatrixXd& ) > partialFunction; + int numberOfRows = 0; + + // Check if parameter dependency exists. + if( parameter->getParameterName( ).second.first == acceleratedBody_ ) + { + switch( parameter->getParameterName( ).first ) + { + // Set function returning partial w.r.t. radiation pressure coefficient. + case estimatable_parameters::radiation_pressure_coefficient: + + partialFunction = boost::bind( &CannonBallRadiationPressurePartial::wrtRadiationPressureCoefficient, + this, _1 ); + numberOfRows = 1; + + break; + default: + break; + } + } + return std::make_pair( partialFunction, numberOfRows ); +} + + +//! Function for setting up and retrieving a function returning a partial w.r.t. a vector parameter. +std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > CannonBallRadiationPressurePartial::getParameterPartialFunction( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > parameter ) +{ + boost::function< void( Eigen::MatrixXd& ) > partialFunction; + int numberOfRows = 0; + + // Check if parameter dependency exists. + if( parameter->getParameterName( ).second.first == acceleratedBody_ ) + { + switch( parameter->getParameterName( ).first ) + { + + default: + break; + } + } + return std::make_pair( partialFunction, numberOfRows ); +} + + +} + +} + diff --git a/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/radiationPressureAccelerationPartial.h b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/radiationPressureAccelerationPartial.h new file mode 100644 index 0000000000..d11c8c4756 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/radiationPressureAccelerationPartial.h @@ -0,0 +1,246 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_RADIATIONPRESSUREACCELERATIONPARTIAL_H +#define TUDAT_RADIATIONPRESSUREACCELERATIONPARTIAL_H + +#include + +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/accelerationPartial.h" +#include "Tudat/Astrodynamics/ElectroMagnetism/radiationPressureInterface.h" + +namespace tudat +{ + +namespace acceleration_partials +{ + +//! Calculates partial derivative of cannon ball radiation pressure acceleration wrt radiation pressure coefficient. +/*! + * Calculates partial derivative of cannon ball radiation pressure acceleration wrt radiation pressure coefficient. + * \param radiationPressure Current radiation pressure (in N/m^2) + * \param area (Reference) area for radiation pressure acceleration. + * \param bodyMass Mass of body undergoing acceleration. + * \param vectorToSource Vector from body undergoing acceleration to source of radiation. + * \return Partial derivative of cannon ball radiation pressure acceleration wrt radiation pressure coefficient. + */ +Eigen::Vector3d computePartialOfCannonBallRadiationPressureAccelerationWrtRadiationPressureCoefficient( + const double radiationPressure, + const double area, + const double bodyMass, + const Eigen::Vector3d& vectorToSource ); + +//! Class to calculate the partials of the cannnonball radiation pressure acceleration w.r.t. parameters and states. +class CannonBallRadiationPressurePartial: public AccelerationPartial +{ +public: + + //! Constructor. + /*! + * Constructor. + * \param radiationPressureInterface Interface object for properties of radiation pressure computation (i.e. reference + * area, pressure magnitude, etc.) + * \param massFunction Function returning the mass of the body undergoing the acceleration. + * \param acceleratedBody Name of the body undergoing acceleration. + * \param acceleratingBody Name of the body exerting acceleration. + */ + CannonBallRadiationPressurePartial( + const boost::shared_ptr< electro_magnetism::RadiationPressureInterface > radiationPressureInterface, + const boost::function< double( ) > massFunction, + const std::string& acceleratedBody, const std::string& acceleratingBody ): + AccelerationPartial( acceleratedBody, acceleratingBody, + basic_astrodynamics::cannon_ball_radiation_pressure ), + sourceBodyState_( radiationPressureInterface->getSourcePositionFunction( ) ), + acceleratedBodyState_( radiationPressureInterface->getTargetPositionFunction( ) ), + areaFunction_( boost::bind( &electro_magnetism::RadiationPressureInterface::getArea, radiationPressureInterface ) ), + radiationPressureCoefficientFunction_( + boost::bind( &electro_magnetism::RadiationPressureInterface::getRadiationPressureCoefficient, + radiationPressureInterface ) ), + radiationPressureFunction_( boost::bind( &electro_magnetism::RadiationPressureInterface::getCurrentRadiationPressure, + radiationPressureInterface ) ), + acceleratedBodyMassFunction_( massFunction ){ } + + //! Destructor. + ~CannonBallRadiationPressurePartial( ){ } + + //! Function for calculating the partial of the acceleration w.r.t. the position of body undergoing acceleration.. + /*! + * Function for calculating the partial of the acceleration w.r.t. the position of body undergoing acceleration + * and adding it to exting partial block. + * Update( ) function must have been called during current time step before calling this function. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian position of body + * undergoing acceleration where current partial is to be added. + * \param addContribution Variable denoting whether to return the partial itself (true) or the negative partial (false). + * \param startRow First row in partialMatrix block where the computed partial is to be added. + * \param startColumn First column in partialMatrix block where the computed partial is to be added. + */ + void wrtPositionOfAcceleratedBody( Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 0 ) + { + if( addContribution ) + { + partialMatrix.block( startRow, startColumn, 3, 3 ) += currentPartialWrtPosition_; + } + else + { + partialMatrix.block( startRow, startColumn, 3, 3 ) -= currentPartialWrtPosition_; + } + } + + //! Function for calculating the partial of the acceleration w.r.t. the velocity of body undergoing acceleration.. + /*! + * Function for calculating the partial of the acceleration w.r.t. the velocity of body undergoing acceleration and + * adding it to exting partial block. + * The update( ) function must have been called during current time step before calling this function. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian position of body + * exerting acceleration where current partial is to be added. + * \param addContribution Variable denoting whether to return the partial itself (true) or the negative partial (false). + * \param startRow First row in partialMatrix block where the computed partial is to be added. + * \param startColumn First column in partialMatrix block where the computed partial is to be added. + */ + void wrtPositionOfAcceleratingBody( Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 0 ) + { + if( addContribution ) + { + partialMatrix.block( startRow, startColumn, 3, 3 ) -= currentPartialWrtPosition_; + } + else + { + partialMatrix.block( startRow, startColumn, 3, 3 ) += currentPartialWrtPosition_; + } + } + + //! Function for calculating the partial of the acceleration w.r.t. a non-translational integrated state + /*! + * Function for calculating the partial of the acceleration w.r.t. a non-translational integrated state + * and adding it to the existing partial block. + * \param partialMatrix Block of partial derivatives of where current partial is to be added. + * \param stateReferencePoint Reference point id of propagated state + * \param integratedStateType Type of propagated state for which partial is to be computed. + */ + void wrtNonTranslationalStateOfAdditionalBody( + Eigen::Block< Eigen::MatrixXd > partialMatrix, + const std::pair< std::string, std::string >& stateReferencePoint, + const propagators::IntegratedStateType integratedStateType ) + { + if( stateReferencePoint.first == acceleratedBody_ && integratedStateType == propagators::body_mass_state ) + { + partialMatrix.block( 0, 0, 3, 1 ) += + radiationPressureFunction_( ) * areaFunction_( ) * radiationPressureCoefficientFunction_( ) * + ( sourceBodyState_( ) - acceleratedBodyState_( ) ).normalized( ) / + ( acceleratedBodyMassFunction_( ) * acceleratedBodyMassFunction_( ) ); + } + } + + //! Function for determining if the acceleration is dependent on a non-translational integrated state. + /*! + * Function for determining if the acceleration is dependent on a non-translational integrated state. + * \param stateReferencePoint Reference point id of propagated state + * \param integratedStateType Type of propagated state for which dependency is to be determined. + * \return True if dependency exists (non-zero partial), false otherwise. + */ + bool isStateDerivativeDependentOnIntegratedNonTranslationalState( + const std::pair< std::string, std::string >& stateReferencePoint, + const propagators::IntegratedStateType integratedStateType ) + { + bool isDependent = 0; + + // Acceleration is dependent on mass of body undergoing acceleration. + if( stateReferencePoint.first == acceleratedBody_ && integratedStateType == propagators::body_mass_state ) + { + isDependent = 1; + } + return isDependent; + } + + //! Function for setting up and retrieving a function returning a partial w.r.t. a double parameter. + /*! + * Function for setting up and retrieving a function returning a partial w.r.t. a double parameter. + * Function returns empty function and zero size indicator for parameters with no dependency for current acceleration. + * \param parameter Parameter w.r.t. which partial is to be taken. + * \return Pair of parameter partial function and number of columns in partial (0 for no dependency, 1 otherwise). + */ + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > + getParameterPartialFunction( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameter ); + + //! Function for setting up and retrieving a function returning a partial w.r.t. a vector parameter. + /*! + * Function for setting up and retrieving a function returning a partial w.r.t. a vector parameter. + * Function returns empty function and zero size indicator for parameters with no dependency for current acceleration. + * \param parameter Parameter w.r.t. which partial is to be taken. + * \return Pair of parameter partial function and number of columns in partial (0 for no dependency). + */ + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > getParameterPartialFunction( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > parameter ); + + void wrtRadiationPressureCoefficient( Eigen::MatrixXd& partial ) + { + partial = computePartialOfCannonBallRadiationPressureAccelerationWrtRadiationPressureCoefficient( + radiationPressureFunction_( ), areaFunction_( ), acceleratedBodyMassFunction_( ), + ( sourceBodyState_( ) - acceleratedBodyState_( ) ).normalized( ) ); + } + + //! Function for updating partial w.r.t. the bodies' positions + /*! + * Function for updating common blocks of partial to current state. For the radiation pressure acceleration, + * position partial is computed and set. + * \param currentTime Time at which partials are to be calculated + */ + void update( const double currentTime = 0.0 ) + { + if( !( currentTime_ == currentTime ) ) + { + // Compute helper quantities. + Eigen::Vector3d rangeVector = ( acceleratedBodyState_( ) - sourceBodyState_( ) ); + double range = rangeVector.norm( ); + double rangeInverse = 1.0 / ( range ); + + // Compute position partial. + currentPartialWrtPosition_ = + ( radiationPressureCoefficientFunction_( ) * areaFunction_( ) * radiationPressureFunction_( ) / + acceleratedBodyMassFunction_( ) ) * ( Eigen::Matrix3d::Identity( ) * rangeInverse - 3.0 * + rangeVector * rangeVector.transpose( ) * rangeInverse / ( + range * range ) ); + currentTime_ = currentTime; + } + } + +private: + + //! Function returning position of radiation source. + boost::function< Eigen::Vector3d( ) > sourceBodyState_; + + //! Function returning position of body undergoing acceleration. + boost::function< Eigen::Vector3d( )> acceleratedBodyState_; + + //! Function returning reflecting (or reference) area of radiation pressure on acceleratedBody_ + boost::function< double( ) > areaFunction_; + + //! Function returning current radiation pressure coefficient (usually denoted C_{r}). + boost::function< double( ) > radiationPressureCoefficientFunction_; + + //! Function returning current radiation pressure (in N/m^{2}) + boost::function< double( ) > radiationPressureFunction_; + + //! Function returning the mass of the body undergoing the acceleration. + boost::function< double( ) > acceleratedBodyMassFunction_; + + //! Current partial of acceleration w.r.t. position of body undergoing acceleration (equal to minus partial w.r.t. + //! position of body exerting acceleration). + Eigen::Matrix3d currentPartialWrtPosition_; +}; + +} // namespace acceleration_partials + +} // namespace tudat + +#endif // TUDAT_RADIATIONPRESSUREACCELERATIONPARTIAL_H diff --git a/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/sphericalHarmonicAccelerationPartial.cpp b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/sphericalHarmonicAccelerationPartial.cpp new file mode 100644 index 0000000000..e12c554ea1 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/sphericalHarmonicAccelerationPartial.cpp @@ -0,0 +1,313 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h" +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/sphericalHarmonicAccelerationPartial.h" +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/sphericalHarmonicPartialFunctions.h" +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/centralGravityAccelerationPartial.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicCosineCoefficients.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicSineCoefficients.h" + +namespace tudat +{ + +namespace acceleration_partials +{ + +//! Contructor. +SphericalHarmonicsGravityPartial::SphericalHarmonicsGravityPartial( + const std::string& acceleratedBody, + const std::string& acceleratingBody, + const boost::shared_ptr< gravitation::SphericalHarmonicsGravitationalAccelerationModel > accelerationModel, + const observation_partials::RotationMatrixPartialNamedList& rotationMatrixPartials ): + AccelerationPartial( acceleratedBody, acceleratingBody, basic_astrodynamics::spherical_harmonic_gravity ), + gravitationalParameterFunction_( accelerationModel->getGravitationalParameterFunction( ) ), + bodyReferenceRadius_( boost::bind( &gravitation::SphericalHarmonicsGravitationalAccelerationModel::getReferenceRadius, + accelerationModel ) ), + cosineCoefficients_( accelerationModel->getCosineHarmonicCoefficientsFunction( ) ), + sineCoefficients_( accelerationModel->getSineHarmonicCoefficientsFunction( ) ), + sphericalHarmonicCache_( accelerationModel->getSphericalHarmonicsCache( ) ), + positionFunctionOfAcceleratedBody_( boost::bind( &gravitation::SphericalHarmonicsGravitationalAccelerationModel:: + getCurrentPositionOfBodySubjectToAcceleration, accelerationModel ) ), + positionFunctionOfAcceleratingBody_( boost::bind( &gravitation::SphericalHarmonicsGravitationalAccelerationModel:: + getCurrentPositionOfBodyExertingAcceleration, accelerationModel ) ), + fromBodyFixedToIntegrationFrameRotation_( boost::bind( &gravitation::SphericalHarmonicsGravitationalAccelerationModel:: + getCurrentRotationToIntegrationFrameMatrix, accelerationModel ) ), + accelerationFunction_( boost::bind( &gravitation::SphericalHarmonicsGravitationalAccelerationModel::getAcceleration, + accelerationModel ) ), + updateFunction_( boost::bind( &gravitation::SphericalHarmonicsGravitationalAccelerationModel::updateMembers, + accelerationModel, _1 ) ), + rotationMatrixPartials_( rotationMatrixPartials ), + accelerationUsesMutualAttraction_( accelerationModel->getIsMutualAttractionUsed( ) ) +{ + sphericalHarmonicCache_->getLegendreCache( )->setComputeSecondDerivatives( 1 ); + + // Update number of degrees and orders in legendre cache for calculation of position partials + + maximumDegree_ = cosineCoefficients_( ).rows( ) - 1; + maximumOrder_ = sineCoefficients_( ).cols( ) - 1; + + if( sphericalHarmonicCache_->getMaximumDegree( ) < maximumDegree_ || + sphericalHarmonicCache_->getMaximumOrder( ) < maximumOrder_ + 2 ) + { + sphericalHarmonicCache_->resetMaximumDegreeAndOrder( maximumDegree_, maximumOrder_ + 2 ); + } +} + +//! Function to create a function returning a partial w.r.t. a double parameter. +std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > SphericalHarmonicsGravityPartial::getParameterPartialFunction( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameter ) +{ + // Declare return variables, default number of rows = 0 (i.e. no dependency) + boost::function< void( Eigen::MatrixXd& ) > partialFunction; + int numberOfRows = 0; + + + // Check properties of body exerting acceleration. + if( parameter->getParameterName( ).first == estimatable_parameters::gravitational_parameter ) + { + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > partialFunctionPair = + getGravitationalParameterPartialFunction( parameter->getParameterName( ) ); + partialFunction = partialFunctionPair.first; + numberOfRows = partialFunctionPair.second; + } + else if( parameter->getParameterName( ).second.first == acceleratingBody_ ) + { + // Check if partial is a rotational property of body exerting acceleration. + if( estimatable_parameters::isParameterRotationMatrixProperty( parameter->getParameterName( ).first ) ) + { + // Check if required rotation matrix partial exists. + if( rotationMatrixPartials_.count( std::make_pair( parameter->getParameterName( ).first, + parameter->getSecondaryIdentifier( ) ) ) != 0 ) + { + // Get partial function. + partialFunction = boost::bind( + &SphericalHarmonicsGravityPartial::wrtRotationModelParameter, + this, _1, parameter->getParameterName( ).first, parameter->getSecondaryIdentifier( ) ); + numberOfRows = 1; + } + else + { + std::string errorMessage = "Error, not taking partial of sh acceleration wrt rotational parameter" + + boost::lexical_cast< std::string >( parameter->getParameterName( ).first ) + " of " + + parameter->getParameterName( ).second.first; + throw std::runtime_error( errorMessage ); + } + + } + } + + // Return partial function and partial size. + return std::make_pair( partialFunction, numberOfRows ); +} + +//! Function to create a function returning a partial w.r.t. a vector parameter. +std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > SphericalHarmonicsGravityPartial::getParameterPartialFunction( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > parameter ) +{ + using namespace tudat::estimatable_parameters; + + // Declare return variables, default number of rows = 0 (i.e. no dependency) + boost::function< void( Eigen::MatrixXd& ) > partialFunction; + int numberOfRows = 0; + + // Check properties of body exerting acceleration. + if( parameter->getParameterName( ).second.first == acceleratingBody_ ) + { + // Check if partial is a rotational property of body exerting acceleration. + if( estimatable_parameters::isParameterRotationMatrixProperty( parameter->getParameterName( ).first ) ) + { + // Check if required rotation matrix partial exists. + if( rotationMatrixPartials_.count( std::make_pair( parameter->getParameterName( ).first, + parameter->getSecondaryIdentifier( ) ) ) != 0 ) + { + // Get partial function. + partialFunction = boost::bind( + &SphericalHarmonicsGravityPartial::wrtRotationModelParameter, + this,_1, parameter->getParameterName( ).first, parameter->getSecondaryIdentifier( ) ); + numberOfRows = parameter->getParameterSize( ); + } + else + { + std::string errorMessage = "Error, not taking partial of sh acceleration wrt rotational parameter" + + boost::lexical_cast< std::string >( parameter->getParameterName( ).first ) + " of " + + parameter->getParameterName( ).second.first; + throw std::runtime_error( errorMessage ); + } + } + // Check non-rotational parameters. + else + { + switch( parameter->getParameterName( ).first ) + { + case spherical_harmonics_cosine_coefficient_block: + { + // Cast parameter object to required type. + boost::shared_ptr< SphericalHarmonicsCosineCoefficients > coefficientsParameter = + boost::dynamic_pointer_cast< SphericalHarmonicsCosineCoefficients >( parameter ); + + partialFunction = boost::bind( &SphericalHarmonicsGravityPartial::wrtCosineCoefficientBlock, this, + coefficientsParameter->getBlockIndices( ), _1 ); + numberOfRows = coefficientsParameter->getParameterSize( ); + + break; + } + case spherical_harmonics_sine_coefficient_block: + { + // Cast parameter object to required type. + + boost::shared_ptr< SphericalHarmonicsSineCoefficients > coefficientsParameter = + boost::dynamic_pointer_cast< SphericalHarmonicsSineCoefficients >( parameter ); + + partialFunction = boost::bind( &SphericalHarmonicsGravityPartial::wrtSineCoefficientBlock, this, + coefficientsParameter->getBlockIndices( ), _1 ); + numberOfRows = coefficientsParameter->getParameterSize( ); + + break; + } + default: + break; + } + } + } + + // Return partial function and partial size. + return std::make_pair( partialFunction, numberOfRows ); +} + +//! Function to create a function returning the current partial w.r.t. a gravitational parameter. +std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > +SphericalHarmonicsGravityPartial::getGravitationalParameterPartialFunction( + const estimatable_parameters::EstimatebleParameterIdentifier& parameterId ) +{ + boost::function< void( Eigen::MatrixXd& ) > partialFunction; + int numberOfColumns = 0; + + if( parameterId.first == estimatable_parameters::gravitational_parameter ) + { + // Check for dependency + if( parameterId.second.first == acceleratingBody_ ) + { + partialFunction = boost::bind( &SphericalHarmonicsGravityPartial::wrtGravitationalParameterOfCentralBody, + this, _1 ); + numberOfColumns = 1; + + } + + if( parameterId.second.first == acceleratedBody_ ) + { + if( accelerationUsesMutualAttraction_ ) + { + partialFunction = boost::bind( &SphericalHarmonicsGravityPartial::wrtGravitationalParameterOfCentralBody, + this, _1 ); + numberOfColumns = 1; + } + } + } + return std::make_pair( partialFunction, numberOfColumns ); +} + +//! Function for updating the partial object to current state and time. +void SphericalHarmonicsGravityPartial::update( const double currentTime ) +{ + using namespace tudat::coordinate_conversions; + + + if( !( currentTime_ == currentTime ) ) + { + // Update acceleration model + updateFunction_( currentTime ); + + // Calculate Cartesian position in frame fixed to body exerting acceleration + Eigen::Matrix3d currentRotationToBodyFixedFrame_ = fromBodyFixedToIntegrationFrameRotation_( ).inverse( ); + bodyFixedPosition_ = currentRotationToBodyFixedFrame_ * + ( positionFunctionOfAcceleratedBody_( ) - positionFunctionOfAcceleratingBody_( ) ); + + // Calculate spherical position in frame fixed to body exerting acceleration + bodyFixedSphericalPosition_ = convertCartesianToSpherical( bodyFixedPosition_ ); + bodyFixedSphericalPosition_( 1 ) = mathematical_constants::PI / 2.0 - bodyFixedSphericalPosition_( 1 ); + + // Get spherical harmonic coefficients + currentCosineCoefficients_ = cosineCoefficients_( ); + currentSineCoefficients_ = sineCoefficients_( ); + + // Update trogonometric functions of multiples of longitude. + sphericalHarmonicCache_->update( + bodyFixedSphericalPosition_( 0 ), std::sin( bodyFixedSphericalPosition_( 1 ) ), + bodyFixedSphericalPosition_( 2 ), bodyReferenceRadius_( ) ); + + + // Calculate partial of acceleration wrt position of body undergoing acceleration. + currentBodyFixedPartialWrtPosition_ = computePartialDerivativeOfBodyFixedSphericalHarmonicAcceleration( + bodyFixedPosition_, bodyReferenceRadius_( ), gravitationalParameterFunction_( ), + currentCosineCoefficients_, currentSineCoefficients_, sphericalHarmonicCache_ ); + + currentPartialWrtVelocity_.setZero( ); + currentPartialWrtPosition_ = + currentRotationToBodyFixedFrame_.inverse( ) * currentBodyFixedPartialWrtPosition_ * + currentRotationToBodyFixedFrame_; + + currentTime_ = currentTime; + } +} + +//! Function to calculate the partial of the acceleration wrt a set of cosine coefficients. +void SphericalHarmonicsGravityPartial::wrtCosineCoefficientBlock( + const std::vector< std::pair< int, int > >& blockIndices, + Eigen::MatrixXd& partialDerivatives ) +{ + calculateSphericalHarmonicGravityWrtCCoefficients( + bodyFixedSphericalPosition_, bodyReferenceRadius_( ), gravitationalParameterFunction_( ), + sphericalHarmonicCache_, + blockIndices, coordinate_conversions::getSphericalToCartesianGradientMatrix( + bodyFixedPosition_ ), fromBodyFixedToIntegrationFrameRotation_( ), partialDerivatives ); +} + +//! Function to calculate the partial of the acceleration wrt a set of sine coefficients. +void SphericalHarmonicsGravityPartial::wrtSineCoefficientBlock( + const std::vector< std::pair< int, int > >& blockIndices, + Eigen::MatrixXd& partialDerivatives ) +{ + calculateSphericalHarmonicGravityWrtSCoefficients( + bodyFixedSphericalPosition_, bodyReferenceRadius_( ), gravitationalParameterFunction_( ), + sphericalHarmonicCache_, + blockIndices, coordinate_conversions::getSphericalToCartesianGradientMatrix( + bodyFixedPosition_ ), fromBodyFixedToIntegrationFrameRotation_( ), partialDerivatives ); +} + +//! Function to calculate an acceleration partial wrt a rotational parameter. +void SphericalHarmonicsGravityPartial::wrtRotationModelParameter( + Eigen::MatrixXd& accelerationPartial, + const estimatable_parameters::EstimatebleParametersEnum parameterType, + const std::string& secondaryIdentifier ) +{ + // Calculate distance vector between bodies. + Eigen::Vector3d distanceVector = positionFunctionOfAcceleratedBody_( ) - positionFunctionOfAcceleratingBody_( ); + + // Get rotation matrix partial(s) wrt requested parameter + std::vector< Eigen::Matrix3d > rotationMatrixPartials = + rotationMatrixPartials_.at( std::make_pair( parameterType, secondaryIdentifier ) )-> + calculatePartialOfRotationMatrixToBaseFrameWrParameter( currentTime_ ); + + // Iterate for each single parameter entry partial. + for( unsigned int i = 0; i < rotationMatrixPartials.size( ); i++ ) + { + // Calculate acceleration partial for current parameter entry. + accelerationPartial.block( 0, i, 3, 1 ) = rotationMatrixPartials[ i ] * + ( fromBodyFixedToIntegrationFrameRotation_( ).inverse( ) ) * accelerationFunction_( ) + + fromBodyFixedToIntegrationFrameRotation_( ) * currentBodyFixedPartialWrtPosition_* + rotationMatrixPartials[ i ].transpose( ) * distanceVector; + } +} + +} + +} + diff --git a/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/sphericalHarmonicAccelerationPartial.h b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/sphericalHarmonicAccelerationPartial.h new file mode 100644 index 0000000000..469d5dd5c8 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/sphericalHarmonicAccelerationPartial.h @@ -0,0 +1,370 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_SPHERICALHARMONICACCELERATIONPARTIAL_H +#define TUDAT_SPHERICALHARMONICACCELERATIONPARTIAL_H + +#include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h" + +#include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h" + +#include "Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h" +#include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.h" +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/accelerationPartial.h" +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/sphericalHarmonicAccelerationPartial.h" +#include "Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/rotationMatrixPartial.h" + + + +namespace tudat +{ + +namespace acceleration_partials +{ + +//! Class for calculating partial derivatives of a spherical harmonic gravitational acceleration. +/*! + * Class for calculating partial derivatives of a spherical harmonic gravitational acceleration, as calculated by the + * SphericalHarmonicsGravitationalAccelerationModel class. + */ +class SphericalHarmonicsGravityPartial: public AccelerationPartial +{ +public: + + //! Contructor. + /*! + * Constructor, requires input on the acceleration model as of which partials are to be computed. + * If any partials of parameters of the rotation model of the body exerting acceleration are to be calculated, + * RotationMatrixPartial objects must be pre-constructed and passed here as a map, with one object for each parameter + * wrt which a partial is to be taken. + * \param acceleratedBody Name of body undergoing acceleration. + * \param acceleratingBody Name of body exerting acceleration. + * \param accelerationModel Spherical harmonic gravity acceleration model from which acceleration is calculated wrt + * which the object being constructed is to calculate partials. + * \param rotationMatrixPartials Map of RotationMatrixPartial, one for each paramater representing a property of the + * rotation of the body exerting the acceleration wrt which an acceleration partial will be calculated. + */ + SphericalHarmonicsGravityPartial( + const std::string& acceleratedBody, + const std::string& acceleratingBody, + const boost::shared_ptr< gravitation::SphericalHarmonicsGravitationalAccelerationModel > accelerationModel, + const observation_partials::RotationMatrixPartialNamedList& rotationMatrixPartials = + observation_partials::RotationMatrixPartialNamedList( ) ); + + //! Destructor + ~SphericalHarmonicsGravityPartial( ){ } + + //! Function for calculating the partial of the acceleration w.r.t. the position of body undergoing acceleration.. + /*! + * Function for calculating the partial of the acceleration w.r.t. the position of body undergoing acceleration + * and adding it to the existing partial block + * Update( ) function must have been called during current time step before calling this function. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian position of body + * undergoing acceleration where current partial is to be added. + * \param addContribution Variable denoting whether to return the partial itself (true) or the negative partial (false). + * \param startRow First row in partialMatrix block where the computed partial is to be added. + * \param startColumn First column in partialMatrix block where the computed partial is to be added. + */ + void wrtPositionOfAcceleratedBody( + Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 0 ) + { + if( addContribution ) + { + partialMatrix.block( startRow, startColumn, 3, 3 ) += currentPartialWrtPosition_; + } + else + { + partialMatrix.block( startRow, startColumn, 3, 3 ) -= currentPartialWrtPosition_; + } + } + + //! Function for calculating the partial of the acceleration w.r.t. the position of body exerting acceleration. + /*! + * Function for calculating the partial of the acceleration w.r.t. the position of body exerting acceleration and + * adding it to the existing partial block. + * The update( ) function must have been called during current time step before calling this function. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian position of body + * exerting acceleration where current partial is to be added. + * \param addContribution Variable denoting whether to return the partial itself (true) or the negative partial (false). + * \param startRow First row in partialMatrix block where the computed partial is to be added. + * \param startColumn First column in partialMatrix block where the computed partial is to be added. + */ + void wrtPositionOfAcceleratingBody( Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 0 ) + { + if( addContribution ) + { + partialMatrix.block( startRow, startColumn, 3, 3 ) -= currentPartialWrtPosition_; + } + else + { + partialMatrix.block( startRow, startColumn, 3, 3 ) += currentPartialWrtPosition_; + } + } + + + //! Function for determining if the acceleration is dependent on a non-translational integrated state. + /*! + * Function for determining if the acceleration is dependent on a non-translational integrated state. + * No dependency is implemented, but a warning is provided if partial w.r.t. mass of body exerting acceleration + * (and undergoing acceleration if mutual attraction is used) is requested. + * \param stateReferencePoint Reference point id of propagated state + * \param integratedStateType Type of propagated state for which dependency is to be determined. + * \return True if dependency exists (non-zero partial), false otherwise. + */ + bool isStateDerivativeDependentOnIntegratedNonTranslationalState( + const std::pair< std::string, std::string >& stateReferencePoint, + const propagators::IntegratedStateType integratedStateType ) + { + if( ( ( stateReferencePoint.first == acceleratingBody_ || + ( stateReferencePoint.first == acceleratedBody_ && accelerationUsesMutualAttraction_ ) ) + && integratedStateType == propagators::body_mass_state ) ) + { + throw std::runtime_error( "Warning, dependency of central gravity on body masses not yet implemented" ); + } + return 0; + } + + //! Function for setting up and retrieving a function returning a partial w.r.t. a double parameter. + /*! + * Function for setting up and retrieving a function returning a partial w.r.t. a double parameter. + * Function returns empty function and zero size indicator for parameters with no dependency for current acceleration. + * \param parameter Parameter w.r.t. which partial is to be taken. + * \return Pair of parameter partial function and number of columns in partial (0 for no dependency, 1 otherwise). + */ + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > + getParameterPartialFunction( boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameter ); + + //! Function for setting up and retrieving a function returning a partial w.r.t. a vector parameter. + /*! + * Function for setting up and retrieving a function returning a partial w.r.t. a vector parameter. + * Function returns empty function and zero size indicator for parameters with no dependency for current acceleration. + * \param parameter Parameter w.r.t. which partial is to be taken. + * \return Pair of parameter partial function and number of columns in partial (0 for no dependency). + */ + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > getParameterPartialFunction( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > parameter ); + + //! Function to create a function returning the current partial w.r.t. a gravitational parameter. + /*! + * Function to create a function returning the current partial w.r.t. a gravitational parameter. + * \param parameterId Identifier of parameter for which the partial is to be created. + * \return Pair with partial function and paramater partial size. The partial function is non-empty only + * if the parameterId input represents the gravitational parameter of acceleratingBody_ (or acceleratedBody_ if + * accelerationUsesMutualAttraction_ is true). + */ + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > getGravitationalParameterPartialFunction( + const estimatable_parameters::EstimatebleParameterIdentifier& parameterId ); + + //! Function for updating the partial object to current state and time. + /*! + * Function for updating the partial object to current state and time. Calculates the variables that are + * used for the calculation of multple partials, to prevent multiple calculations of same function. + * \param currentTime Time to which object is to be updated (note that most update functions are time-independent, + * since the 'current' state of the bodies is typically updated globally by the NBodyStateDerivative class). + */ + virtual void update( const double currentTime = TUDAT_NAN ); + + + //! Function to calculate the partial wrt the gravitational parameter. + /*! + * Function to calculate the partial wrt the gravitational parameter of the central body. Note that in the case of + * mutual attraction (see SphericalHarmonicsGravitationalAccelerationModel), the partial wrt the gravitational + * parameter of the body exerting acceleration is equal to the partial wrt to the body undergoing the acceleration, + * which is zero otherwise. + * \return Partial wrt the gravitational parameter of the central body. + */ + void wrtGravitationalParameterOfCentralBody( Eigen::MatrixXd& partialMatrix ) + { + if( gravitationalParameterFunction_( ) != 0.0 ) + { + partialMatrix = accelerationFunction_( ) / gravitationalParameterFunction_( ); + } + else + { + throw std::runtime_error( "Error cannot compute partial of spherical harminic gravity w.r.t mu for zero value" ); + } + } + +protected: + + //! Function to calculate the partial of the acceleration wrt a set of cosine coefficients. + /*! + * Function to calculate the partial of the acceleration wrt a set of cosine coefficients. + * The set of coefficients wrt which a partial is to be taken is provided as input. + * \param blockIndices List of cosine coefficient indices wrt which the partials are to be taken (first and second + * are degree and order for each vector entry). + * \param partialDerivatives Matrix of acceleration partials that is set by this function (returned by reference), + * with each column containg the partial wrt a single coefficient (in same order as blockIndices). + */ + void wrtCosineCoefficientBlock( + const std::vector< std::pair< int, int > >& blockIndices, + Eigen::MatrixXd& partialDerivatives ); + + //! Function to calculate the partial of the acceleration wrt a set of sine coefficients. + /*! + * Function to calculate the partial of the acceleration wrt a set of sine coefficients. + * The set of coefficients wrt which a partial is to be taken is provided as input. + * \param blockIndices List of sine coefficient indices wrt which the partials are to be taken (first and second + * are degree and order for each vector entry). + * \param partialDerivatives Matrix of acceleration partials that is set by this function (returned by reference), + * with each column containg the partial wrt a single coefficient (in same order as blockIndices). + */ + void wrtSineCoefficientBlock( + const std::vector< std::pair< int, int > >& blockIndices, + Eigen::MatrixXd& partialDerivatives ); + + //! Function to calculate an acceleration partial wrt a rotational parameter. + /*! + * Function to calculate an acceleration partial wrt a rotational parameter of the rotation model of the body + * exerting the acceleration. + * \param accelerationPartial Matrix of partials of spherical harmonic acceleration wrt a rotational parameter + * that is set by this function (returned by reference) + * \param parameterType Type of parameter wrt which a partial is to be calculated. + * An entry of the requested type must be present in the rotationMatrixPartials_ map. + * \param secondaryIdentifier Identifier required to unambiguously define the parameter (in addition to information in + * parameterType. + */ + void wrtRotationModelParameter( + Eigen::MatrixXd& accelerationPartial, + const estimatable_parameters::EstimatebleParametersEnum parameterType, + const std::string& secondaryIdentifier ); + + + //! Function to return the gravitational parameter used for calculating the acceleration. + boost::function< double( ) > gravitationalParameterFunction_; + + //! Function to return the reference radius used for calculating the acceleration. + boost::function< double( ) > bodyReferenceRadius_; + + //! Function to return the current cosine coefficients of the spherical harmonic gravity field. + boost::function< Eigen::MatrixXd( ) > cosineCoefficients_; + + //! Function to return the current sine coefficients of the spherical harmonic gravity field. + boost::function< Eigen::MatrixXd( ) > sineCoefficients_; + + //! Cache object used for storing calculated values at current time and state for spherical harmonic gravity + //! calculations. + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicCache_; + + //! Function returning position of body undergoing acceleration. + boost::function< Eigen::Vector3d( ) > positionFunctionOfAcceleratedBody_; + + //! Function returning position of body exerting acceleration. + boost::function< Eigen::Vector3d( ) > positionFunctionOfAcceleratingBody_; + + //! Function return current rotation from inertial frame to frame fixed to body exerting acceleration. + boost::function< Eigen::Matrix3d( ) > fromBodyFixedToIntegrationFrameRotation_; + + + //! Function to retrieve the current spherical harmonic acceleration. + boost::function< Eigen::Matrix< double, 3, 1 >( ) > accelerationFunction_; + + //! Function to update the acceleration to the current state and time. + /*! + * Function to update the acceleration to the current state and time. + * Called when updating an object of this class with the update( time ) function, + * in case the partial is called before the acceleration model in the current iteration of the numerical integration. + */ + boost::function< void( const double ) > updateFunction_; + + //! Current cosine coefficients of the spherical harmonic gravity field. + /*! + * Current cosine coefficients of the spherical harmonic gravity field, set by update( time ) function. + */ + Eigen::MatrixXd currentCosineCoefficients_; + + //! Current sine coefficients of the spherical harmonic gravity field. + /*! + * Current sine coefficients of the spherical harmonic gravity field, set by update( time ) function. + */ + Eigen::MatrixXd currentSineCoefficients_; + + //! Current body-fixed (w.r.t body exerting acceleration) position of body undergoing acceleration + /*! + * Current body-fixed (w.r.t body exerting acceleration) position of body undergoing acceleration, + * set by update( time ) function. + */ + Eigen::Vector3d bodyFixedPosition_; + + + //! Current spherical coordinate of body undergoing acceleration + /*! + * Current spherical coordinate of body undergoing acceleration, in reference frame fixed to body exerting acceleration. + * Order of components is radial distance (from center of body), latitude, longitude. Note that the the second entry + * differs from the direct output of the cartesian -> spherical coordinates, which produces a colatitude. + */ + Eigen::Vector3d bodyFixedSphericalPosition_; + + //! The current partial of the acceleration wrt the position of the body undergoing the acceleration. + /*! + * The current partial of the acceleration wrt the position of the body undergoing the acceleration. + * The partial wrt the position of the body exerting the acceleration is minus this value. + * Value is set by the update( time ) function. + */ + Eigen::Matrix3d currentPartialWrtPosition_; + + //! The current partial of the acceleration wrt the position of the body undergoing the acceleration, + //! with both acceleration and position in body-fixed frame. + /*! + * The current partial of the acceleration wrt the position of the body undergoing the acceleration. + * with both acceleration and position in body-fixed frame. Value is set by the update( time ) function. + */ + Eigen::Matrix3d currentBodyFixedPartialWrtPosition_; + + //! The current partial of the acceleration wrt the velocity of the body undergoing the acceleration. + /*! + * The current partial of the acceleration wrt the velocity of the body undergoing the acceleration. + * The partial wrt the velocity of the body exerting the acceleration is minus this value. + * Value is set by the update( time ) function. + */ + Eigen::Matrix3d currentPartialWrtVelocity_; + + + //! Maximum degree of spherical harmonic expansion. + /*! + * Maximum degree of spherical harmonic expansion of body exerting acceleration used in the calculation + * of the acceleration. + */ + int maximumDegree_; + + //! Maximum order of spherical harmonic expansion. + /*! + * Maximum order of spherical harmonic expansion of body exerting acceleration used in the calculation + * of the acceleration. + */ + int maximumOrder_; + + + //! Map of RotationMatrixPartial, one for each relevant rotation parameter + /*! + * Map of RotationMatrixPartial, one for each parameter representing a property of the rotation of the + * body exerting the acceleration wrt which an acceleration partial will be calculated. + * Map is pre-created and set through the constructor. + */ + observation_partials::RotationMatrixPartialNamedList rotationMatrixPartials_; + + //! Boolean denoting whether the mutual attraction between the bodies is taken into account + /*! + * Boolean denoting whether the mutual attraction between the bodies is taken into account, as is the case when + * integrting in a (non-rotating) frame centered on the body exerting the acceleration, in which case the + * gravitational acceleration of the body undergoing the acceleration on that exerting the acceleration must be taken + * into account as an inertial effect. + */ + bool accelerationUsesMutualAttraction_; + +}; + +} // namespace acceleration_partials + +} // namespace tudat + +#endif // TUDAT_SPHERICALHARMONICACCELERATIONPARTIAL_H diff --git a/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/sphericalHarmonicPartialFunctions.cpp b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/sphericalHarmonicPartialFunctions.cpp new file mode 100644 index 0000000000..c887991568 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/sphericalHarmonicPartialFunctions.cpp @@ -0,0 +1,290 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h" +#include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h" + +#include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h" + + +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/sphericalHarmonicPartialFunctions.h" + +namespace tudat +{ + +namespace acceleration_partials +{ + +using namespace orbital_element_conversions; + +//! Function to compute the spherical Hessian of a single term of a spherical harmonic potential +void computePotentialSphericalHessian( + const double distance, + const double radiusPowerTerm, + const double cosineOfOrderLongitude, + const double sineOfOrderLongitude, + const double cosineOfLatitude, + const double sineOfLatitude, + const double preMultiplier, + const int degree, + const int order, + const double cosineHarmonicCoefficient, + const double sineHarmonicCoefficient, + const double legendrePolynomial, + const double legendrePolynomialDerivative, + const double legendrePolynomialSecondDerivative, + Eigen::Matrix3d& sphericalHessian ) +{ + sphericalHessian.setZero( ); + + sphericalHessian( 0, 0 ) += static_cast< double >( degree + 1 ) * static_cast< double >( degree + 2 ) / + ( distance * distance ) * legendrePolynomial * + ( cosineHarmonicCoefficient * cosineOfOrderLongitude + sineHarmonicCoefficient * sineOfOrderLongitude ); + sphericalHessian( 1, 0 ) += -static_cast< double >( degree + 1 ) / distance * cosineOfLatitude * + legendrePolynomialDerivative * + ( cosineHarmonicCoefficient * cosineOfOrderLongitude + sineHarmonicCoefficient * sineOfOrderLongitude ); + sphericalHessian( 2, 0 ) += -static_cast< double >( order ) * static_cast< double >( degree + 1 ) / distance * + legendrePolynomial * + ( -cosineHarmonicCoefficient * sineOfOrderLongitude + sineHarmonicCoefficient * cosineOfOrderLongitude ); + + sphericalHessian( 0, 1 ) = sphericalHessian( 1, 0 ); + sphericalHessian( 1, 1 ) = ( cosineOfLatitude * cosineOfLatitude * legendrePolynomialSecondDerivative - + sineOfLatitude * legendrePolynomialDerivative ) * + ( cosineHarmonicCoefficient * cosineOfOrderLongitude + sineHarmonicCoefficient * sineOfOrderLongitude ); + sphericalHessian( 2, 1 ) = static_cast< double >( order ) * cosineOfLatitude * legendrePolynomialDerivative * + ( -cosineHarmonicCoefficient * sineOfOrderLongitude + sineHarmonicCoefficient * cosineOfOrderLongitude ); + + sphericalHessian( 0, 2 ) = sphericalHessian( 2, 0 ); + sphericalHessian( 1, 2 ) = sphericalHessian( 2, 1 ); + sphericalHessian( 2, 2 ) += static_cast< double >( order ) * static_cast< double >( order ) * legendrePolynomial * + ( -cosineHarmonicCoefficient * cosineOfOrderLongitude - sineHarmonicCoefficient * sineOfOrderLongitude ); + + + sphericalHessian *= preMultiplier * radiusPowerTerm; +} + +//! Function to compute the spherical Hessian of a single term of a spherical harmonic potential +void computePotentialSphericalHessian( + const Eigen::Vector3d& sphericalPosition, + const double referenceRadius, + const double preMultiplier, + const int degree, + const int order, + const double cosineHarmonicCoefficient, + const double sineHarmonicCoefficient, + const double legendrePolynomial, + const double legendrePolynomialDerivative, + const double legendrePolynomialSecondDerivative, + Eigen::Matrix3d& sphericalHessian ) +{ + computePotentialSphericalHessian( + sphericalPosition( radiusIndex ), + basic_mathematics::raiseToIntegerPower + ( referenceRadius / sphericalPosition( radiusIndex ), static_cast< double >( degree ) + 1.0 ), + std::cos( static_cast< double >( order ) * sphericalPosition( longitudeIndex ) ), + std::sin( static_cast< double >( order ) * sphericalPosition( longitudeIndex ) ), + std::cos( sphericalPosition( latitudeIndex ) ), std::sin( sphericalPosition( latitudeIndex ) ), + preMultiplier, degree, order, cosineHarmonicCoefficient, sineHarmonicCoefficient, + legendrePolynomial,legendrePolynomialDerivative, legendrePolynomialSecondDerivative, sphericalHessian ); +} + +//! Function to compute the spherical Hessian of a single term of a spherical harmonic potential +void computePotentialSphericalHessian( + const Eigen::Vector3d& sphericalPosition, + const double preMultiplier, + const int degree, + const int order, + const double cosineHarmonicCoefficient, + const double sineHarmonicCoefficient, + const boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache, + Eigen::Matrix3d& sphericalHessian ) +{ + computePotentialSphericalHessian( + sphericalPosition( 0 ) , sphericalHarmonicsCache->getReferenceRadiusRatioPowers( degree + 1 ), + sphericalHarmonicsCache->getCosineOfMultipleLongitude( order ), + sphericalHarmonicsCache->getSineOfMultipleLongitude( order ), + sphericalHarmonicsCache->getLegendreCache( )->getCurrentPolynomialParameterComplement( ), + sphericalHarmonicsCache->getLegendreCache( )->getCurrentPolynomialParameter( ), + preMultiplier, + degree, order, cosineHarmonicCoefficient, sineHarmonicCoefficient, + sphericalHarmonicsCache->getLegendreCache( )->getLegendrePolynomial( degree, order ), + sphericalHarmonicsCache->getLegendreCache( )->getLegendrePolynomialDerivative( degree, order ), + sphericalHarmonicsCache->getLegendreCache( )->getLegendrePolynomialSecondDerivative( degree, order ), + sphericalHessian ); +} + +//! Function to compute the spherical Hessian of a full spherical harmonic potential +Eigen::Matrix3d computeCumulativeSphericalHessian( + const Eigen::Vector3d& sphericalPosition, + const double referenceRadius, + const double gravitionalParameter, + const Eigen::MatrixXd cosineHarmonicCoefficients, + const Eigen::MatrixXd sineHarmonicCoefficients, + const boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache ) +{ + double preMultiplier = gravitionalParameter / referenceRadius; + + Eigen::Matrix3d sphericalHessian, sphericalHessianTerm; + + sphericalHessian.setZero( ); + for( unsigned int i = 0; i < cosineHarmonicCoefficients.rows( ); i++ ) + { + for( unsigned int j = 0; ( j <= i && j < cosineHarmonicCoefficients.cols( ) ); j++ ) + { + computePotentialSphericalHessian( + sphericalPosition, preMultiplier, i, j, cosineHarmonicCoefficients( i, j ), + sineHarmonicCoefficients( i, j ), sphericalHarmonicsCache, sphericalHessianTerm ); + sphericalHessian += sphericalHessianTerm; + } + } + return sphericalHessian; + +} + +//! Calculate partial of spherical harmonic acceleration w.r.t. position of body undergoing acceleration +//! (in the body-fixed frame) +Eigen::Matrix3d computePartialDerivativeOfBodyFixedSphericalHarmonicAcceleration( + const Eigen::Vector3d& cartesianPosition, + const Eigen::Vector3d& sphericalPosition, + const double referenceRadius, + const double gravitionalParameter, + const Eigen::MatrixXd cosineHarmonicCoefficients, + const Eigen::MatrixXd sineHarmonicCoefficients, + const boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache, + const Eigen::Vector3d& sphericalPotentialGradient, + const Eigen::Matrix3d& sphericalToCartesianGradientMatrix ) +{ + // Compute Hessian in spherical coordinates. + Eigen::Matrix3d sphericalHessian = computeCumulativeSphericalHessian( + sphericalPosition, referenceRadius, gravitionalParameter, cosineHarmonicCoefficients, + sineHarmonicCoefficients, sphericalHarmonicsCache ); + + // Convert to Cartesian Hessian + Eigen::Matrix3d accelerationPartial = + sphericalToCartesianGradientMatrix * sphericalHessian * sphericalToCartesianGradientMatrix.transpose( ); + + // Add effect of direct change in rotation matrix + accelerationPartial += coordinate_conversions::getDerivativeOfSphericalToCartesianGradient( + sphericalPotentialGradient, cartesianPosition ); + + return accelerationPartial; +} + +//! Calculate partial of spherical harmonic acceleration w.r.t. position of body undergoing acceleration +//! (in the body-fixed frame) +Eigen::Matrix3d computePartialDerivativeOfBodyFixedSphericalHarmonicAcceleration( + const Eigen::Vector3d& cartesianPosition, + const double referenceRadius, + const double gravitionalParameter, + const Eigen::MatrixXd cosineHarmonicCoefficients, + const Eigen::MatrixXd sineHarmonicCoefficients, + const boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache ) +{ + // Compute spherical position. + Eigen::Vector3d sphericalPosition = + coordinate_conversions::convertCartesianToSpherical( cartesianPosition ); + sphericalPosition( 1 ) = mathematical_constants::PI / 2.0 - sphericalPosition( 1 ); + + // Compute spherical to Cartesian gradient transformation. + Eigen::Matrix3d gradientTransformationMatrix = + coordinate_conversions::getSphericalToCartesianGradientMatrix( cartesianPosition ); + + // Compute spherical gradient. + Eigen::Vector3d sphericalPotentialGradient = gradientTransformationMatrix.inverse( ) * + gravitation::computeGeodesyNormalizedGravitationalAccelerationSum( + cartesianPosition, gravitionalParameter, referenceRadius, cosineHarmonicCoefficients, + sineHarmonicCoefficients, sphericalHarmonicsCache ); + + return computePartialDerivativeOfBodyFixedSphericalHarmonicAcceleration( + cartesianPosition, sphericalPosition, referenceRadius, gravitionalParameter, cosineHarmonicCoefficients, + sineHarmonicCoefficients, sphericalHarmonicsCache, sphericalPotentialGradient, + gradientTransformationMatrix ); +} + +//! Calculate partial of spherical harmonic acceleration w.r.t. a set of cosine coefficients +void calculateSphericalHarmonicGravityWrtCCoefficients( + const Eigen::Vector3d& sphericalPosition, + const double referenceRadius, + const double gravitionalParameter, + const boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache, + const std::vector< std::pair< int, int > >& blockIndices, + const Eigen::Matrix3d& sphericalToCartesianGradientMatrix, + const Eigen::Matrix3d& bodyFixedToIntegrationFrame, + Eigen::MatrixXd& partialsMatrix ) +{ + double preMultiplier = gravitionalParameter / referenceRadius; + const boost::shared_ptr< basic_mathematics::LegendreCache > legendreCache = sphericalHarmonicsCache->getLegendreCache( ); + + int degree, order; + for( unsigned int i = 0; i < blockIndices.size( ); i++ ) + { + degree = blockIndices.at( i ).first; + order = blockIndices.at( i ).second; + + // Calculate and set partial of current degree and order. + partialsMatrix.block( 0, i, 3, 1 ) = + basic_mathematics::computePotentialGradient( + sphericalPosition( radiusIndex ), + sphericalHarmonicsCache->getReferenceRadiusRatioPowers( degree + 1 ), + sphericalHarmonicsCache->getCosineOfMultipleLongitude( order ), + sphericalHarmonicsCache->getSineOfMultipleLongitude( order ), + sphericalHarmonicsCache->getLegendreCache( )->getCurrentPolynomialParameterComplement( ), + preMultiplier, degree, order, + 1.0, 0.0, legendreCache->getLegendrePolynomial( degree, order ), + legendreCache->getLegendrePolynomialDerivative( degree, order ) ); + + } + + // Transform partials to Cartesian position and integration frame. + partialsMatrix = bodyFixedToIntegrationFrame * sphericalToCartesianGradientMatrix * partialsMatrix; +} + +//! Calculate partial of spherical harmonic acceleration w.r.t. a set of sine coefficients +void calculateSphericalHarmonicGravityWrtSCoefficients( + const Eigen::Vector3d& sphericalPosition, + const double referenceRadius, + const double gravitionalParameter, + const boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache, + const std::vector< std::pair< int, int > >& blockIndices, + const Eigen::Matrix3d& sphericalToCartesianGradientMatrix, + const Eigen::Matrix3d& bodyFixedToIntegrationFrame, + Eigen::MatrixXd& partialsMatrix ) +{ + double preMultiplier = gravitionalParameter / referenceRadius; + const boost::shared_ptr< basic_mathematics::LegendreCache > legendreCache = sphericalHarmonicsCache->getLegendreCache( ); + + int degree, order; + for( unsigned int i = 0; i < blockIndices.size( ); i++ ) + { + degree = blockIndices.at( i ).first; + order = blockIndices.at( i ).second; + + // Calculate and set partial of current degree and order. + partialsMatrix.block( 0, i, 3, 1 ) = + basic_mathematics::computePotentialGradient( + sphericalPosition( radiusIndex ), + sphericalHarmonicsCache->getReferenceRadiusRatioPowers( degree + 1 ), + sphericalHarmonicsCache->getCosineOfMultipleLongitude( order ), + sphericalHarmonicsCache->getSineOfMultipleLongitude( order ), + sphericalHarmonicsCache->getLegendreCache( )->getCurrentPolynomialParameterComplement( ), + preMultiplier, degree, order, + 0.0, 1.0, legendreCache->getLegendrePolynomial( degree, order ), + legendreCache->getLegendrePolynomialDerivative( degree, order ) ); + + } + + // Transform partials to Cartesian position and integration frame. + partialsMatrix = bodyFixedToIntegrationFrame * sphericalToCartesianGradientMatrix * partialsMatrix; +} + +} + +} diff --git a/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/sphericalHarmonicPartialFunctions.h b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/sphericalHarmonicPartialFunctions.h new file mode 100644 index 0000000000..b5e2425a0a --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/sphericalHarmonicPartialFunctions.h @@ -0,0 +1,258 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_SPHERICALHARMONICPARTIALFUNCTIONS_H +#define TUDAT_SPHERICALHARMONICPARTIALFUNCTIONS_H + +#include + +#include + +#include "Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h" + +namespace tudat +{ + +namespace acceleration_partials +{ + +//! Function to compute the spherical Hessian of a single term of a spherical harmonic potential +/*! + * Function to compute the spherical Hessian (i.e. matrix of second derivatives w.r.t. spherical components radius, latitude + * and longitude) of a single term of a spherical harmonic potential. + * \param distance Distance to center of body with gravity field at which the partials are to be calculated + * \param radiusPowerTerm Distance divided by the reference radius of the gravity field, to the power (degree + 1) + * \param cosineOfOrderLongitude Cosine of order times the longitude at which the potential is to be calculated + * \param sineOfOrderLongitude Sine of order times the longitude at which the potential is to be calculated + * \param cosineOfLatitude Cosine of the latitude at which the potential is to be calculated + * \param sineOfLatitude Sine of the latitude at which the potential is to be calculated + * \param preMultiplier Pre-multiplier of potential (gravitational parametere divided by reference radius in normal + * representation) + * \param degree Degree of the harmonic for which the gradient is to be computed. + * \param order Order of the harmonic for which the gradient is to be computed. + * \param cosineHarmonicCoefficient Coefficient which characterizes relative strengh of a harmonic term. + * \param sineHarmonicCoefficient Coefficient which characterizes relative strengh of a harmonic term. + * \param legendrePolynomial Value of associated Legendre polynomial with the same degree and order as the to be computed + * harmonic, and with the sine of the latitude coordinate as polynomial parameter. + * \param legendrePolynomialDerivative Value of the derivative of parameter 'legendrePolynomial' with respect to the sine + * of the latitude angle. + * \param legendrePolynomialSecondDerivative Value of second derivative of parameter 'legendrePolynomial' with respect to + * the sine of the latitude angle. + * \param sphericalHessian Hessian of potential term in spherical coordinates (returned by reference). + */ +void computePotentialSphericalHessian( + const double distance, + const double radiusPowerTerm, + const double cosineOfOrderLongitude, + const double sineOfOrderLongitude, + const double cosineOfLatitude, + const double sineOfLatitude, + const double preMultiplier, + const int degree, + const int order, + const double cosineHarmonicCoefficient, + const double sineHarmonicCoefficient, + const double legendrePolynomial, + const double legendrePolynomialDerivative, + const double legendrePolynomialSecondDerivative, + Eigen::Matrix3d& sphericalHessian ); + +//! Function to compute the spherical Hessian of a single term of a spherical harmonic potential +/*! + * Function to compute the spherical Hessian (i.e. matrix of second derivatives w.r.t. spherical components radius, latitude + * and longitude) of a single term of a spherical harmonic potential. + * \param sphericalPosition Spherical position (radius, ,latitude, longitude) at which potential partials are to be + * evaluated + * \param referenceRadius Reference radius of spherical harmonic potential. + * \param preMultiplier Pre-multiplier of potential (gravitational parametere divided by reference radius in normal + * representation) + * \param degree Degree of the harmonic for which the gradient is to be computed. + * \param order Order of the harmonic for which the gradient is to be computed. + * \param cosineHarmonicCoefficient Coefficient which characterizes relative strengh of a harmonic term. + * \param sineHarmonicCoefficient Coefficient which characterizes relative strengh of a harmonic term. + * \param legendrePolynomial Value of associated Legendre polynomial with the same degree and order as the to be computed + * harmonic, and with the sine of the latitude coordinate as polynomial parameter. + * \param legendrePolynomialDerivative Value of the derivative of parameter 'legendrePolynomial' with respect to the sine + * of the latitude angle. + * \param legendrePolynomialSecondDerivative Value of second derivative of parameter 'legendrePolynomial' with respect to + * the sine of the latitude angle. + * \param sphericalHessian Hessian of potential term in spherical coordinates (returned by reference). + */ +void computePotentialSphericalHessian( + const Eigen::Vector3d& sphericalPosition, + const double referenceRadius, + const double preMultiplier, + const int degree, + const int order, + const double cosineHarmonicCoefficient, + const double sineHarmonicCoefficient, + const double legendrePolynomial, + const double legendrePolynomialDerivative, + const double legendrePolynomialSecondDerivative, + Eigen::Matrix3d& sphericalHessian ); + +//! Function to compute the spherical Hessian of a single term of a spherical harmonic potential +/*! + * Function to compute the spherical Hessian (i.e. matrix of second derivatives w.r.t. spherical components radius, latitude + * and longitude) of a single term of a spherical harmonic potential. + * \param sphericalPosition Spherical position (radius, ,latitude, longitude) at which potential partials are to be + * evaluated + * \param preMultiplier Pre-multiplier of potential (gravitational parametere divided by reference radius in normal + * representation) + * \param degree Degree of the harmonic for which the gradient is to be computed. + * \param order Order of the harmonic for which the gradient is to be computed. + * \param cosineHarmonicCoefficient Coefficient which characterizes relative strengh of a harmonic term. + * \param sineHarmonicCoefficient Coefficient which characterizes relative strengh of a harmonic term. + * \param sphericalHarmonicsCache Cache object containing precomputed spherical harmonics terms. + * \param sphericalHessian Hessian of potential term in spherical coordinates (returned by reference). + */ +void computePotentialSphericalHessian( + const Eigen::Vector3d& sphericalPosition, + const double preMultiplier, + const int degree, + const int order, + const double cosineHarmonicCoefficient, + const double sineHarmonicCoefficient, + const boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache, + Eigen::Matrix3d& sphericalHessian ); + +//! Function to compute the spherical Hessian of a full spherical harmonic potential +/*! + * Function to compute the spherical Hessian (i.e. matrix of second derivatives w.r.t. spherical components radius, latitude + * and longitude) of a full spherical harmonic potential. + * \param sphericalPosition Spherical position (radius, ,latitude, longitude) at which potential partials are to be + * evaluated + * \param referenceRadius Reference radius of spherical harmonic potential. + * \param gravitionalParameter Gravitational parameter used for spherical harmonic expansion + * \param cosineHarmonicCoefficients Matrix of coefficient which characterize the relative strengh of cosine harmonic + * terms. + * \param sineHarmonicCoefficients Matrix of coefficient which characterize the relative strengh of sine harmonic terms. + * \param sphericalHarmonicsCache Cache object containing precomputed spherical harmonics terms. + * \return Hessian of potential in spherical coordinates (returned by reference). + */ +Eigen::Matrix3d computeCumulativeSphericalHessian( + const Eigen::Vector3d& sphericalPosition, + const double referenceRadius, + const double gravitionalParameter, + const Eigen::MatrixXd cosineHarmonicCoefficients, + const Eigen::MatrixXd sineHarmonicCoefficients, + const boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache ); + +//! Calculate partial of spherical harmonic acceleration w.r.t. position of body undergoing acceleration +//! (in the body-fixed frame) +/*! + * Calculate partial of spherical harmonic acceleration w.r.t. position of body undergoing acceleration + * (in the body-fixed frame) + * \param cartesianPosition Cartesian position at which potential partials are to be evaluated + * \param sphericalPosition Spherical position (radius, ,latitude, longitude) at which potential partials are to be + * evaluated + * \param referenceRadius Reference radius of spherical harmonic potential. + * \param gravitionalParameter Gravitational parameter used for spherical harmonic expansion + * \param cosineHarmonicCoefficients Cosine spherical harmonic coefficients. + * \param sineHarmonicCoefficients Sine spherical harmonic coefficients + * \param sphericalHarmonicsCache Cache object containing precomputed spherical harmonics terms. + * \param sphericalPotentialGradient Potential gradient in spherical coordinates + * \param sphericalToCartesianGradientMatrix Matrix to convert (by premultiplication) a spherical gradient to a Cartesian + * gradient + * \return Partial of spherical harmonic acceleration w.r.t. position of body undergoinng acceleration (equals minus + * partial of spherical harmonic acceleration w.r.t. position of body exerting acceleration) with both acceleration and + * position in body-fixed frame. + */ +Eigen::Matrix3d computePartialDerivativeOfBodyFixedSphericalHarmonicAcceleration( + const Eigen::Vector3d& cartesianPosition, + const Eigen::Vector3d& sphericalPosition, + const double referenceRadius, + const double gravitionalParameter, + const Eigen::MatrixXd cosineHarmonicCoefficients, + const Eigen::MatrixXd sineHarmonicCoefficients, + const boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache, + const Eigen::Vector3d& sphericalPotentialGradient, + const Eigen::Matrix3d& sphericalToCartesianGradientMatrix ); + +//! Calculate partial of spherical harmonic acceleration w.r.t. position of body undergoing acceleration +//! (in the body-fixed frame) +/*! + * Calculate partial of spherical harmonic acceleration w.r.t. position of body undergoing acceleration + * (in the body-fixed frame) + * \param cartesianPosition Cartesian position at which potential partials are to be evaluated + * \param referenceRadius Reference radius of spherical harmonic potential. + * \param gravitionalParameter Gravitational parameter used for spherical harmonic expansion + * \param cosineHarmonicCoefficients Cosine spherical harmonic coefficients. + * \param sineHarmonicCoefficients Sine spherical harmonic coefficients + * \param sphericalHarmonicsCache Cache object containing precomputed spherical harmonics terms. + * \return Partial of spherical harmonic acceleration w.r.t. position of body undergoinng acceleration (equals minus + * partial of spherical harmonic acceleration w.r.t. position of body exerting acceleration) with both acceleration and + * position in body-fixed frame. + */ +Eigen::Matrix3d computePartialDerivativeOfBodyFixedSphericalHarmonicAcceleration( + const Eigen::Vector3d& cartesianPosition, + const double referenceRadius, + const double gravitionalParameter, + const Eigen::MatrixXd cosineHarmonicCoefficients, + const Eigen::MatrixXd sineHarmonicCoefficients, + const boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache ); + +//! Calculate partial of spherical harmonic acceleration w.r.t. a set of cosine coefficients +/*! + * Calculate partial of spherical harmonic acceleration w.r.t. a set of cosine coefficients + * \param sphericalPosition Spherical coordinate of body undergoing acceleration in frame fixed to body exerting + * acceleration, as radius, latitude, longitude. + * \param referenceRadius Reference radius of spherical harmonic potential. + * \param gravitionalParameter Gravitational parameter used for spherical harmonic expansion + * \param sphericalHarmonicsCache Cache object containing precomputed spherical harmonics terms. + * \param blockIndices List of cosine coefficient indices wrt which the partials are to be taken (first and second + * are degree and order for each vector entry). + * \param sphericalToCartesianGradientMatrix Matrix to convert (by premultiplication) a spherical gradient to a Cartesian + * gradient + * \param bodyFixedToIntegrationFrame Matrix to rotate from body-fixed to integration frame. + * \param partialsMatrix Partials of spherical harmonic acceleration w.r.t. to requested set of cosine coefficients + * (returned by reference). + */ +void calculateSphericalHarmonicGravityWrtCCoefficients( + const Eigen::Vector3d& sphericalPosition, + const double referenceRadius, + const double gravitionalParameter, + const boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache, + const std::vector< std::pair< int, int > >& blockIndices, + const Eigen::Matrix3d& sphericalToCartesianGradientMatrix, + const Eigen::Matrix3d& bodyFixedToIntegrationFrame, + Eigen::MatrixXd& partialsMatrix ); + +//! Calculate partial of spherical harmonic acceleration w.r.t. a set of sine coefficients +/*! + * Calculate partial of spherical harmonic acceleration w.r.t. a set of sine coefficients + * \param sphericalPosition Spherical coordinate of body undergoing acceleration in frame fixed to body exerting + * acceleration, as radius, latitude, longitude. + * \param referenceRadius Reference radius of spherical harmonic potential. + * \param gravitionalParameter Gravitational parameter used for spherical harmonic expansion + * \param sphericalHarmonicsCache Cache object containing precomputed spherical harmonics terms. + * \param blockIndices List of cosine coefficient indices wrt which the partials are to be taken (first and second + * are degree and order for each vector entry). + * \param sphericalToCartesianGradientMatrix Matrix to convert (by premultiplication) a spherical gradient to a Cartesian + * gradient + * \param bodyFixedToIntegrationFrame Matrix to rotate from body-fixed to integration frame. + * \param partialsMatrix Partials of spherical harmonic acceleration w.r.t. to requested set of sine coefficients + * (returned by reference). + */ +void calculateSphericalHarmonicGravityWrtSCoefficients( + const Eigen::Vector3d& sphericalPosition, + const double referenceRadius, + const double gravitionalParameter, + const boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache, + const std::vector< std::pair< int, int > >& blockIndices, + const Eigen::Matrix3d& sphericalToCartesianGradientMatrix, + const Eigen::Matrix3d& bodyFixedToIntegrationFrame, + Eigen::MatrixXd& partialsMatrix ); + +} // namespace acceleration_partials + +} // namespace tudat + +#endif // TUDAT_SPHERICALHARMONICPARTIALFUNCTIONS_H diff --git a/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/thirdBodyGravityPartial.h b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/thirdBodyGravityPartial.h new file mode 100644 index 0000000000..85a7dd8365 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/thirdBodyGravityPartial.h @@ -0,0 +1,501 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_THIRDBODYGRAVITYPARTIAL_H +#define TUDAT_THIRDBODYGRAVITYPARTIAL_H + +#include + +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/centralGravityAccelerationPartial.h" + +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/accelerationPartial.h" + +namespace tudat +{ + +namespace acceleration_partials +{ + +//! Function to get the third body acceleration type from the direct acceleration partial object. +/*! + * Function to get the third body acceleration type from the direct acceleration partial object. + * \param directGravityPartial Partial derivative of direct acceleration. + * \return Type of acceleration for third body acceleration for which direct acceleration partial is given by + * directGravityPartial (i.e. third_body_central_gravity if input is of type CentralGravitationPartial). + */ +template< typename DirectGravityPartial > +basic_astrodynamics::AvailableAcceleration getAccelerationTypeOfThirdBodyGravity( + const boost::shared_ptr< DirectGravityPartial > directGravityPartial ) +{ + using namespace basic_astrodynamics; + AvailableAcceleration accelerationType; + + // Check type of direct partial derivative. + if( boost::dynamic_pointer_cast< CentralGravitationPartial >( directGravityPartial ) != NULL ) + { + accelerationType = third_body_central_gravity; + } + else + { + throw std::runtime_error( "Error when getting third body partial type, type not identified" ); + } + return accelerationType; +} + +//! Class to calculate the partials of a third-body gravitational acceleration w.r.t. parameters and states. +/*! + * Class to calculate the partials of a third-body gravitational acceleration w.r.t. parameters and states. This class may + * be used for any direct gravitational acceleration (central, spherical harmonic, mutual spherical harmonic, etc.), + * providin a generic third-body partial interface. The template parameter is the derived class of AccelerationPartial + * for the associated direct acceleration partial. + */ +template< typename DirectGravityPartial > +class ThirdBodyGravityPartial: public AccelerationPartial +{ +public: + + //! Constructor + /*! + * Constructor + * \param partialOfDirectGravityOnBodyUndergoingAcceleration Partial derivative of direct acceleration from + * acceleratingBody on acceleratedBody. + * \param partialOfDirectGravityOnCentralBody Partial derivative of direct acceleration from + * centralBodyName on acceleratedBody. + * \param acceleratedBody Name of body undergoing acceleration + * \param acceleratingBody Name of body exerting acceleration (third-body) + * \param centralBodyName Name of central body w.r.t. which the acceleration is computed. + */ + ThirdBodyGravityPartial( + const boost::shared_ptr< DirectGravityPartial > partialOfDirectGravityOnBodyUndergoingAcceleration, + const boost::shared_ptr< DirectGravityPartial > partialOfDirectGravityOnCentralBody, + const std::string& acceleratedBody, const std::string& acceleratingBody, + const std::string& centralBodyName ): + AccelerationPartial( acceleratedBody, acceleratingBody, getAccelerationTypeOfThirdBodyGravity( + partialOfDirectGravityOnBodyUndergoingAcceleration ) ), + partialOfDirectGravityOnBodyUndergoingAcceleration_( partialOfDirectGravityOnBodyUndergoingAcceleration ), + partialOfDirectGravityOnCentralBody_( partialOfDirectGravityOnCentralBody ), centralBodyName_( centralBodyName ){ } + + //! Function for calculating the partial of the acceleration w.r.t. the position of body undergoing acceleration.. + /*! + * Function for calculating the partial of the acceleration w.r.t. the position of body undergoing acceleration. + * Update( ) function must have been called during current time step before calling this function. + * \return Partial derivative of acceleration w.r.t. position of body undergoing acceleration. + */ + void wrtPositionOfAcceleratedBody( Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 0 ) + { + partialOfDirectGravityOnBodyUndergoingAcceleration_->wrtPositionOfAcceleratedBody( + partialMatrix, addContribution, startRow, startColumn ); + + // Check if acceleration on central body is dependent on acceleratedBody_ + if( partialOfDirectGravityOnCentralBody_->isAccelerationPartialWrtAdditionalBodyNonNull( acceleratedBody_ ) == 1 ) + { + partialOfDirectGravityOnCentralBody_->wrtPositionOfAdditionalBody( + acceleratedBody_, partialMatrix, addContribution, startRow, startColumn ); + } + } + + //! Function for calculating the partial of the acceleration w.r.t. the velocity of body undergoing acceleration.. + /*! + * Function for calculating the partial of the acceleration w.r.t. the velocity of body undergoing acceleration. + * Update( ) function must have been called during current time step before calling this function. + * \return Partial derivative of acceleration w.r.t. velocity of body undergoing acceleration. + */ + void wrtVelocityOfAcceleratedBody( Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 3 ) + { + partialOfDirectGravityOnBodyUndergoingAcceleration_->wrtVelocityOfAcceleratedBody( + partialMatrix, addContribution, startRow, startColumn ); + + // Check if acceleration on central body is dependent on acceleratedBody_ + if( partialOfDirectGravityOnCentralBody_->isAccelerationPartialWrtAdditionalBodyNonNull( acceleratedBody_ ) == 1 ) + { + partialOfDirectGravityOnCentralBody_->wrtVelocityOfAdditionalBody( + acceleratedBody_, partialMatrix, addContribution, startRow, startColumn ); + } + } + + //! Function for calculating the partial of the acceleration w.r.t. the position of body exerting acceleration.. + /*! + * Function for calculating the partial of the acceleration w.r.t. the position of body exerting acceleration. + * Update( ) function must have been called during current time step before calling this function. + * \return Partial derivative of acceleration w.r.t. position of body exerting acceleration. + */ + void wrtPositionOfAcceleratingBody( Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 0 ) + + { + // Add partials for both direct acceleration and acceleration on central body. + partialOfDirectGravityOnBodyUndergoingAcceleration_->wrtPositionOfAcceleratingBody( + partialMatrix, addContribution, startRow, startColumn ); + partialOfDirectGravityOnCentralBody_->wrtPositionOfAcceleratingBody( + partialMatrix, ( ( addContribution == true ) ? ( false ) : ( true ) ), startRow, startColumn ); + + } + + + //! Function for calculating the partial of the acceleration w.r.t. the velocity of body exerting acceleration.. + /*! + * Function for calculating the partial of the acceleration w.r.t. the velocity of body exerting acceleration. + * Update( ) function must have been called during current time step before calling this function. + * \return Partial derivative of acceleration w.r.t. velocity of body exerting acceleration. + */ + void wrtVelocityOfAcceleratingBody( Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 3 ) + { + // Add partials for both direct acceleration and acceleration on central body. + partialOfDirectGravityOnBodyUndergoingAcceleration_->wrtVelocityOfAcceleratingBody( + partialMatrix, addContribution, startRow, startColumn ); + partialOfDirectGravityOnCentralBody_->wrtVelocityOfAcceleratingBody( + partialMatrix, ( ( addContribution == true ) ? ( false ) : ( true ) ), startRow, startColumn ); + } + + //! Function for calculating the partial of the acceleration w.r.t. the position of an additiona body. + /*! + * Function for calculating the partial of the acceleration w.r.t. the position of an additional body (in addition + * to the body undergoing and exerting the acceleration) and adding it to the existing partial block. + * This function check if the requested additional body equals the central body name. Also, it checks whether either + * the direct or indirect acceleration depend on this additional body. The partial computation is then updated + * accordingly + * \param bodyName Name of additional body. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian position of third body where + * current partial is to be added. + * \param addContribution Variable denoting whether to return the partial itself (true) or the negative partial (false). + * \param startRow First row in partialMatrix block where the computed partial is to be added. + * \param startColumn First column in partialMatrix block where the computed partial is to be added. + */ + void wrtPositionOfAdditionalBody( const std::string& bodyName, Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 0 ) + { + if( bodyName == centralBodyName_ ) + { + partialOfDirectGravityOnCentralBody_->wrtPositionOfAcceleratedBody( + partialMatrix, ( ( addContribution == true ) ? ( false ) : ( true ) ), startRow, startColumn ); + } + + if( partialOfDirectGravityOnBodyUndergoingAcceleration_->isAccelerationPartialWrtAdditionalBodyNonNull( bodyName ) ) + { + partialOfDirectGravityOnBodyUndergoingAcceleration_->wrtPositionOfAdditionalBody( + bodyName, partialMatrix, addContribution, startRow, startColumn ); + } + + if( partialOfDirectGravityOnCentralBody_->isAccelerationPartialWrtAdditionalBodyNonNull( bodyName ) ) + { + partialOfDirectGravityOnCentralBody_->wrtPositionOfAdditionalBody( + bodyName, partialMatrix, ( ( addContribution == true ) ? ( false ) : ( true ) ), startRow, startColumn ); + } + } + + //! Function for calculating the partial of the acceleration w.r.t. the velocity of an additiona body. + /*! + * Function for calculating the partial of the acceleration w.r.t. the velocity of an additional body (in addition + * to the body undergoing and exerting the acceleration) and adding it to the existing partial block. + * This function check if the requested additional body equals the central body name. Also, it checks whether either + * the direct or indirect acceleration depend on this additional body. The partial computation is then updated + * accordingly + * \param bodyName Name of additional body. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian position of third body where + * current partial is to be added. + * \param addContribution Variable denoting whether to return the partial itself (true) or the negative partial (false). + * \param startRow First row in partialMatrix block where the computed partial is to be added. + * \param startColumn First column in partialMatrix block where the computed partial is to be added. + */ + void wrtVelocityOfAdditionalBody( const std::string& bodyName, Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 3 ) + { + if( bodyName == centralBodyName_ ) + { + partialOfDirectGravityOnCentralBody_->wrtVelocityOfAcceleratedBody( + partialMatrix, ( ( addContribution == true ) ? ( false ) : ( true ) ), startRow, startColumn ); + } + + if( partialOfDirectGravityOnBodyUndergoingAcceleration_->isAccelerationPartialWrtAdditionalBodyNonNull( bodyName ) ) + { + partialOfDirectGravityOnBodyUndergoingAcceleration_->wrtVelocityOfAdditionalBody( + bodyName, partialMatrix, addContribution, startRow, startColumn ); + } + + if( partialOfDirectGravityOnCentralBody_->isAccelerationPartialWrtAdditionalBodyNonNull( bodyName ) ) + { + partialOfDirectGravityOnCentralBody_->wrtVelocityOfAdditionalBody( + bodyName, partialMatrix, ( ( addContribution == true ) ? ( false ) : ( true ) ), startRow, startColumn ); + } + } + + //! Function for checking whether the partial of the acceleration w.r.t. the state of an additional body is non-zero. + /*! + * Function for checking whether the partial of the acceleration w.r.t. the state of an additional body is non-zero + * \param bodyName Name of additional body. + * \return True if dependency exists + */ + bool isAccelerationPartialWrtAdditionalBodyNonNull( const std::string& bodyName ) + { + bool isAccelerationDependentOnBody = 0; + if( bodyName == centralBodyName_ ) + { + isAccelerationDependentOnBody = 1; + } + + if( ( partialOfDirectGravityOnCentralBody_->isAccelerationPartialWrtAdditionalBodyNonNull( bodyName ) == 1 ) || + ( partialOfDirectGravityOnBodyUndergoingAcceleration_->isAccelerationPartialWrtAdditionalBodyNonNull( bodyName ) == 1 ) ) + { + isAccelerationDependentOnBody = 1; + } + + return isAccelerationDependentOnBody; + } + + //! Function for calculating the partial of the acceleration w.r.t. a non-translational integrated state + /*! + * Function for calculating the partial of the acceleration w.r.t. a non-translational integrated state + * and adding it to the existing partial block. + * \param partialMatrix Block of partial derivatives of where current partial is to be added. + * \param stateReferencePoint Reference point id of propagated state + * \param integratedStateType Type of propagated state for which partial is to be computed. + */ + void wrtNonTranslationalStateOfAdditionalBody( + Eigen::Block< Eigen::MatrixXd > partialMatrix, + const std::pair< std::string, std::string >& stateReferencePoint, + const propagators::IntegratedStateType integratedStateType ) + { + partialOfDirectGravityOnCentralBody_-> + wrtNonTranslationalStateOfAdditionalBody( + partialMatrix, stateReferencePoint, integratedStateType ); + partialOfDirectGravityOnBodyUndergoingAcceleration_-> + wrtNonTranslationalStateOfAdditionalBody( + partialMatrix, stateReferencePoint, integratedStateType ); + } + + //! Function for determining if the acceleration is dependent on a non-translational integrated state. + /*! + * Function for determining if the acceleration is dependent on a non-translational integrated state. + * \param stateReferencePoint Reference point id of propagated state + * \param integratedStateType Type of propagated state for which dependency is to be determined. + * \return True if dependency exists (non-zero partial), false otherwise. + */ + bool isStateDerivativeDependentOnIntegratedNonTranslationalState( + const std::pair< std::string, std::string >& stateReferencePoint, + const propagators::IntegratedStateType integratedStateType ) + { + if( partialOfDirectGravityOnCentralBody_-> + isStateDerivativeDependentOnIntegratedNonTranslationalState( + stateReferencePoint, integratedStateType ) || + partialOfDirectGravityOnBodyUndergoingAcceleration_-> + isStateDerivativeDependentOnIntegratedNonTranslationalState( + stateReferencePoint, integratedStateType ) ) + { + return true; + } + else + { + return false; + } + } + + + + //! Function for setting up and retrieving a function returning a partial w.r.t. a double parameter. + /*! + * Function for setting up and retrieving a function returning a partial w.r.t. a double parameter. + * Function returns empty function and zero size indicator for parameters with no dependency for current acceleration. + * \param parameter Parameter w.r.t. which partial is to be taken. + * \return Pair of parameter partial function and number of columns in partial (0 for no dependency, 1 otherwise). + */ + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > getParameterPartialFunction( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameter ) + { + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > partialFunctionFromDirectGravity = + partialOfDirectGravityOnBodyUndergoingAcceleration_->getParameterPartialFunction( parameter ); + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > partialFunctionFromCentralGravity = + partialOfDirectGravityOnCentralBody_->getParameterPartialFunction( parameter ); + + return orbit_determination::createMergedParameterPartialFunction( + partialFunctionFromDirectGravity, partialFunctionFromCentralGravity ); + } + + //! Function for setting up and retrieving a function returning a partial w.r.t. a vector parameter. + /*! + * Function for setting up and retrieving a function returning a partial w.r.t. a vector parameter. + * Function returns empty function and zero size indicator for parameters with no dependency for current acceleration. + * \param parameter Parameter w.r.t. which partial is to be taken. + * \return Pair of parameter partial function and number of columns in partial (0 for no dependency). + */ + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > getParameterPartialFunction( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > parameter ) + { + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > partialFunctionFromDirectGravity = + partialOfDirectGravityOnBodyUndergoingAcceleration_->getParameterPartialFunction( parameter ); + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > partialFunctionFromCentralGravity = + partialOfDirectGravityOnCentralBody_->getParameterPartialFunction( parameter ); + + return orbit_determination::createMergedParameterPartialFunction( + partialFunctionFromDirectGravity, partialFunctionFromCentralGravity ); + } + + //! Function to set a dependency of this partial object w.r.t. a given double parameter. + /*! + * Function to set a dependency of this partial object w.r.t. a given double parameter. If a dependency exists, the given + * partial is recomputed on every call of updateParameterPartials. + * \param parameter Partial w.r.t. which dependency is to be checked and set. + * \return Size (number of columns) of parameter partial. Zero if no dependency, 1 otherwise. + */ + int setParameterPartialUpdateFunction( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameter ) + { + // Check parameter dependency of direct acceleration. + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > partialFunctionFromDirectGravity = + partialOfDirectGravityOnBodyUndergoingAcceleration_->getParameterPartialFunction( parameter ); + if( partialFunctionFromDirectGravity.second > 0 ) + { + partialOfDirectGravityOnBodyUndergoingAcceleration_->setParameterPartialUpdateFunction( parameter ); + } + + // Check parameter dependency of indirect acceleration. + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > partialFunctionFromCentralGravity = + partialOfDirectGravityOnCentralBody_->getParameterPartialFunction( parameter ); + if( partialFunctionFromCentralGravity.second > 0 ) + { + partialOfDirectGravityOnCentralBody_->setParameterPartialUpdateFunction( parameter ); + } + + // If either dependency exists, create combined partial. + if( partialFunctionFromCentralGravity.second > 0 || partialFunctionFromDirectGravity.second > 0 ) + { + parameterDoublePartialFunctions_[ parameter ] = + getCombinedCurrentDoubleParameterFunction( + partialOfDirectGravityOnBodyUndergoingAcceleration_, + partialOfDirectGravityOnCentralBody_, + parameter, partialFunctionFromDirectGravity.second, partialFunctionFromCentralGravity.second, 1 ); + isCurrentDoubleParameterPartialSet_[ parameter ] = 0; + currentDoubleParameterPartials_[ parameter ] = Eigen::MatrixXd( accelerationSize_, 1 ); + } + return std::max( partialFunctionFromDirectGravity.second, partialFunctionFromCentralGravity.second ); + } + + //! Function to set a dependency of this partial object w.r.t. a given vector parameter. + /*! + * Function to set a dependency of this partial object w.r.t. a given vector parameter. If a dependency exists, the given + * partial is recomputed on every call of updateParameterPartials. + * \param parameter Partial w.r.t. which dependency is to be checked and set. + * \return Size (number of columns) of parameter partial. Zero if no dependency, size of parameter otherwise. + */ + int setParameterPartialUpdateFunction( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > parameter ) + { + // Check parameter dependency of direct acceleration. + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > partialFunctionFromDirectGravity = + partialOfDirectGravityOnBodyUndergoingAcceleration_->getParameterPartialFunction( parameter ); + if( partialFunctionFromDirectGravity.second > 0 ) + { + partialOfDirectGravityOnBodyUndergoingAcceleration_->setParameterPartialUpdateFunction( parameter ); + } + + // Check parameter dependency of indirect acceleration. + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > partialFunctionFromCentralGravity = + partialOfDirectGravityOnCentralBody_->getParameterPartialFunction( parameter ); + if( partialFunctionFromCentralGravity.second > 0 ) + { + partialOfDirectGravityOnCentralBody_->setParameterPartialUpdateFunction( parameter ); + } + + // If either dependency exists, create combined partial. + if( partialFunctionFromCentralGravity.second > 0 || partialFunctionFromDirectGravity.second > 0 ) + { + parameterVectorPartialFunctions_[ parameter ] = + getCombinedCurrentVectorParameterFunction( + partialOfDirectGravityOnBodyUndergoingAcceleration_, + partialOfDirectGravityOnCentralBody_, + parameter, partialFunctionFromDirectGravity.second, partialFunctionFromCentralGravity.second, 1 ); + isCurrentVectorParameterPartialSet_[ parameter ] = 0; + currentVectorParameterPartials_[ parameter ] = Eigen::MatrixXd( accelerationSize_, parameter->getParameterSize( ) ); + + } + return std::max( partialFunctionFromDirectGravity.second, partialFunctionFromCentralGravity.second ); + } + + //! Function for updating partials w.r.t. the bodies' positions + /*! + * Function for updating common blocks of partial to current state. For the third body gravitational acceleration, + * the update functions of both constituent accelerations are updated. + * \param currentTime Time at which partials are to be calculated + */ + void update( const double currentTime ) + { + partialOfDirectGravityOnBodyUndergoingAcceleration_->update( currentTime ); + partialOfDirectGravityOnCentralBody_->update( currentTime ); + + currentTime_ = currentTime; + } + + //! Function to get partial derivative object of direct acceleration from centralBodyName on acceleratedBody. + /*! + * Function to get the partial derivative object of direct acceleration from centralBodyName on acceleratedBody. + * \return Partial derivative object of direct acceleration from centralBodyName on acceleratedBody. + */ + boost::shared_ptr< DirectGravityPartial > getPartialOfDirectGravityOnBodyUndergoingAcceleration( ) + { + return partialOfDirectGravityOnBodyUndergoingAcceleration_; + } + + //! Function to get partial derivative object of direct acceleration from acceleratingBody on acceleratedBody. + /*! + * Function to get the partial derivative object of direct acceleration from acceleratingBody on acceleratedBody. + * \return Partial derivative object of direct acceleration from acceleratingBody on acceleratedBody. + */ + boost::shared_ptr< DirectGravityPartial > getPartialOfDirectGravityOnCentralBody( ) + { + return partialOfDirectGravityOnCentralBody_; + } + + //! Function to get the name of central body w.r.t. which the acceleration is computed. + /*! + * Function to get the name of central body w.r.t. which the acceleration is computed. + * \return Name of central body w.r.t. which the acceleration is computed. + */ + std::string getCentralBodyName( ) + { + return centralBodyName_; + } + + +protected: + + //! Function to reset the constituent DirectGravityPartial objects to the current time. + void resetTimeOfMemberObjects( ) + { + partialOfDirectGravityOnBodyUndergoingAcceleration_->resetTime( currentTime_ ); + partialOfDirectGravityOnCentralBody_->resetTime( currentTime_ ); + } + + //! Function to update the parameter partials of the constituent DirectGravityPartial objects. + void updateParameterPartialsOfMemberObjects( ) + { + partialOfDirectGravityOnBodyUndergoingAcceleration_->updateParameterPartials( ); + partialOfDirectGravityOnCentralBody_->updateParameterPartials( ); + } + +private: + //! Partial derivative object of direct acceleration from acceleratingBody on acceleratedBody. + boost::shared_ptr< DirectGravityPartial > partialOfDirectGravityOnBodyUndergoingAcceleration_; + + //! Partial derivative object of direct acceleration from centralBodyName on acceleratedBody. + boost::shared_ptr< DirectGravityPartial > partialOfDirectGravityOnCentralBody_; + + //! Name of central body w.r.t. which the acceleration is computed. + std::string centralBodyName_; + +}; + +} // namespace acceleration_partials + +} // namespace tudat + + +#endif // TUDAT_THIRDBODYGRAVITYPARTIAL_H diff --git a/Tudat/Astrodynamics/OrbitDetermination/CMakeLists.txt b/Tudat/Astrodynamics/OrbitDetermination/CMakeLists.txt new file mode 100644 index 0000000000..7ad5bce8c1 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/CMakeLists.txt @@ -0,0 +1,47 @@ + # Copyright (c) 2010-2016, Delft University of Technology + # All rigths reserved + # + # This file is part of the Tudat. Redistribution and use in source and + # binary forms, with or without modification, are permitted exclusively + # under the terms of the Modified BSD license. You should have received + # a copy of the license with this file. If not, please or visit: + # http://tudat.tudelft.nl/LICENSE. + # + # Changelog + # YYMMDD Author Comment + # 110820 S.M. Persson File created. + # 111025 K. Kumar Adapted file to work with Revision 194. + # 111026 K. Kumar Adapted file so all headers show in project tree in Qt Creator. + # + +set(ACCELERATIONPARTIALSDIR "${ORBITDETERMINATIONDIR}/AccelerationPartials") +set(ESTIMATABLEPARAMETERSDIR "${ORBITDETERMINATIONDIR}/EstimatableParameters") +set(OBSERVATIONPARTIALSDIR "${ORBITDETERMINATIONDIR}/ObservationPartials") +set(LIGHTTIMECORRECTIONPARTIALSDIR "${ORBITDETERMINATIONDIR}/LightTimeCorrectionPartials") + +# Add subdirectories. +add_subdirectory("${SRCROOT}${ACCELERATIONPARTIALSDIR}") +add_subdirectory("${SRCROOT}${ESTIMATABLEPARAMETERSDIR}") +add_subdirectory("${SRCROOT}${OBSERVATIONPARTIALSDIR}") + +# Get target properties for static libraries. +get_target_property(ACCELERATIONPARTIALSSOURCES tudat_acceleration_partials SOURCES) +get_target_property(ESTIMATABLEPARAMETERSOURCES tudat_estimatable_parameters SOURCES) +get_target_property(OBSERVATIONPARTIALSSOURCES tudat_observation_partials SOURCES) + +# Set the source files. +set(ORBIT_DETERMINATION_SOURCES + "${SRCROOT}${ORBITDETERMINATIONDIR}/stateDerivativePartial.cpp" +) + +# Set the header files. +set(ORBIT_DETERMINATION_HEADERS + "${SRCROOT}${ORBITDETERMINATIONDIR}/stateDerivativePartial.h" +) + + +# Add static libraries. +add_library(tudat_orbit_determination STATIC ${ORBIT_DETERMINATION_SOURCES} ${ORBIT_DETERMINATION_HEADERS}) +setup_tudat_library_target(tudat_orbit_determination "${SRCROOT}{ORBITDETERMINATIONDIR}") + + diff --git a/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/CMakeLists.txt b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/CMakeLists.txt new file mode 100644 index 0000000000..020f1959af --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/CMakeLists.txt @@ -0,0 +1,39 @@ + # Copyright (c) 2010-2016, Delft University of Technology + # All rigths reserved + # + # This file is part of the Tudat. Redistribution and use in source and + # binary forms, with or without modification, are permitted exclusively + # under the terms of the Modified BSD license. You should have received + # a copy of the license with this file. If not, please or visit: + # http://tudat.tudelft.nl/LICENSE. + # + # Changelog + # YYMMDD Author Comment + # 110820 S.M. Persson File created. + # 111025 K. Kumar Adapted file to work with Revision 194. + # 111026 K. Kumar Adapted file so all headers show in project tree in Qt Creator. + # + +# Set the source files. +set(ESTIMATABLE_PARAMETERS_SOURCES + "${SRCROOT}${ESTIMATABLEPARAMETERSDIR}/estimatableParameter.cpp" + "${SRCROOT}${ESTIMATABLEPARAMETERSDIR}/sphericalHarmonicSineCoefficients.cpp" + "${SRCROOT}${ESTIMATABLEPARAMETERSDIR}/sphericalHarmonicCosineCoefficients.cpp" +) + +# Set the header files. +set(ESTIMATABLE_PARAMETERS_HEADERS + "${SRCROOT}${ESTIMATABLEPARAMETERSDIR}/estimatableParameter.h" + "${SRCROOT}${ESTIMATABLEPARAMETERSDIR}/gravitationalParameter.h" + "${SRCROOT}${ESTIMATABLEPARAMETERSDIR}/initialTranslationalState.h" + "${SRCROOT}${ESTIMATABLEPARAMETERSDIR}/radiationPressureCoefficient.h" + "${SRCROOT}${ESTIMATABLEPARAMETERSDIR}/sphericalHarmonicSineCoefficients.h" + "${SRCROOT}${ESTIMATABLEPARAMETERSDIR}/sphericalHarmonicCosineCoefficients.h" + "${SRCROOT}${ESTIMATABLEPARAMETERSDIR}/constantRotationRate.h" + "${SRCROOT}${ESTIMATABLEPARAMETERSDIR}/constantRotationalOrientation.h" +) + +# Add static libraries. +add_library(tudat_estimatable_parameters STATIC ${ESTIMATABLE_PARAMETERS_SOURCES} ${ESTIMATABLE_PARAMETERS_HEADERS}) +setup_tudat_library_target(tudat_estimatable_parameters "${SRCROOT}{ESTIMATABLEPARAMETERSDIR}") + diff --git a/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/constantRotationRate.h b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/constantRotationRate.h new file mode 100644 index 0000000000..461556786a --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/constantRotationRate.h @@ -0,0 +1,89 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_CONSTANTROTATIONRATE_H +#define TUDAT_CONSTANTROTATIONRATE_H + +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h" +#include "Tudat/Astrodynamics/Ephemerides/simpleRotationalEphemeris.h" + +namespace tudat +{ + +namespace estimatable_parameters +{ + +//! Interface class for estimation of a body's constant rotation rate parameter. +/*! + * Interface class for estimation of a body's constant rotation rate parameter. Interfaces the estimation with the rotation + * rate parameter of a SimpleRotationalEphemeris object + */ +class RotationRate: public EstimatableParameter< double > +{ + +public: + + //! Constructor + /*! + * Constructor + * \param rotationModel SimpleRotationalEphemeris object of which rotation rate parameter is a property + * \param associatedBody Name of body of which parameter is a property. + */ + RotationRate( const boost::shared_ptr< ephemerides::SimpleRotationalEphemeris > rotationModel, + const std::string& associatedBody ): + EstimatableParameter< double >( constant_rotation_rate, associatedBody ), + rotationModel_( rotationModel ){ } + + //! Destructor + ~RotationRate( ) { } + + //! Get value of rotation rate. + /*! + * Get value of rotation rate. + * \return Value of rotation rate + */ + double getParameterValue( ) + { + return rotationModel_->getRotationRate( ); + } + + //! Reset value of rotation rate. + /*! + * Reset value of rotation rate. + * \param parameterValue New value of rotation rate + */ + void setParameterValue( double parameterValue ) + { + rotationModel_->resetRotationRate( parameterValue ); + } + + //! Function to retrieve the size of the parameter + /*! + * Function to retrieve the size of the parameter + * \return Size of parameter value, 1 for this parameter + */ + int getParameterSize( ) + { + return 1; + } + +protected: + +private: + + //! SimpleRotationalEphemeris object of which rotation rate parameter is a property + boost::shared_ptr< ephemerides::SimpleRotationalEphemeris > rotationModel_; +}; + +} // namespace estimatable_parameters + +} // namespace tudat + +#endif // TUDAT_CONSTANTROTATIONRATE_H diff --git a/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/constantRotationalOrientation.h b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/constantRotationalOrientation.h new file mode 100644 index 0000000000..a12a586ec9 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/constantRotationalOrientation.h @@ -0,0 +1,93 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_CONSTANTROTATIONALORIENTATION_H +#define TUDAT_CONSTANTROTATIONALORIENTATION_H + + +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h" +#include "Tudat/Astrodynamics/Ephemerides/simpleRotationalEphemeris.h" + +namespace tudat +{ + +namespace estimatable_parameters +{ + +//! Interface class for estimation of a body's constant pole position (right ascension and declination of north pole). +/*! + * Interface class for estimation of a body's constant pole position (right ascension and declination of north pole). + * Interfaces the estimation with the right ascension and declination Euler angle members of a SimpleRotationalEphemeris + * object + */ +class ConstantRotationalOrientation: public EstimatableParameter< Eigen::VectorXd > +{ + +public: + + //! Constructor + /*! + * Constructor + * \param rotationModel SimpleRotationalEphemeris object of which pole position is a property + * \param associatedBody Name of body of which parameter is a property. + */ + ConstantRotationalOrientation( + const boost::shared_ptr< ephemerides::SimpleRotationalEphemeris > rotationModel, + const std::string& associatedBody ): + EstimatableParameter< Eigen::VectorXd >( rotation_pole_position, associatedBody ), + rotationModel_( rotationModel ) { } + + //! Destructor + ~ConstantRotationalOrientation( ) { } + + //! Get value of pole right ascension and declination (in that order) + /*! + * Get value of pole right ascension and declination (in that order) + * \return Right ascension and declination (in that order) + */ + Eigen::VectorXd getParameterValue( ) + { + return rotationModel_->getInitialEulerAngles( ).segment( 0, 2 ); + } + + //! Reset value of pole right ascension and declination (in that order) + /*! + * Reset value of pole right ascension and declination (in that order) + * \param parameterValue New right ascension and declination (in that order) + */ + void setParameterValue( const Eigen::VectorXd parameterValue ) + { + rotationModel_->resetInitialPoleRightAscensionAndDeclination( + parameterValue.x( ), parameterValue.y( ) ); + } + + //! Function to retrieve the size of the parameter + /*! + * Function to retrieve the size of the parameter + * \return Size of parameter value, 2 for this parameter + */ + int getParameterSize( ) + { + return 2; + } + +protected: + +private: + + //! SimpleRotationalEphemeris object of which rotation rate parameter is a property + boost::shared_ptr< ephemerides::SimpleRotationalEphemeris > rotationModel_; +}; + +} // namespace estimatable_parameters + +} // namespace tudat + +#endif // TUDAT_CONSTANTROTATIONALORIENTATION_H diff --git a/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.cpp b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.cpp new file mode 100644 index 0000000000..21b734d817 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.cpp @@ -0,0 +1,93 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h" + +namespace tudat +{ + +namespace estimatable_parameters +{ + +//! Function to determine whether the given parameter represents an initial dynamical state, or a static parameter. +bool isParameterDynamicalPropertyInitialState( const EstimatebleParametersEnum parameterType ) +{ + bool flag; + switch( parameterType ) + { + case initial_body_state: + flag = true; + break; + default: + flag = false; + break; + } + return flag; +} + +//! Function to determine whether the given (non-dynamical) parameter is a double or vector parameter. +bool isDoubleParameter( const EstimatebleParametersEnum parameterType ) +{ + bool isDoubleParameter; + switch( parameterType ) + { + case gravitational_parameter: + isDoubleParameter = true; + break; + case constant_drag_coefficient: + isDoubleParameter = true; + break; + case radiation_pressure_coefficient: + isDoubleParameter = true; + break; + case spherical_harmonics_cosine_coefficient_block: + isDoubleParameter = false; + break; + case spherical_harmonics_sine_coefficient_block: + isDoubleParameter = false; + break; + case constant_rotation_rate: + isDoubleParameter = true; + break; + case rotation_pole_position: + isDoubleParameter = false; + break; + default: + throw std::runtime_error( "Error, parameter type " + boost::lexical_cast< std::string >( parameterType ) + + " not found when getting parameter type" ); + } + return isDoubleParameter; +} + +//! Function to determine whether the given (non-dynamical) parameter influences a body's orientation. +bool isParameterRotationMatrixProperty( const EstimatebleParametersEnum parameterType ) +{ + bool flag; + switch( parameterType ) + { + case constant_rotation_rate: + flag = true; + break; + case rotation_pole_position: + flag = true; + break; + default: + flag = false; + break; + } + return flag; +} + + + +} + +} + diff --git a/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h new file mode 100644 index 0000000000..ea4d1a5c7e --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h @@ -0,0 +1,544 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_ESTIMATABLEPARAMETERS_H +#define TUDAT_ESTIMATABLEPARAMETERS_H + +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include "Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h" + +namespace tudat +{ + +namespace estimatable_parameters +{ + +//! List of parameters that can be estimated by the orbit determination code. +enum EstimatebleParametersEnum +{ + initial_body_state, + gravitational_parameter, + constant_drag_coefficient, + radiation_pressure_coefficient, + spherical_harmonics_cosine_coefficient_block, + spherical_harmonics_sine_coefficient_block, + constant_rotation_rate, + rotation_pole_position +}; + +//! Function to determine whether the given parameter represents an initial dynamical state, or a static parameter. +/*! + * Function to determine whether the given parameter represents an initial dynamical state, or a static parameter. + * \param parameterType Parameter identifier. + * \return True if parameter is an initial dynamical state. + */ +bool isParameterDynamicalPropertyInitialState( const EstimatebleParametersEnum parameterType ); + +//! Function to determine whether the given (non-dynamical) parameter is a double or vector parameter. +/*! + * Function to determine whether the given (non-dynamical) parameter is a double or vector parameter. + * \param parameterType Parameter identifier. + * \return True if parameter is a double parameter. + */ +bool isDoubleParameter( const EstimatebleParametersEnum parameterType ); + +//! Function to determine whether the given (non-dynamical) parameter influences a body's orientation. +/*! + * Function to determine whether the given (non-dynamical) parameter influences a body's orientation. + * \param parameterType Parameter identifier. + * \return True if parameter is a property of rotation model + */ +bool isParameterRotationMatrixProperty( const EstimatebleParametersEnum parameterType ); + + +//! Typedef for full parameter identifier. +typedef std::pair< EstimatebleParametersEnum, std::pair< std::string, std::string > > EstimatebleParameterIdentifier; + + + +//! Base class for a parameter that is to be estimated. +/*! + * Base class for a parameter that is to be estimated. A separate derived class is to be made for each type of parameter + * (i.e. gravitational parameter, initial translational state, etc. ). + */ +template< typename ParameterType > +class EstimatableParameter +{ + +public: + //! Constructor. + /*! + * Constructor taking parameter name and associated body. All parameters are identified by a these two variables. + * Any additional information that may be required for uniquely defining a parameter is to be defined in the derived class. + * \param parameterName Enum value defining the type of the parameter. + * \param associatedBody Name of body associated with patameters + * \param pointOnBodyId Reference point on body associated with parameter (empty by default). + */ + EstimatableParameter( const EstimatebleParametersEnum parameterName, + const std::string& associatedBody, + const std::string& pointOnBodyId = "" ): + parameterName_( std::make_pair( parameterName, std::make_pair( associatedBody, pointOnBodyId ) ) ){ } + + //! Virtual destructor. + virtual ~EstimatableParameter( ) { } + + //! Pure virtual function to retrieve the value of the parameter + /*! + * Pure virtual function to retrieve the value of the parameter + * \return Current value of parameter. + */ + virtual ParameterType getParameterValue( ) = 0; + + //! Pure virtual function to (re)set the value of the parameter. + /*! + * Pure virtual function to (re)set the value of the parameter. + * \param parameterValue to which the parameter is to be set. + */ + virtual void setParameterValue( const ParameterType parameterValue ) = 0; + + //! Function to retrieve the type and associated body of the parameter. + /*! + * Function to retrieve the type and associated body of the parameter. + * \return Identifier of parameter as a pair of parameter type and body of which parameter is a property. + */ + EstimatebleParameterIdentifier getParameterName( ) { return parameterName_; } + + //! Function to retrieve the size of the parameter + /*! + * Pure virtual function to retrieve the size of the parameter (i.e. 1 for double parameters) + * \return Size of parameter value. + */ + virtual int getParameterSize( ) = 0; + + //! Function to return additional identifier for parameter + /*! + * Function to return additional identifier for parameter, beyond information stored in parameterName_, default + * none. + * \return Additional identifier for parameter (default empty string). + */ + virtual std::string getSecondaryIdentifier( ) + { + return ""; + } + +protected: + + //! Identifier of parameter. + EstimatebleParameterIdentifier parameterName_; +}; + +//! Container class for all parameters that are to be estimated. +/*! + * Container class for all parameters that are to be estimated. Class is templated with the scalar type used for the + * estimation of any initial dynamical states that may be included + */ +template< typename InitialStateParameterType = double > +class EstimatableParameterSet +{ +public: + + //! Constructor of parameter set. + /*! + * Constructor of parameter set. + * \param estimatedDoubleParameters List of double parameters that are estimated. + * \param estimatedVectorParameters List of vector parameters that are estimated. + * \param estimateInitialStateParameters List of initial dynamical states that are to be estimated. + */ + EstimatableParameterSet( + const std::vector< boost::shared_ptr< EstimatableParameter< double > > >& estimatedDoubleParameters, + const std::vector< boost::shared_ptr< EstimatableParameter< Eigen::VectorXd > > >& estimatedVectorParameters, + const std::vector< boost::shared_ptr< EstimatableParameter< Eigen::Matrix + < InitialStateParameterType, Eigen::Dynamic, 1 > > > >& estimateInitialStateParameters = + ( std::vector< boost::shared_ptr< EstimatableParameter< Eigen::Matrix + < InitialStateParameterType, Eigen::Dynamic, 1 > > > >( ) ) ): + estimatedDoubleParameters_( estimatedDoubleParameters ), estimatedVectorParameters_( estimatedVectorParameters ), + estimateInitialStateParameters_( estimateInitialStateParameters ) + { + // Initialize total number of parameters to 0. + estimatedParameterSetSize_ = 0; + initialDynamicalStateParameterSize_ = 0; + + // Iterate over all double parameters and add to parameter size. + for( unsigned int i = 0; i < estimateInitialStateParameters_.size( ); i++ ) + { + initialStateParameters_[ estimatedParameterSetSize_ ] = estimateInitialStateParameters_[ i ]; + parameterIndices_.push_back( std::make_pair( estimatedParameterSetSize_, + estimateInitialStateParameters_[ i ]->getParameterSize( ) ) ); + estimatedParameterSetSize_ += estimateInitialStateParameters_[ i ]->getParameterSize( ); + initialDynamicalStateParameterSize_ += estimateInitialStateParameters_[ i ]->getParameterSize( ); + } + + // Iterate over all double parameters and add to parameter size and set indices in parameterIndices_ + for( unsigned int i = 0; i < estimatedDoubleParameters_.size( ); i++ ) + { + doubleParameters_[ estimatedParameterSetSize_ ] = estimatedDoubleParameters_[ i ]; + parameterIndices_.push_back( std::make_pair( estimatedParameterSetSize_, 1 ) ); + estimatedParameterSetSize_++; + } + + // Iterate over all vector parameter, add to total number of parameters and set indices in parameterIndices_ + for( unsigned int i = 0; i < estimatedVectorParameters_.size( ); i++ ) + { + vectorParameters_[ estimatedParameterSetSize_ ] = estimatedVectorParameters_[ i ]; + parameterIndices_.push_back( std::make_pair( estimatedParameterSetSize_, + estimatedVectorParameters_[ i ]->getParameterSize( ) ) ); + estimatedParameterSetSize_ += estimatedVectorParameters_[ i ]->getParameterSize( ); + } + + totalParameterSetSize_ = estimatedParameterSetSize_; + } + + //! Function to return the total number of parameter values (including consider parameters) + /*! + * Function to return the total number of parameter values (including consider parameters) + * \return Size of parameter vector (including consider parameters) + */ + int getParameterSetSize( ) + { + return totalParameterSetSize_; + } + + //! Function to return the total number of parameter values (excluding consider parameters). + /*! + * Function to return the total number of parameter values (excluding consider parameters) + * \return Size of parameter vector (excluding consider parameters) + */ + int getEstimatedParameterSetSize( ) + { + return estimatedParameterSetSize_; + } + + //! Function to return the total number of initial state values that are estimated. + /*! + * Function to return the total number of initial state values that are estimated. + * \return Function to return the total number of initial state values that are estimated. + */ + int getInitialDynamicalStateParameterSize( ) + { + return initialDynamicalStateParameterSize_; + } + + //! Function that returns a vector containing all current parameter values + /*! + * Function that returns a vector containing all current parameter values. The total vector starts with the initial + * state parameters, followed by the double and vector parameters, respectively. + * Initial state, double and vector parameter values are concatenated in the order in which they are set in the + * estimateInitialStateParameters_, doubleParameters_ and vectorParameters_ members. + * \return Vector containing all parameter values + */ + template< typename ParameterScalar > + Eigen::Matrix< ParameterScalar, Eigen::Dynamic, 1 > getFullParameterValues( ) + { + Eigen::Matrix< ParameterScalar, Eigen::Dynamic, 1 > parameterValues = + Eigen::Matrix< ParameterScalar, Eigen::Dynamic, 1 >::Zero( totalParameterSetSize_ ); + + int currentStartIndex = 0; + + // Retrieve initial state parameter values. + for( unsigned int i = 0; i < estimateInitialStateParameters_.size( ); i++ ) + { + parameterValues.segment( currentStartIndex, estimateInitialStateParameters_[ i ]->getParameterSize( ) ) = + estimateInitialStateParameters_[ i ]->getParameterValue( ).template cast< ParameterScalar >( ); + currentStartIndex += estimateInitialStateParameters_[ i ]->getParameterSize( ); + } + + // Retrieve double parameter values. + for( unsigned int i = 0; i < estimatedDoubleParameters_.size( ); i++ ) + { + parameterValues( currentStartIndex ) = static_cast< ParameterScalar >( + estimatedDoubleParameters_[ i ]->getParameterValue( ) ); + currentStartIndex++; + } + + // Retrieve vector parameter values. + for( unsigned int i = 0; i < estimatedVectorParameters_.size( ); i++ ) + { + parameterValues.segment( currentStartIndex, estimatedVectorParameters_[ i ]->getParameterSize( ) ) = + estimatedVectorParameters_[ i ]->getParameterValue( ).template cast< ParameterScalar >( ); + currentStartIndex += estimatedVectorParameters_[ i ]->getParameterSize( ); + } + + return parameterValues; + } + + //! Function to reset all parameter values. + /*! + * Function to reset all parameter values. + * \param newParameterValues New parameter values. Order of values in vector must be same order as return vector of getFullParameterValues + */ + template< typename ParameterScalar > + void resetParameterValues( const Eigen::Matrix< ParameterScalar, Eigen::Dynamic, 1 >& newParameterValues ) + { + // Check input consistency + if( newParameterValues.rows( ) != totalParameterSetSize_ ) + { + throw std::runtime_error( "Error when resetting parameters of parameter set, given vector has size " + + boost::lexical_cast< std::string >( newParameterValues.rows( ) ) + + ", while internal size is " + boost::lexical_cast< std::string >( totalParameterSetSize_ ) ); + } + else + { + int currentStartIndex = 0; + + for( unsigned int i = 0; i < estimateInitialStateParameters_.size( ); i++ ) + { + estimateInitialStateParameters_[ i ]->setParameterValue( + newParameterValues.segment( currentStartIndex, estimateInitialStateParameters_[ i ]->getParameterSize( ) ). + template cast< InitialStateParameterType >( ) ); + currentStartIndex += estimateInitialStateParameters_[ i ]->getParameterSize( ); + } + + // Set double parameter values. + for( unsigned int i = 0; i < estimatedDoubleParameters_.size( ); i++ ) + { + estimatedDoubleParameters_[ i ]->setParameterValue( static_cast< double >( newParameterValues( currentStartIndex ) ) ); + currentStartIndex++; + } + + // Set vector parameter values. + + for( unsigned int i = 0; i < estimatedVectorParameters_.size( ); i++ ) + { + estimatedVectorParameters_[ i ]->setParameterValue( + newParameterValues.segment( currentStartIndex, estimatedVectorParameters_[ i ]->getParameterSize( ) ). + template cast< double >( ) ); + + currentStartIndex += estimatedVectorParameters_[ i ]->getParameterSize( ); + } + } + } + + //! Function to retrieve double parameter objects. + /*! + * Function to retrieve double parameter objects. + * \return Vector containing all double parameter objects + */ + std::map< int, boost::shared_ptr< EstimatableParameter< double > > > getDoubleParameters( ) + { + return doubleParameters_; + } + + //! Function to retrieve vector parameter objects. + /*! + * Function to retrieve vector parameter objects. + * \return Vector containing all vector parameter objects + */ + std::map< int, boost::shared_ptr< EstimatableParameter< Eigen::VectorXd > > > getVectorParameters( ) + { + return vectorParameters_; + } + + std::vector< boost::shared_ptr< EstimatableParameter< double > > > getEstimatedDoubleParameters( ) + { + return estimatedDoubleParameters_; + } + + std::vector< boost::shared_ptr< EstimatableParameter< Eigen::VectorXd > > > getEstimatedVectorParameters( ) + { + return estimatedVectorParameters_; + } + + //! Function to get list of initial dynamical states that are to be estimated. + //! + /*! + * Function to get list of initial dynamical states that are to be estimated. + * \return List of initial dynamical states that are to be estimated. + */ + std::vector< boost::shared_ptr< EstimatableParameter< Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 > > > > + getEstimatedInitialStateParameters( ) + { + return estimateInitialStateParameters_; + } + + //! Function to retrieve list of start indices and sizes (map keys) of estimated parameters. + /*! + * Function to retrieve list of start indices and sizes (map keys) of estimated parameters. + * \return List of start indices and sizes (map keys) of estimated parameters. + */ + std::vector< std::pair< int, int > > getParametersIndices( ) + { + return parameterIndices_; + } + + +protected: + + //! Total size of all initial dynamical states that is to be estimated. + int initialDynamicalStateParameterSize_; + + //! Total number of parameter values (including currently non yet implemented consider parameters). + int totalParameterSetSize_; + + //! Total number of estimated parameter values (excluding currently non yet implemented consider parameters). + int estimatedParameterSetSize_; + + //! List of start indices and sizes (map keys) of estimated parameters. + /*! + * List of start indices and sizes (map keys) of estimated parameters, in order of vector + * estimateInitialStateParameters_, followed by estimatedDoubleParameters_, followed by estimatedVectorParameters_. + */ + std::vector< std::pair< int, int > > parameterIndices_; + + //! List of double parameters that are to be estimated. + std::vector< boost::shared_ptr< EstimatableParameter< double > > > estimatedDoubleParameters_; + + //! List of vector parameters that are to be estimated. + std::vector< boost::shared_ptr< EstimatableParameter< Eigen::VectorXd > > > estimatedVectorParameters_; + + //! List of initial dynamical states that are to be estimated. + std::vector< boost::shared_ptr< EstimatableParameter< Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 > > > > + estimateInitialStateParameters_; + + //! Map of double parameters that are to be estimated, with start index in total parameter vector as key. + std::map< int, boost::shared_ptr< EstimatableParameter< double > > > doubleParameters_; + + //! Map of vector parameters that are to be estimated, with start index in total parameter vector as key. + std::map< int, boost::shared_ptr< EstimatableParameter< Eigen::VectorXd > > > vectorParameters_; + + //! Map of initial dynamical states that are to be estimated, with start index in total parameter vector as key. + std::map< int, boost::shared_ptr< + EstimatableParameter< Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 > > > > initialStateParameters_; + +}; + +//! Function to get the list of names of bodies for which initial translational dynamical state is estimated. +/*! + * Function to get the list of names of bodies for which initial translational dynamical state is estimated. + * \param estimatableParameters Object containing all parameters that are to be estimated. + * \return List of names of bodies for which initial state is estimated. + */ +template< typename InitialStateParameterType > +std::vector< std::string > getListOfBodiesWithTranslationalStateToEstimate( + const boost::shared_ptr< EstimatableParameterSet< InitialStateParameterType > > estimatableParameters ) +{ + std::vector< std::string > bodiesToEstimate; + + // Retrieve initial dynamical parameters. + std::vector< boost::shared_ptr< EstimatableParameter< + Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 > > > > initialDynamicalParameters = + estimatableParameters->getEstimatedInitialStateParameters( ); + + // Iterate over list of bodies of which the partials of the accelerations acting on them are required. + for( unsigned int i = 0; i < initialDynamicalParameters.size( ); i++ ) + { + if( initialDynamicalParameters.at( i )->getParameterName( ).first == initial_body_state ) + { + bodiesToEstimate.push_back( initialDynamicalParameters.at( i )->getParameterName( ).second.first ); + } + } + + return bodiesToEstimate; +} + +template< typename InitialStateParameterType > +std::vector< std::string > getListOfBodiesToEstimate( + const boost::shared_ptr< EstimatableParameterSet< InitialStateParameterType > > estimatableParameters ) +{ + std::vector< std::string > bodiesToEstimate; + + std::vector< boost::shared_ptr< EstimatableParameter< + Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 > > > > initialDynamicalParameters = + estimatableParameters->getEstimatedInitialStateParameters( ); + + // Iterate over list of bodies of which the partials of the accelerations acting on them are required. + for( unsigned int i = 0; i < initialDynamicalParameters.size( ); i++ ) + { + if( ( initialDynamicalParameters.at( i )->getParameterName( ).first == initial_body_state ) ) + { + bodiesToEstimate.push_back( initialDynamicalParameters.at( i )->getParameterName( ).second.first ); + } + } + + return bodiesToEstimate; +} + +//! Function to get the complete list of initial dynamical states that are to be estimated, sorted by dynamics type. +/*! + * Function to get the complete list of initial dynamical states that are to be estimated, sorted by dynamics type. + * \param estimatableParameters Object containing all parameters that are to be estimated. + * \return Map containing dynamics type (key) and vector of pairs: list of bodies (first in pair) with reference point + * identifier (second in pair; empty if not relevant) for which given dynamics type is estimated. + */ +template< typename InitialStateParameterType > +std::map< propagators::IntegratedStateType, std::vector< std::pair< std::string, std::string > > > +getListOfInitialDynamicalStateParametersEstimate( + const boost::shared_ptr< EstimatableParameterSet< InitialStateParameterType > > estimatableParameters ) +{ + // Retrieve initial dynamical parameters. + std::vector< boost::shared_ptr< EstimatableParameter< + Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 > > > > initialDynamicalParameters = + estimatableParameters->getEstimatedInitialStateParameters( ); + + std::map< propagators::IntegratedStateType, std::vector< std::pair< std::string, std::string > > > initialDynamicalStateParametersEstimate; + // Iterate over list of bodies of which the partials of the accelerations acting on them are required. + for( unsigned int i = 0; i < initialDynamicalParameters.size( ); i++ ) + { + if( initialDynamicalParameters.at( i )->getParameterName( ).first == initial_body_state ) + { + initialDynamicalStateParametersEstimate[ propagators::transational_state ].push_back( + initialDynamicalParameters.at( i )->getParameterName( ).second ); + } + } + + return initialDynamicalStateParametersEstimate; +} + +//! Function to get initial state vector of estimated dynamical states. +/*! + * Function to get initial state vector of estimated dynamical states (i.e. presently estimated state at propagation + * start time. + * \param estimatableParameters Object containing all parameters that are to be estimated. + * \return State vector of estimated dynamics at propagation start time. + */ +template< typename InitialStateParameterType = double > +Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 > getInitialStateVectorOfBodiesToEstimate( + const boost::shared_ptr< EstimatableParameterSet< InitialStateParameterType > > estimatableParameters ) +{ + // Retrieve initial dynamical parameters. + std::vector< boost::shared_ptr< EstimatableParameter< + Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 > > > > initialDynamicalParameters = + estimatableParameters->getEstimatedInitialStateParameters( ); + + // Initialize state vector. + Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 > initialStateVector = + Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 >::Zero( + estimatableParameters->getInitialDynamicalStateParameterSize( ), 1 ); + + int vectorSize = 0; + // Iterate over list of bodies of which the partials of the accelerations acting on them are required. + for( unsigned int i = 0; i < initialDynamicalParameters.size( ); i++ ) + { + if( isParameterDynamicalPropertyInitialState( initialDynamicalParameters.at( i )->getParameterName( ).first ) ) + { + int currentParameterSize = initialDynamicalParameters.at( i )->getParameterSize( ); + initialStateVector.block( vectorSize, 0, currentParameterSize, 1 ) = initialDynamicalParameters.at( i )->getParameterValue( ); + + vectorSize += currentParameterSize; + } + } + + return initialStateVector.block( 0, 0, vectorSize, 1 ); +} + +} // namespace estimatable_parameters + +} // namespace tudat + +#endif // TUDAT_ESTIMATABLEPARAMETERS_H diff --git a/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/gravitationalParameter.h b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/gravitationalParameter.h new file mode 100644 index 0000000000..de5da0be94 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/gravitationalParameter.h @@ -0,0 +1,87 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_GRAVITATIONALPARAMETER_H +#define TUDAT_GRAVITATIONALPARAMETER_H + +#include "Tudat/Astrodynamics/Gravitation/gravityFieldModel.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h" + +namespace tudat +{ + +namespace estimatable_parameters +{ + +//! Interface class for the estimation of a gravitational parameter +class GravitationalParameter: public EstimatableParameter< double > +{ + +public: + + //! Constructor + /*! + * Constructor + * \param gravityFieldModel Gravity field object containing the gravitational parameter to be estimated. + * \param associatedBody Name of body containing the gravityFieldModel object + */ + GravitationalParameter( + const boost::shared_ptr< gravitation::GravityFieldModel > gravityFieldModel, const std::string& associatedBody ): + EstimatableParameter< double >( gravitational_parameter, associatedBody ), + gravityFieldModel_( gravityFieldModel ){ } + + //! Destructor + ~GravitationalParameter( ) { } + + //! Function to get the current value of the gravitational parameter that is to be estimated. + /*! + * Function to get the current value of the gravitational parameter that is to be estimated. + * \return Current value of the gravitational parameter that is to be estimated. + */ + double getParameterValue( ) + { + return gravityFieldModel_->getGravitationalParameter( ); + } + + //! Function to reset the value of the gravitational parameter that is to be estimated. + /*! + * Function to reset the value of the gravitational parameter that is to be estimated. + * \param parameterValue New value of the gravitational parameter that is to be estimated. + */ + void setParameterValue( double parameterValue ) + { + gravityFieldModel_->resetGravitationalParameter( parameterValue ); + } + + //! Function to retrieve the size of the parameter (always 1). + /*! + * Function to retrieve the size of the parameter (always 1). + * \return Size of parameter value (always 1). + */ + int getParameterSize( ) + { + return 1; + } + +protected: + +private: + + //! Gravity field object containing the gravitational parameter to be estimated. + boost::shared_ptr< gravitation::GravityFieldModel > gravityFieldModel_; + +}; + +} // namespace estimatable_parameters + +} // namespace tudat + + +#endif // TUDAT_GRAVITATIONALPARAMETER_H diff --git a/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/initialTranslationalState.h b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/initialTranslationalState.h new file mode 100644 index 0000000000..1bdbd2f294 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/initialTranslationalState.h @@ -0,0 +1,141 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_INITIALTRANSLATIONALSTATE_H +#define TUDAT_INITIALTRANSLATIONALSTATE_H + +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h" + +namespace tudat +{ + +namespace estimatable_parameters +{ + +//! Interface class for the estimation of an initial translational state. +template< typename InitialStateParameterType = double > +class InitialTranslationalStateParameter: public EstimatableParameter< Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 > > +{ +public: + + //! Constructor, sets initial value of translational state. + /*! + * Constructor, sets initial value of translational state. + * \param associatedBody Body for which initial state is to be estimated. + * \param initialTranslationalState Current value of initial state (w.r.t. centralBody) + * \param centralBody Body w.r.t. which the initial state is to be estimated. + * \param frameOrientation Orientation of the frame in which the state is defined. + */ + InitialTranslationalStateParameter( + const std::string& associatedBody, + const Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 >& initialTranslationalState, + const std::string& centralBody = "SSB", const std::string& frameOrientation = "ECLIPJ2000" ): + EstimatableParameter< Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 > >( initial_body_state, associatedBody ), + initialTranslationalState_( initialTranslationalState ), centralBody_( centralBody ), frameOrientation_( frameOrientation ) + { } + + //! Function to get the current value of initial state w.r.t. centralBody. + /*! + * Function to get the current value of initial state w.r.t. centralBody. + * \return The current value of initial state w.r.t. centralBody. + */ + Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 > getParameterValue( ) + { + return initialTranslationalState_; + } + + //! Function to reset the current value of initial state w.r.t. centralBody. + /*! + * Function to reset the current value of initial state w.r.t. centralBody. + * \param parameterValue The new value of initial state w.r.t. centralBody. + */ + void setParameterValue( Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 > parameterValue ) + { + initialTranslationalState_ = parameterValue; + } + + //! Function to retrieve the size of the parameter (always 6). + /*! + * Function to retrieve the size of the parameter (always 6). + * \return Size of parameter value (always 6). + */ + int getParameterSize( ) + { + return 6; + } + + //! Function to get the name of the body w.r.t. which the initial state is to be estimated. + /*! + * Function to get the name of the body w.r.t. which the initial state is to be estimated. + * \return Name of the body w.r.t. which the initial state is to be estimated. + */ + std::string getCentralBody( ) + { + return centralBody_; + } + +private: + + //! Current value of initial state (w.r.t. centralBody) + Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 > initialTranslationalState_; + + //! Body w.r.t. which the initial state is to be estimated. + std::string centralBody_; + + //! Orientation of the frame in which the state is defined. + std::string frameOrientation_; + +}; + +//! Function to retrieve the size of the estimatable parameter set. +/*! + * Function to retrieve the size of the estimatable parameter set. + * \param estimatableParameterSet Set of estimatable parameters. + * \return Size of parameter set. + */ +template< typename InitialStateParameterType = double > +int getSingleArcParameterSetSize( + boost::shared_ptr< EstimatableParameterSet< InitialStateParameterType > > estimatableParameterSet ) +{ + int totalParameterSetSize = estimatableParameterSet->getEstimatedParameterSetSize( ); + std::vector< boost::shared_ptr< EstimatableParameter< Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 > > > > + initialStateParameters = estimatableParameterSet->getEstimatedInitialStateParameters( ); + + for( unsigned int i = 0; i < initialStateParameters.size( ); i++ ) + { + if( ( initialStateParameters.at( i )->getParameterName( ).first != initial_body_state ) ) + { + throw std::runtime_error( "Error when getting single arc paramater vector, did not recognize initial state parameter " + + boost::lexical_cast< std::string >( initialStateParameters.at( i )->getParameterName( ).first ) ); + } + } + return totalParameterSetSize; +} + +//! Function to retrieve the size of the dynamical state. +/*! + * Function to retrieve the size of the dynamical state. + * \param estimatableParameterSet Set of estimatable parameters. + * \return Size of the initial dynamical state. + */ +template< typename InitialStateParameterType = double > +int getSingleArcInitialDynamicalStateParameterSetSize( + boost::shared_ptr< EstimatableParameterSet< InitialStateParameterType > > estimatableParameterSet ) +{ + return getSingleArcParameterSetSize( estimatableParameterSet ) - + ( estimatableParameterSet->getEstimatedParameterSetSize( ) - estimatableParameterSet->getInitialDynamicalStateParameterSize( ) ); +} + + +} // namespace estimatable_parameters + +} // namespace tudat + +#endif // TUDAT_INITIALTRANSLATIONALSTATE_H diff --git a/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/radiationPressureCoefficient.h b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/radiationPressureCoefficient.h new file mode 100644 index 0000000000..f09bfca33e --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/radiationPressureCoefficient.h @@ -0,0 +1,85 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_RADIATIONPRESSURECOEFFICIENT_H +#define TUDAT_RADIATIONPRESSURECOEFFICIENT_H + +#include + +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h" +#include "Tudat/Astrodynamics/ElectroMagnetism/radiationPressureInterface.h" + +namespace tudat +{ + +namespace estimatable_parameters +{ + +//! Interface class for the estimation of a radiation pressure coefficient +class RadiationPressureCoefficient: public EstimatableParameter< double > +{ + +public: + //! Constructor. + /*! + * Constructor + * \param radiationPressureInterface Object containing the radiation pressure coefficient to be estimated. + * \param associatedBody Name of body containing the radiationPressureInterface object + */ + RadiationPressureCoefficient( + boost::shared_ptr< electro_magnetism::RadiationPressureInterface > radiationPressureInterface, + std::string& associatedBody ): + EstimatableParameter< double >( radiation_pressure_coefficient, associatedBody ), + radiationPressureInterface_( radiationPressureInterface ) + { } + + //! Destructor. + ~RadiationPressureCoefficient( ) { } + + //! Function to get the current value of the radiation pressure coefficient that is to be estimated. + /*! + * Function to get the current value of the radiation pressure coefficient that is to be estimated. + * \return Current value of the radiation pressure coefficient that is to be estimated. + */ + double getParameterValue( ) + { + return radiationPressureInterface_->getRadiationPressureCoefficient( ); + } + + //! Function to reset the value of the radiation pressure coefficient that is to be estimated. + /*! + * Function to reset the value of the radiation pressure coefficient that is to be estimated. + * \param parameterValue New value of the radiation pressure coefficient that is to be estimated. + */ + void setParameterValue( double parameterValue ) + { + radiationPressureInterface_->resetRadiationPressureCoefficient( parameterValue ); + } + + //! Function to retrieve the size of the parameter (always 1). + /*! + * Function to retrieve the size of the parameter (always 1). + * \return Size of parameter value (always 1). + */ + int getParameterSize( ){ return 1; } + +protected: + +private: + + //! Object containing the radiation pressure coefficient to be estimated. + boost::shared_ptr< electro_magnetism::RadiationPressureInterface > radiationPressureInterface_; +}; + +} // namespace estimatable_parameters + +} // namespace tudat + +#endif // TUDAT_RADIATIONPRESSURECOEFFICIENT_H diff --git a/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicCosineCoefficients.cpp b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicCosineCoefficients.cpp new file mode 100644 index 0000000000..df8c56e312 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicCosineCoefficients.cpp @@ -0,0 +1,50 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicCosineCoefficients.h" + +namespace tudat +{ + +namespace estimatable_parameters +{ + +//! Function to retrieve the current values of the cosine coefficients that are to be estimated. +Eigen::VectorXd SphericalHarmonicsCosineCoefficients::getParameterValue( ) +{ + Eigen::VectorXd parameterVector = Eigen::VectorXd::Zero( parameterSize_ ); + + Eigen::MatrixXd coefficientBlock = getCosineCoefficients_( ); + + for( unsigned int i = 0; i < blockIndices_.size( ); i++ ) + { + parameterVector( i ) = coefficientBlock( blockIndices_.at( i ).first, blockIndices_.at( i ).second ); + } + return parameterVector; + +} + +//! Function to reset the cosine coefficients that are to be estimated. +void SphericalHarmonicsCosineCoefficients::setParameterValue( const Eigen::VectorXd parameterValue ) +{ + Eigen::MatrixXd coefficients = getCosineCoefficients_( ); + + for( unsigned int i = 0; i < blockIndices_.size( ); i++ ) + { + coefficients( blockIndices_.at( i ).first, blockIndices_.at( i ).second ) = parameterValue( i ); + } + setCosineCoefficients_( coefficients ); +} + +} + +} + + diff --git a/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicCosineCoefficients.h b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicCosineCoefficients.h new file mode 100644 index 0000000000..93cc33e93f --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicCosineCoefficients.h @@ -0,0 +1,121 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_SPHERICALHARMONICCOSINECOEFFICIENTS_H +#define TUDAT_SPHERICALHARMONICCOSINECOEFFICIENTS_H + +#include + +#include + +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h" + +namespace tudat +{ + +namespace estimatable_parameters +{ + +//! Interface class for estimation of spherical harmonic gravity field cosine coefficients +/*! + * Interface class for estimation of spherical harmonic gravity field cosine coefficients. Interfaces the estimation with the + * member coefficients of an instance of the SphericalHarmonicsGravityField (or nominal coefficients of + * TimeDependentSphericalHarmonicsGravityField). + */ +class SphericalHarmonicsCosineCoefficients: public EstimatableParameter< Eigen::VectorXd > +{ + +public: + + //! Constructor + /*! + * Constructor + * \param getCosineCoefficients Function to retrieve the full set of sine coefficients, of which a subset is to be + * estimated. + * \param setCosineCoefficients Function to reset the full set of sine coefficients, of which a subset is to be + * estimated. + * \param blockIndices List of cosine coefficient indices which are to be estimated (first and second + * are degree and order for each vector entry). + * \param associatedBody Name of body for which cosine coefficients are to be estimated. + */ + SphericalHarmonicsCosineCoefficients( + const boost::function< Eigen::MatrixXd( ) > getCosineCoefficients, + const boost::function< void( Eigen::MatrixXd ) > setCosineCoefficients, + const std::vector< std::pair< int, int > > blockIndices, + const std::string& associatedBody ): + EstimatableParameter< Eigen::VectorXd >( spherical_harmonics_cosine_coefficient_block, associatedBody ), + getCosineCoefficients_( getCosineCoefficients ), setCosineCoefficients_( setCosineCoefficients ), + blockIndices_( blockIndices ) + { + parameterSize_ = blockIndices_.size( ); + } + + //! Destructor + ~SphericalHarmonicsCosineCoefficients( ) { } + + //! Function to retrieve the current values of the cosine coefficients that are to be estimated. + /*! + * Function to retrieve the current values of the cosine coefficients that are to be estimated. The order of the entries + * in this vector is the same as in the blockIndices_ member vector. + * \return List of values for cosine coefficients that are to be estimated. + */ + Eigen::VectorXd getParameterValue( ); + + //! Function to reset the cosine coefficients that are to be estimated. + /*! + * Function to reset the cosine coefficients that are to be estimated. The order of the entries in this vector + * is the same as in the blockIndices_ member vector. + * \param parameterValue List of new values for cosine coefficients that are to be estimated. + */ + void setParameterValue( const Eigen::VectorXd parameterValue ); + + //! Function to retrieve the size of the parameter + /*! + * Function to retrieve the size of the parameter, equal to the length of the blcokIndices_ member vector, i.e. the + * number of coefficients that are to be estimated. + * \return Size of parameter value. + */ + int getParameterSize( ) { return parameterSize_; } + + //! Function to retrieve the list of cosine coefficient indices which are to be estimated. + /*! + * Function to retrieve the list of cosine coefficient indices which are to be estimated (first and second are degree + * and order for each vector entry). + * \return List of cosine coefficient indices which are to be estimated. + */ + std::vector< std::pair< int, int > > getBlockIndices( ){ return blockIndices_; } + + +protected: + +private: + + + //! Function to retrieve the full set of sine coefficients, of which a subset is to be estimated. + boost::function< Eigen::MatrixXd( ) > getCosineCoefficients_; + + //! Function to reset the full set of sine coefficients, of which a subset is to be estimated. + boost::function< void( Eigen::MatrixXd ) > setCosineCoefficients_; + + //! List of cosine coefficient indices which are to be estimated + /*! + * List of cosine coefficient indices which are to be estimated (first and second are degree and order for + * each vector entry). + */ + std::vector< std::pair< int, int > > blockIndices_; + + int parameterSize_; +}; + +} // namespace estimatable_parameters + +} // namespace tudat + +#endif // TUDAT_SPHERICALHARMONICCOSINECOEFFICIENTS_H diff --git a/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicSineCoefficients.cpp b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicSineCoefficients.cpp new file mode 100644 index 0000000000..7b87a66888 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicSineCoefficients.cpp @@ -0,0 +1,50 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicSineCoefficients.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicCosineCoefficients.h" + +namespace tudat +{ + +namespace estimatable_parameters +{ + +//! Function to retrieve the current values of the sine coefficients that are to be estimated. +Eigen::VectorXd SphericalHarmonicsSineCoefficients::getParameterValue( ) +{ + Eigen::VectorXd parameterVector = Eigen::VectorXd::Zero( parameterSize_ ); + + Eigen::MatrixXd coefficientBlock = getSineCoefficients_( ); + + for( unsigned int i = 0; i < blockIndices_.size( ); i++ ) + { + parameterVector( i ) = coefficientBlock( blockIndices_.at( i ).first, blockIndices_.at( i ).second ); + } + return parameterVector; + +} + +//! Function to reset the sine coefficients that are to be estimated. +void SphericalHarmonicsSineCoefficients::setParameterValue( const Eigen::VectorXd parameterValue ) +{ + Eigen::MatrixXd coefficients = getSineCoefficients_( ); + + for( unsigned int i = 0; i < blockIndices_.size( ); i++ ) + { + coefficients( blockIndices_.at( i ).first, blockIndices_.at( i ).second ) = parameterValue( i ); + } + setSineCoefficients_( coefficients ); +} + + +} + +} diff --git a/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicSineCoefficients.h b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicSineCoefficients.h new file mode 100644 index 0000000000..ee42f0c5bc --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicSineCoefficients.h @@ -0,0 +1,124 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_SPHERICALHARMONICSINECOEFFICIENTS_H +#define TUDAT_SPHERICALHARMONICSINECOEFFICIENTS_H + +#include + +#include + +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h" + +namespace tudat +{ + +namespace estimatable_parameters +{ + +//! Interface class for estimation of spherical harmonic gravity field sine coefficients +/*! + * Interface class for estimation of spherical harmonic gravity field sine coefficients. Interfaces the estimation with the + * member coefficients of an instance of the SphericalHarmonicsGravityField (or nominal coefficients of + * TimeDependentSphericalHarmonicsGravityField). + */ +class SphericalHarmonicsSineCoefficients: public EstimatableParameter< Eigen::VectorXd > +{ + +public: + + //! Constructor + /*! + * Constructor + * \param getSineCoefficients Function to retrieve the full set of sine coefficients, of which a subset is to be + * estimated. + * \param setSineCoefficients Function to reset the full set of sine coefficients, of which a subset is to be + * estimated. + * \param blockIndices List of sine coefficient indices which are to be estimated (first and second + * are degree and order for each vector entry). + * \param associatedBody Name of body for which sine coefficients are to be estimated. + */ + SphericalHarmonicsSineCoefficients( + const boost::function< Eigen::MatrixXd( ) > getSineCoefficients, + const boost::function< void( Eigen::MatrixXd ) > setSineCoefficients, + const std::vector< std::pair< int, int > >& blockIndices, + const std::string& associatedBody ): + EstimatableParameter< Eigen::VectorXd >( spherical_harmonics_sine_coefficient_block, associatedBody ), + getSineCoefficients_( getSineCoefficients ), setSineCoefficients_( setSineCoefficients ), + blockIndices_( blockIndices ) + { + parameterSize_ = blockIndices_.size( ); + } + + //! Destructor + ~SphericalHarmonicsSineCoefficients( ) { } + + //! Function to retrieve the current values of the sine coefficients that are to be estimated. + /*! + * Function to retrieve the current values of the sine coefficients that are to be estimated. The order of the entries + * in this vector is the same as in the blockIndices_ member vector. + * \return List of values for sine coefficients that are to be estimated. + */ + Eigen::VectorXd getParameterValue( ); + + //! Function to reset the sine coefficients that are to be estimated. + /*! + * Function to reset the sine coefficients that are to be estimated. The order of the entries in this vector + * is the same as in the blockIndices_ member vector. + * \param parameterValue List of new values for sine coefficients that are to be estimated. + */ + void setParameterValue( const Eigen::VectorXd parameterValue ); + + //! Function to retrieve the size of the parameter + /*! + * Function to retrieve the size of the parameter, equal to the length of the blcokIndices_ member vector, i.e. the + * number of coefficients that are to be estimated. + * \return Size of parameter value. + */ + int getParameterSize( ) { return parameterSize_; } + + //! Function to retrieve the list of sine coefficient indices which are to be estimated. + /*! + * Function to retrieve the list of sine coefficient indices which are to be estimated (first and second are degree + * and order for each vector entry). + * \return List of sine coefficient indices which are to be estimated. + */ + std::vector< std::pair< int, int > > getBlockIndices( ) + { + return blockIndices_; + } + + +protected: + +private: + + //! Function to retrieve the full set of sine coefficients, of which a subset is to be estimated. + boost::function< Eigen::MatrixXd( ) > getSineCoefficients_; + + //! Function to reset the full set of sine coefficients, of which a subset is to be estimated. + boost::function< void( Eigen::MatrixXd ) > setSineCoefficients_; + + //! List of sine coefficient indices which are to be estimated + /*! + * List of sine coefficient indices which are to be estimated (first and second are degree and order for + * each vector entry). + */ + std::vector< std::pair< int, int > > blockIndices_; + + //! Number of coefficients that are to be estimated (i.e. length of blockIndices_ vector). + int parameterSize_; +}; + +} // namespace estimatable_parameters + +} // namespace tudat + +#endif // TUDAT_SPHERICALHARMONICSINECOEFFICIENTS_H diff --git a/Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/CMakeLists.txt b/Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/CMakeLists.txt new file mode 100644 index 0000000000..1a7c3ca183 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/CMakeLists.txt @@ -0,0 +1,34 @@ + # Copyright (c) 2010-2016, Delft University of Technology + # All rigths reserved + # + # This file is part of the Tudat. Redistribution and use in source and + # binary forms, with or without modification, are permitted exclusively + # under the terms of the Modified BSD license. You should have received + # a copy of the license with this file. If not, please or visit: + # http://tudat.tudelft.nl/LICENSE. + # + # Changelog + # YYMMDD Author Comment + # 110820 S.M. Persson File created. + # 111025 K. Kumar Adapted file to work with Revision 194. + # 111026 K. Kumar Adapted file so all headers show in project tree in Qt Creator. + # + +# Set the source files. +set(OBSERVATION_PARTIALS_SOURCES + "${SRCROOT}${OBSERVATIONPARTIALSDIR}/rotationMatrixPartial.cpp" +) + +# Set the header files. +set(OBSERVATION_PARTIALS_HEADERS + "${SRCROOT}${OBSERVATIONPARTIALSDIR}/rotationMatrixPartial.h" +) + + +# Add static libraries. +add_library(tudat_observation_partials STATIC ${OBSERVATION_PARTIALS_SOURCES} ${OBSERVATION_PARTIALS_HEADERS}) +setup_tudat_library_target(tudat_observation_partials "${SRCROOT}{OBSERVATIONPARTIALSDIR}") + +add_executable(test_RotationMatrixPartials "${SRCROOT}${OBSERVATIONPARTIALSDIR}/UnitTests/unitTestRotationMatrixPartials.cpp") +setup_custom_test_program(test_RotationMatrixPartials "${SRCROOT}${OBSERVATIONPARTIALSDIR}") +target_link_libraries(test_RotationMatrixPartials ${TUDAT_ESTIMATION_LIBRARIES} ${Boost_LIBRARIES}) diff --git a/Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/UnitTests/unitTestRotationMatrixPartials.cpp b/Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/UnitTests/unitTestRotationMatrixPartials.cpp new file mode 100644 index 0000000000..a867832a56 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/UnitTests/unitTestRotationMatrixPartials.cpp @@ -0,0 +1,160 @@ + +#define BOOST_TEST_MAIN + +#include +#include + +#include +#include +#include + +#include "Tudat/External/SpiceInterface/spiceInterface.h" +#include "Tudat/SimulationSetup/EstimationSetup/createPositionPartials.h" +#include "Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/rotationMatrixPartial.h" +#include "Tudat/InputOutput/basicInputOutput.h" + +namespace tudat +{ +namespace unit_tests +{ + +using namespace tudat::ephemerides; +using namespace tudat::estimatable_parameters; +using namespace tudat::observation_partials; + +BOOST_AUTO_TEST_SUITE( test_rotation_matrix_partaisl ) + +//! Test whether partial derivatives of rotation matrix computed by SimpleRotationalEphemeris works correctly +BOOST_AUTO_TEST_CASE( testSimpleRotationalEphemerisPartials ) +{ + // Load spice kernels. + std::string kernelsPath = input_output::getSpiceKernelPath( ); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de-403-masses.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de421.bsp"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "naif0009.tls"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "pck00009.tpc"); + + // Create rotation model + double nominalRotationRate = 2.0 * mathematical_constants::PI / 86400.0; + boost::shared_ptr< SimpleRotationalEphemeris > rotationalEphemeris = + boost::make_shared< SimpleRotationalEphemeris >( + spice_interface::computeRotationQuaternionBetweenFrames( "ECLIPJ2000", "IAU_Earth", 1.0E7 ), + nominalRotationRate, 1.0E7, basic_astrodynamics::JULIAN_DAY_ON_J2000, "ECLIPJ2000", "IAU_Earth" ); + + { + // Create partial object. + boost::shared_ptr< RotationMatrixPartialWrtConstantRotationRate > rotationMatrixPartialObject = + boost::make_shared< RotationMatrixPartialWrtConstantRotationRate >( rotationalEphemeris ); + + // Compute partial analytically + double testTime = 1.0E6; + Eigen::Matrix3d rotationMatrixPartial = + rotationMatrixPartialObject->calculatePartialOfRotationMatrixToBaseFrameWrParameter( + testTime ).at( 0 ); + + // Compute partial numerically. + double perturbation = 1.0E-12; + rotationalEphemeris->resetRotationRate( nominalRotationRate + perturbation ); + Eigen::Matrix3d upperturbedRotationMatrix = rotationalEphemeris->getRotationToBaseFrame( + testTime, basic_astrodynamics::JULIAN_DAY_ON_J2000 ).toRotationMatrix( ); + + rotationalEphemeris->resetRotationRate( nominalRotationRate - perturbation ); + Eigen::Matrix3d downperturbedRotationMatrix = rotationalEphemeris->getRotationToBaseFrame( + testTime, basic_astrodynamics::JULIAN_DAY_ON_J2000 ).toRotationMatrix( ); + + Eigen::Matrix3d numericalRotationMatrixPartial = + ( upperturbedRotationMatrix - downperturbedRotationMatrix ) / ( 2.0 * perturbation ); + Eigen::Matrix3d matrixDifference = rotationMatrixPartial - numericalRotationMatrixPartial; + + // Compare analytical and numerical result. + for( unsigned int i = 0; i < 3; i++ ) + { + for( unsigned int j = 0; j < 3; j++ ) + { + BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 0.1 ); + } + } + } + + { + // Create partial object. + boost::shared_ptr< RotationMatrixPartialWrtPoleOrientation > rotationMatrixPartialObject = + boost::make_shared< RotationMatrixPartialWrtPoleOrientation >( rotationalEphemeris ); + + // Compute partial analytically + double testTime = 1.0E6; + std::vector< Eigen::Matrix3d > rotationMatrixPartials = + rotationMatrixPartialObject->calculatePartialOfRotationMatrixToBaseFrameWrParameter( + testTime ); + + Eigen::Vector3d nominalEulerAngles = rotationalEphemeris->getInitialEulerAngles( ); + double perturbedAngle; + + // Compute partial numerically. + double perturbation = 1.0E-6; + { + // Compute partial for right ascension numerically. + { + perturbedAngle = nominalEulerAngles( 0 ) + perturbation; + rotationalEphemeris->resetInitialPoleRightAscensionAndDeclination( perturbedAngle, nominalEulerAngles( 1 ) ); + Eigen::Matrix3d upperturbedRotationMatrix = rotationalEphemeris->getRotationToBaseFrame( + testTime, basic_astrodynamics::JULIAN_DAY_ON_J2000 ).toRotationMatrix( ); + + perturbedAngle = nominalEulerAngles( 0 ) - perturbation; + rotationalEphemeris->resetInitialPoleRightAscensionAndDeclination( perturbedAngle, nominalEulerAngles( 1 ) ); + Eigen::Matrix3d downperturbedRotationMatrix = rotationalEphemeris->getRotationToBaseFrame( + testTime, basic_astrodynamics::JULIAN_DAY_ON_J2000 ).toRotationMatrix( ); + + Eigen::Matrix3d numericalRotationMatrixPartial = + ( upperturbedRotationMatrix - downperturbedRotationMatrix ) / ( 2.0 * perturbation ); + Eigen::Matrix3d matrixDifference = rotationMatrixPartials.at( 0 ) - numericalRotationMatrixPartial; + + // Compare analytical and numerical result. + for( unsigned int i = 0; i < 3; i++ ) + { + for( unsigned int j = 0; j < 3; j++ ) + { + BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-8 ); + } + } + } + + // Compute partial for declination numerically. + { + perturbedAngle = nominalEulerAngles( 1 ) + perturbation; + rotationalEphemeris->resetInitialPoleRightAscensionAndDeclination( nominalEulerAngles( 0 ), perturbedAngle ); + Eigen::Matrix3d upperturbedRotationMatrix = rotationalEphemeris->getRotationToBaseFrame( + testTime, basic_astrodynamics::JULIAN_DAY_ON_J2000 ).toRotationMatrix( ); + + perturbedAngle = nominalEulerAngles( 1 ) - perturbation; + rotationalEphemeris->resetInitialPoleRightAscensionAndDeclination( nominalEulerAngles( 0 ), perturbedAngle ); + Eigen::Matrix3d downperturbedRotationMatrix = rotationalEphemeris->getRotationToBaseFrame( + testTime, basic_astrodynamics::JULIAN_DAY_ON_J2000 ).toRotationMatrix( ); + + Eigen::Matrix3d numericalRotationMatrixPartial = + ( upperturbedRotationMatrix - downperturbedRotationMatrix ) / ( 2.0 * perturbation ); + Eigen::Matrix3d matrixDifference = rotationMatrixPartials.at( 1 ) - numericalRotationMatrixPartial; + + // Compare analytical and numerical result. + for( unsigned int i = 0; i < 3; i++ ) + { + for( unsigned int j = 0; j < 3; j++ ) + { + BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-8 ); + } + } + } + } + } +} + +BOOST_AUTO_TEST_SUITE_END( ) + +} // namespace unit_tests + +} // namespace tudat + + + + + diff --git a/Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/rotationMatrixPartial.cpp b/Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/rotationMatrixPartial.cpp new file mode 100644 index 0000000000..45d74df4e9 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/rotationMatrixPartial.cpp @@ -0,0 +1,95 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/rotationMatrixPartial.h" + +namespace tudat +{ + +namespace observation_partials +{ + +//! Function to calculate a rotation matrix from a body-fixed to inertial frame w.r.t. a constant rotation rate. +Eigen::Matrix3d calculatePartialOfRotationMatrixFromLocalFrameWrtConstantRotationRate( + const Eigen::Quaterniond intertialBodyFixedToIntegrationFrame, + const double rotationRate, const double timeSinceEpoch ) +{ + double currentRotationAngle = rotationRate * timeSinceEpoch; + + // Compute partial of rotation term containing rotation rate + Eigen::Matrix3d rotationMatrixDerivative; + double sineOfAngle = sin( currentRotationAngle ); + double cosineOfAngle = cos( currentRotationAngle ); + rotationMatrixDerivative << -sineOfAngle, -cosineOfAngle, 0.0, cosineOfAngle, - sineOfAngle, 0.0, 0.0, 0.0, 0.0; + + return timeSinceEpoch * ( intertialBodyFixedToIntegrationFrame.toRotationMatrix( ) ) * rotationMatrixDerivative; + +} +//! Function to calculate a rotation matrix from a body-fixed to inertial frame w.r.t. a constant pole right ascension +//! and declination +std::vector< Eigen::Matrix3d > calculatePartialOfRotationMatrixFromLocalFrameWrtPoleOrientation( + const Eigen::Vector3d initialOrientationAngles,//right ascension, declination, longitude of prime meridian. + const double rotationRate, const double timeSinceEpoch ) +{ + Eigen::Matrix3d commonTerm = Eigen::AngleAxisd( -1.0 * ( -initialOrientationAngles.z( ) - rotationRate * timeSinceEpoch ), + Eigen::Vector3d::UnitZ( ) ).toRotationMatrix( ); + + + double rightAscension = initialOrientationAngles.x( ); + double declination = initialOrientationAngles.y( ); + + // Compute partial of rotation term containing right ascension. + Eigen::Matrix3d rightAscensionPartial; + rightAscensionPartial<< -std::sin( rightAscension + mathematical_constants::PI / 2.0 ), + -std::cos( rightAscension + mathematical_constants::PI / 2.0 ), 0.0, + std::cos( rightAscension + mathematical_constants::PI / 2.0 ), + -std::sin( rightAscension + mathematical_constants::PI / 2.0 ), 0.0, + 0.0, 0.0, 0.0; + + // Compute partial of rotation term containing declination. + Eigen::Matrix3d declinationPartial; + declinationPartial<< 0.0, 0.0, 0.0, + 0.0, std::sin( mathematical_constants::PI / 2.0 - declination ), + std::cos( mathematical_constants::PI / 2.0 - declination ), + 0.0, -std::cos( mathematical_constants::PI / 2.0 - declination ), + std::sin( mathematical_constants::PI / 2.0 - declination ); + + // Compute partials. + std::vector< Eigen::Matrix3d > rotationMatrixPartials; + rotationMatrixPartials.push_back( + rightAscensionPartial * Eigen::Matrix3d( + Eigen::AngleAxisd( -( declination - mathematical_constants::PI / 2.0 ), + Eigen::Vector3d::UnitX( ) ) ) * commonTerm ); + rotationMatrixPartials.push_back( + Eigen::AngleAxisd( ( rightAscension + mathematical_constants::PI / 2.0 ), + Eigen::Vector3d::UnitZ( ) ).toRotationMatrix( ) * + declinationPartial * commonTerm ); + + return rotationMatrixPartials; +} + +Eigen::Matrix< double, 3, Eigen::Dynamic > RotationMatrixPartial::calculatePartialOfRotatedVector( + const double time, + const Eigen::Vector3d vectorInLocalFrame ) +{ + std::vector< Eigen::Matrix3d > rotationMatrixPartials = calculatePartialOfRotationMatrixToBaseFrameWrParameter( time ); + Eigen::Matrix< double, 3, Eigen::Dynamic > rotatedVectorPartial = Eigen::Matrix< double, 3, Eigen::Dynamic >::Zero( + 3, rotationMatrixPartials.size( ) ); + + for( unsigned int i = 0; i < rotationMatrixPartials.size( ); i++ ) + { + rotatedVectorPartial.block( 0, i, 3, 1 ) = rotationMatrixPartials[ i ] * vectorInLocalFrame; + } + return rotatedVectorPartial; +} + +} + +} diff --git a/Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/rotationMatrixPartial.h b/Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/rotationMatrixPartial.h new file mode 100644 index 0000000000..c9ebeb57f0 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/rotationMatrixPartial.h @@ -0,0 +1,217 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_ROTATIONMATRIXPARTIAL_H +#define TUDAT_ROTATIONMATRIXPARTIAL_H + +#include + +#include +#include +#include + +#include + +#include "Tudat/Astrodynamics/Ephemerides/simpleRotationalEphemeris.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h" + +namespace tudat +{ + +namespace observation_partials +{ + +//! Function to calculate a rotation matrix from a body-fixed to inertial frame w.r.t. a constant rotation rate. +/*! + * Function to calculate a rotation matrix from a body-fixed to inertial frame, as computed by the SimpleRotationalEphemeris + * class, w.r.t. the constant rotation rate. + * \param inertialBodyFixedToIntegrationFrame Rotation matrix at reference epoch + * \param rotationRate Nominal rotation rate (about local z-axis) + * \param timeSinceEpoch Elapsed time (in seconds) since reference epoch + * \return Partial derivative of rotation matrix w.r.t. rotation rate. + */ +Eigen::Matrix3d calculatePartialOfRotationMatrixFromLocalFrameWrtConstantRotationRate( + const Eigen::Quaterniond inertialBodyFixedToIntegrationFrame, + const double rotationRate, const double timeSinceEpoch ); + +//! Function to calculate a rotation matrix from a body-fixed to inertial frame w.r.t. a constant pole right ascension +//! and declination. +/*! + * Function to calculate a rotation matrix from a body-fixed to inertial frame, as computed by the SimpleRotationalEphemeris + * class, w.r.t. a constant pole right ascension and declination. + * \param initialOrientationAngles Rotation Euler angles at reference epoch, in order right ascension, declination, + * prime meridian + * \param rotationRate Rotation rate (about local z-axis) + * \param timeSinceEpoch Elapsed time (in seconds) since reference epoch + * \return Partial derivatives of rotation matrix w.r.t. right ascension (entry 0) and declination (entry 1) of pole. + */ +std::vector< Eigen::Matrix3d > calculatePartialOfRotationMatrixFromLocalFrameWrtPoleOrientation( + const Eigen::Vector3d initialOrientationAngles, + const double rotationRate, const double timeSinceEpoch ); + +//! Base class for partial derivatives of rotation matrix from body fixed to inertial frame w.r.t. a parameter. +/*! + * Base class for partial derivatives of rotation matrix from body fixed to inertial frame w.r.t. a parameter. + * A derived class is implemented for each estimatable parameter which represents a property of a rotation matrix from a + * body-fixed frame (as defined by the isParameterRotationMatrixProperty function). Pointers to this class are used for + * both PositionPartials and the SphericalHarmonicsGravityPartial partial. In this manner, only a single partial object + * needs to be implemented for both the position and partial and the sh acceleration + * partial wrt a rotational parameter when such a parameter is added. + */ +class RotationMatrixPartial +{ +public: + + //! Virtual destructor + virtual ~RotationMatrixPartial( ){ } + + //! Function to calculate partials of rotation matrix wrt a parameter (for either double or VectorXd parameters). + /*! + * Function to calculate partials of rotation matrix from the body-fixed to the inertial base frame wrt a parameter. + * For a double parameter, the size of the return vector is 1, for a VectorXd parameter, the size is equal to the size + * of the parameter and the entries of the vector returned from here correspond to partials of the same index in the + * parameter. + * \param time Time at which the partial(s) is(are) to be evaluated. + * \return Vector of partials of rotation matrix from body-fixed to inertial frame wrt parameter(s) + */ + virtual std::vector< Eigen::Matrix3d > calculatePartialOfRotationMatrixToBaseFrameWrParameter( + const double time ) = 0; + + //! Function to calculate the partial of the position of a vector, which is given in a body-fixed frame, in the inertial + //! frame wrt a parameter. + /*! + * Function to calculate the partial of the position of a vector, which is given in a body-fixed frame, in the inertial + * frame wrt a parameter denoting a property of the rotation between a body-fixed and an inertial frame. The type of + * parameter is defined by the derived class and a a separate derived class is impleneted for each parameter. + * An example is the change in position of a ground station, as expressed in the inertial frame, wrt a rotational + * parameter. Each column of the return Matrix denotes a single entry of the parameter + * (so it is a Vector3d for a double parameter). + * \param time Time at which the partial is to be evaluated. + * \param vectorInLocalFrame Vector, expressed in the body-fixed frame, of which the partial in an inertial frame wrt + * parameter is to be determined. + * \return Partial of the value of the vector in an inertial frame wrt the parameter(s). + */ + Eigen::Matrix< double, 3, Eigen::Dynamic > calculatePartialOfRotatedVector( + const double time, + const Eigen::Vector3d vectorInLocalFrame ); + + //! Function to return the secondary identifier of the estimated parameter + /*! + * Function to return the secondary identifier of the estimated parameter. This function returns an empty string by + * default, and can be overridden in the derived class if the associated parameter has a secondary identifier. + * \return Secondary identifier of the estimated parameter + */ + virtual std::string getSecondaryIdentifier( ) + { + return ""; + } + +}; + +//! Class to calculate a rotation matrix from a body-fixed to inertial frame w.r.t. a constant rotation rate. +/*! + * Class to calculate a rotation matrix from a body-fixed to inertial frame, as computed by the SimpleRotationalEphemeris + * class, w.r.t. the constant rotation rate. + */ +class RotationMatrixPartialWrtConstantRotationRate: public RotationMatrixPartial +{ +public: + + //! Constructor + /*! + * Constructor + * \param bodyRotationModel Rotation model for which the partial derivative w.r.t. the rotation rate is to be taken. + */ + RotationMatrixPartialWrtConstantRotationRate( + const boost::shared_ptr< ephemerides::SimpleRotationalEphemeris > bodyRotationModel ): + bodyRotationModel_( bodyRotationModel ){ } + + //! Destructor. + ~RotationMatrixPartialWrtConstantRotationRate( ){ } + + //! Function to calculate partial of rotation matrix from the body-fixed to the inertial frame wrt a rotation rate. + /*! + * Function to calculate partial of rotation matrix from the body-fixed to the inertial frame wrt a rotation rate, + * using the properties of the bodyRotationModel_ member. + * \param time Time at which the partial is to be evaluated. + * \return Vector of size 1 containing partials of rotation matrix from body-fixed to inertial frame wrt + * rotation rate. + */ + std::vector< Eigen::Matrix3d > calculatePartialOfRotationMatrixToBaseFrameWrParameter( + const double time ) + { + return boost::assign::list_of( + calculatePartialOfRotationMatrixFromLocalFrameWrtConstantRotationRate( + bodyRotationModel_->getInitialRotationToTargetFrame( ).inverse( ), + bodyRotationModel_->getRotationRate( ), + time - bodyRotationModel_->getInitialSecondsSinceEpoch( ) ) ); + } +private: + + //! Rotation model for which the partial derivative w.r.t. the rotation rate is to be taken. + boost::shared_ptr< ephemerides::SimpleRotationalEphemeris > bodyRotationModel_; +}; + +//! Class to calculate a rotation matrix from a body-fixed to inertial frame w.r.t. a constant pole right ascension +//! and declination +/*! + * Class to calculate a rotation matrix from a body-fixed to inertial frame, as computed by the SimpleRotationalEphemeris + * class, w.r.t. the constant pole right ascension and declination + */ +class RotationMatrixPartialWrtPoleOrientation: public RotationMatrixPartial +{ +public: + + //! Constructor + /*! + * Constructor + * \param bodyRotationModel Rotation model for which the partial derivative w.r.t. the pole position is to be taken. + */ + RotationMatrixPartialWrtPoleOrientation( + const boost::shared_ptr< ephemerides::SimpleRotationalEphemeris > bodyRotationModel ): + bodyRotationModel_( bodyRotationModel ){ } + + //! Destructor. + ~RotationMatrixPartialWrtPoleOrientation( ){ } + + //! Function to calculate partial of rotation matrix from the body-fixed to the inertial frame wrt pole + //! right ascension and declination. + /*! + * Function to calculate partial of rotation matrix from the body-fixed to the inertial frame wrt pole + * right ascension and declination, using the properties of the bodyRotationModel_ member. + * \param time Time at which the partial is to be evaluated. + * \return Vector of size 2 containing partials of rotation matrix from body-fixed to inertial frame right ascension + * (entry 0) and declination (entry 1) of pole. + */ + std::vector< Eigen::Matrix3d > calculatePartialOfRotationMatrixToBaseFrameWrParameter( + const double time ) + { + return calculatePartialOfRotationMatrixFromLocalFrameWrtPoleOrientation( + bodyRotationModel_->getInitialEulerAngles( ), + bodyRotationModel_->getRotationRate( ), time - bodyRotationModel_->getInitialSecondsSinceEpoch( ) ); + } + +private: + + //! Rotation model for which the partial derivative w.r.t. the rotation rate is to be taken. + boost::shared_ptr< ephemerides::SimpleRotationalEphemeris > bodyRotationModel_; +}; + + +//! Typedef of list of RotationMatrixPartial objects, ordered by parameter. +typedef std::map< std::pair< estimatable_parameters::EstimatebleParametersEnum, std::string >, +boost::shared_ptr< observation_partials::RotationMatrixPartial > > RotationMatrixPartialNamedList; + + +} // namespace observation_partials + +} // namespace tudat + +#endif // TUDAT_ROTATIONMATRIXPARTIAL_H diff --git a/Tudat/Astrodynamics/OrbitDetermination/stateDerivativePartial.cpp b/Tudat/Astrodynamics/OrbitDetermination/stateDerivativePartial.cpp new file mode 100644 index 0000000000..5c9d8f6de9 --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/stateDerivativePartial.cpp @@ -0,0 +1,229 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/Astrodynamics/OrbitDetermination/stateDerivativePartial.h" + +namespace tudat +{ + +namespace orbit_determination +{ + +//! Function to evaluate the negative value of a parameter partial. +void evaluateNegativeParameterPartialFunction( + const boost::function< void( Eigen::MatrixXd& ) > parameterPartialFunction, + Eigen::MatrixXd& partial ) +{ + parameterPartialFunction( partial ); + partial *= -1.0; +} + +//! Function to evaluate the subtraction of two parameter partials. +void evaluateSubtractedParameterPartialFunction( + const boost::function< void( Eigen::MatrixXd& ) > firstParameterPartialFunction, + const boost::function< void( Eigen::MatrixXd& ) > parameterPartialFunctionToSubtract, + Eigen::MatrixXd& partial ) +{ + firstParameterPartialFunction( partial ); + + Eigen::MatrixXd subtractedPartial = Eigen::MatrixXd::Zero( partial.rows( ), partial.cols( ) ); + parameterPartialFunctionToSubtract( subtractedPartial ); + + partial -= subtractedPartial; +} + +//! Function to evaluate the addition of two parameter partials. +void evaluateAddedParameterPartialFunction( + const boost::function< void( Eigen::MatrixXd& ) > firstParameterPartialFunction, + const boost::function< void( Eigen::MatrixXd& ) > parameterPartialFunctionToAdd, + Eigen::MatrixXd& partial ) +{ + firstParameterPartialFunction( partial ); + + Eigen::MatrixXd addedPartial = Eigen::MatrixXd::Zero( partial.rows( ), partial.cols( ) ); + parameterPartialFunctionToAdd( addedPartial ); + + partial += addedPartial; +} + +//! Create a parameter partial function obtained from the subtraction of two such function results. +std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > createMergedParameterPartialFunction( + const std::pair< boost::function< void( Eigen::MatrixXd& ) >, int >& partialFunctionOfAccelerationToAdd, + const std::pair< boost::function< void( Eigen::MatrixXd& ) >, int >& partialFunctionOfAccelerationToSubtract ) +{ + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > parameterPartialFunction; + + // Check partial size and act accordingly. + if( ( partialFunctionOfAccelerationToAdd.second == 0 ) && + ( partialFunctionOfAccelerationToSubtract.second == 0 ) ) + { + parameterPartialFunction = std::make_pair( boost::function< void( Eigen::MatrixXd& ) >( ), 0 ); + } + else if( partialFunctionOfAccelerationToSubtract.second == 0 ) + { + parameterPartialFunction = partialFunctionOfAccelerationToAdd; + } + else if( partialFunctionOfAccelerationToAdd.second == 0 ) + { + parameterPartialFunction = std::make_pair( boost::bind( + &evaluateNegativeParameterPartialFunction, + partialFunctionOfAccelerationToSubtract.first, _1 ), + partialFunctionOfAccelerationToSubtract.second ); + } + // Partial size must be equal if both non-zero + else if( partialFunctionOfAccelerationToSubtract.second != + partialFunctionOfAccelerationToAdd.second ) + { + throw std::runtime_error( "Error when making merged parameter partial function, separate functions are incompatible" ); + } + else + { + parameterPartialFunction = std::make_pair( boost::bind( &evaluateSubtractedParameterPartialFunction, + partialFunctionOfAccelerationToAdd.first, + partialFunctionOfAccelerationToSubtract.first, _1 ), + partialFunctionOfAccelerationToSubtract.second ); + } + return parameterPartialFunction; +} + +//! Function to create a parameter partial evaluation function, obtained by adding or subtracting a given partial +//! w.r.t. a double parameter from 2 state derivative partial models. +boost::function< void( Eigen::MatrixXd& ) > getCombinedCurrentDoubleParameterFunction( + const boost::shared_ptr< StateDerivativePartial > firstPartial, + const boost::shared_ptr< StateDerivativePartial > secondPartial, + const boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameterObject, + const int firstPartialSize, const int secondPartialSize, + const bool subtractPartials ) +{ + boost::function< void( Eigen::MatrixXd& ) > partialFunction; + + // Get two partial functions. + boost::function< void( Eigen::MatrixXd& ) > firstPartialFunction = + boost::bind( &StateDerivativePartial::getCurrentDoubleParameterPartial, firstPartial, parameterObject, _1 ); + boost::function< void( Eigen::MatrixXd& ) > secondPartialFunction = + boost::bind( &StateDerivativePartial::getCurrentDoubleParameterPartial, secondPartial, parameterObject, _1 ); + + // If both partial function sizes are zero, cannot create partial. + if( firstPartialSize == 0 && secondPartialSize == 0 ) + { + throw std::runtime_error( "Error when getting combined current partial size, both partials have size zero " ); + } + // Check other cases and combine partial functions accordingly + else if( secondPartialSize == 0 ) + { + partialFunction = firstPartialFunction; + } + else if( firstPartialSize == 0 ) + { + if( subtractPartials ) + { + partialFunction = boost::bind( &evaluateNegativeParameterPartialFunction, + secondPartialFunction, _1 ); + } + else + { + partialFunction = secondPartialFunction; + + } + } + else if( firstPartialSize == secondPartialSize ) + { + if( subtractPartials ) + { + partialFunction = boost::bind( &evaluateSubtractedParameterPartialFunction, + firstPartialFunction, + secondPartialFunction, _1 ); + } + else + { + partialFunction = boost::bind( &evaluateAddedParameterPartialFunction, + firstPartialFunction, + secondPartialFunction, _1 ); + } + + } + // Partial size must be equal if both non-zero + else + { + throw std::runtime_error( + "Error when getting combined current partial size, both partials have different non-zero size." ); + } + return partialFunction; +} + +//! Function to create a parameter partial evaluation function, obtained by adding or subtracting a given partial +//! w.r.t. a vector parameter from 2 state derivative partial models. +boost::function< void( Eigen::MatrixXd& ) > getCombinedCurrentVectorParameterFunction( + const boost::shared_ptr< StateDerivativePartial > firstPartial, + const boost::shared_ptr< StateDerivativePartial > secondPartial, + const boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > parameterObject, + const int firstPartialSize, const int secondPartialSize, + const bool subtractPartials ) +{ + boost::function< void( Eigen::MatrixXd& ) > partialFunction; + + // Get two partial functions. + boost::function< void( Eigen::MatrixXd& ) > firstPartialFunction = + boost::bind( &StateDerivativePartial::getCurrentVectorParameterPartial, firstPartial, parameterObject, _1 ); + boost::function< void( Eigen::MatrixXd& ) > secondPartialFunction = + boost::bind( &StateDerivativePartial::getCurrentVectorParameterPartial, secondPartial, parameterObject, _1 ); + + + // If both partial function sizes are zero, cannot create partial. + if( firstPartialSize == 0 && secondPartialSize == 0 ) + { + throw std::runtime_error( "Error when getting combined current partial size, both partials have size zero " ); + } + // Check other cases and combine partial functions accordingly + else if( secondPartialSize == 0 ) + { + partialFunction = firstPartialFunction; + } + else if( firstPartialSize == 0 ) + { + if( subtractPartials ) + { + partialFunction = boost::bind( &evaluateNegativeParameterPartialFunction, + secondPartialFunction, _1 ); + } + else + { + partialFunction = secondPartialFunction; + + } + } + else if( firstPartialSize == secondPartialSize ) + { + if( subtractPartials ) + { + partialFunction = boost::bind( &evaluateSubtractedParameterPartialFunction, + firstPartialFunction, + secondPartialFunction, _1 ); + } + else + { + partialFunction = boost::bind( &evaluateAddedParameterPartialFunction, + firstPartialFunction, + secondPartialFunction, _1 ); + } + + } + // Partial size must be equal if both non-zero + else + { + throw std::runtime_error( "Error when getting combined current partial size, both partials have different non-zero size." ); + } + return partialFunction; +} + +} + +} + diff --git a/Tudat/Astrodynamics/OrbitDetermination/stateDerivativePartial.h b/Tudat/Astrodynamics/OrbitDetermination/stateDerivativePartial.h new file mode 100644 index 0000000000..596fd8345e --- /dev/null +++ b/Tudat/Astrodynamics/OrbitDetermination/stateDerivativePartial.h @@ -0,0 +1,642 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_STATEDERIVATIVEPARTIAL_H +#define TUDAT_STATEDERIVATIVEPARTIAL_H + +#include +#include +#include + +#include +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" + +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h" +#include "Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h" + + +namespace tudat +{ + +namespace orbit_determination +{ + +//! Base class for computing the partial derivatives of a state derivative model +/*! + * Base class for computing the partial derivatives of a state derivative model (i.e. acceleration model for + * translational dynamics, torque model for rottional dynamics, etc. + * Typically, two levels of derived classes are required: one for the type of dynamics, and one for the type of model + * (e.g. one level for acceleration model, and one level for central gravitational, spherical harmonic, etc. model). + */ +class StateDerivativePartial +{ + +public: + + //! Constructor. + /*! + * Constructor + * \param integratedStateType Type of dynamics for which partials are to be computed + * \param integrationReferencePoint Reference point (i.e. propagated body and point) for which the dynamics is + * propagated. First entry denotes the full body, second entry the reference point on the body that is propagated + * (empty for translational, rotational dynamics). + */ + StateDerivativePartial( const propagators::IntegratedStateType integratedStateType, + const std::pair< std::string, std::string >& integrationReferencePoint ): + integratedStateType_( integratedStateType ), integrationReferencePoint_( integrationReferencePoint ) + { + currentTime_ = TUDAT_NAN; + accelerationSize_ = propagators::getSingleIntegrationSize( integratedStateType_ ) / + propagators::getSingleIntegrationDifferentialEquationOrder( integratedStateType_ ); + } + + //! Destructor. + virtual ~StateDerivativePartial( ) { } + + //! Pure virtual function to retrieve the function that returns the partial derivative w.r.t. a propagated state. + /*! + * Pure virtual function to retrieve the function that returns the partial derivative w.r.t. a propagated state. + * \param stateReferencePoint Reference point (id) for propagated state (i.e. body name for translational dynamics). + * \param integratedStateType Type of propagated state. + * \return Pair with function, returning partial derivative, and number of columns in partial vector, + */ + virtual std::pair< boost::function< void( Eigen::Block< Eigen::MatrixXd > ) >, int > + getDerivativeFunctionWrtStateOfIntegratedBody( + const std::pair< std::string, std::string >& stateReferencePoint, + const propagators::IntegratedStateType integratedStateType ) = 0; + + //! Pure virtual function to check whether a partial w.r.t. some integrated state is non-zero. + /*! + * Pure virtual function to check whether a partial w.r.t. some integrated state is non-zero. + * \param stateReferencePoint Reference point (id) for propagated state (i.e. body name for translational dynamics). + * \param integratedStateType Type of propagated state. + * \return True if dependency exists, false otherwise. + */ + virtual bool isStateDerivativeDependentOnIntegratedNonTranslationalState( + const std::pair< std::string, std::string >& stateReferencePoint, + const propagators::IntegratedStateType integratedStateType ) = 0; + + //! Function to directly compute the partial of the state derivative w.r.t. a double parameter. + /*! + * Function to directly compute the partial of the state derivative w.r.t. a double parameter. NOTE: This function is + * incldued for testing purposes, and is not to be used during propagation (highly inefficient). + * \param parameter Parameter w.r.t. which partial is to be computed + * \return Partial of state derivative w.r.t. given parameter. + */ + Eigen::MatrixXd wrtParameter( + const boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameter ) + { + // Initialize partial + Eigen::MatrixXd partial = Eigen::MatrixXd( accelerationSize_, 1 ); + + // Get partial computation function. + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > partialFunction = + getParameterPartialFunction( parameter ); + + // If parameter dependency exists, compute it, otherwise set partial to zero. + if( partialFunction.second > 0 ) + { + partialFunction.first( partial ); + } + else + { + partial = Eigen::MatrixXd::Zero( propagators::getSingleIntegrationSize( integratedStateType_ ), 1 ); + } + return partial; + } + + //! Function to retrieve the function that computes (by reference) a given double parameter partial. + /*! + * Function to retrieve the function that computes (by reference) a given double parameter partial. NOTE: this function + * is implemented in this base class with default no dependency. If any double parameter dependencioes exists, this + * function should be overriden in derived class. + * \param parameter Parameter w.r.t. which partial is to be computed + */ + virtual std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > getParameterPartialFunction( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameter ) + { + boost::function< void( Eigen::MatrixXd& ) > partialFunction; + return std::make_pair( partialFunction, 0 ); + } + + //! Function to directly compute the partial of the state derivative w.r.t. a vector parameter. + /*! + * Function to directly compute the partial of the state derivative w.r.t. a vector parameter. NOTE: This function is + * incldued for testing purposes, and is not to be used during propagation (highly inefficient). + * \param parameter Parameter w.r.t. which partial is to be computed + * \return Partial of state derivative w.r.t. given parameter. + */ + Eigen::MatrixXd wrtParameter( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > parameter ) + { + // Initialize partial + Eigen::MatrixXd partial = Eigen::MatrixXd( accelerationSize_, parameter->getParameterSize( ) ); + + // Get partial computation function. + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > partialFunction = + getParameterPartialFunction( parameter ); + + // If parameter dependency exists, compute it, otherwise set partial to zero. + if( partialFunction.second > 0 ) + { + partialFunction.first( partial ); + } + else + { + partial = Eigen::MatrixXd::Zero( propagators::getSingleIntegrationSize( integratedStateType_ ), + parameter->getParameterSize( ) ); + } + return partial; + } + + //! Function to retrieve the function that computes (by reference) a given vector parameter partial. + /*! + * Function to retrieve the function that computes (by reference) a given vector parameter partial. NOTE: this function + * is implemented in this base class with default no dependency. If any double parameter dependencioes exists, this + * function should be overriden in derived class. + * \param parameter Parameter w.r.t. which partial is to be computed + */ + virtual std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > getParameterPartialFunction( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > parameter ) + { + boost::function< void( Eigen::MatrixXd& ) > partialFunction; + return std::make_pair( partialFunction, 0 ); + } + + //! Pure virtual for updating the common parts of the partial object to current state and time. + /*! + * Pure virtual for updating the common parts of the partial object to current state and time. Note that this + * function is distinct from the updateParameterPartials, which only updates the specific required partial + * derivatives w.r.t. required parameters. This function computes variables that are always required when + * the derived class partial is used. + * \param currentTime Time to which partials are to be updated. + */ + virtual void update( const double currentTime ) = 0; + + //! Function to get the type of state for which partials are to be computed. + /*! + * Function to get the type of state for which partials are to be computed. + * \return Type of state for which partials are to be computed. + */ + propagators::IntegratedStateType getIntegratedStateType( ) + { + return integratedStateType_; + } + + //! Function to get the identifier for body/reference point for which propagation is performed. + /*! + * Function to get the identifier for body/reference point for which propagation is performed. + * \return Identifier for body/reference point for which propagation is performed. + */ + std::pair< std::string, std::string > getIntegrationReferencePoint( ) + { + return integrationReferencePoint_; + } + + //! Function to reset the object to the current time + /*! + * Function to reset the object to the current time, recomputing partials to current state. + * \param currentTime Time to which partials are to be updated. + */ + void resetTime( const double currentTime = TUDAT_NAN ) + { + // Check if update is needed. + if( !( currentTime_ == currentTime ) ) + { + resetCurrentParameterValues( ); + currentTime_ = currentTime; + } + + // Perform updates of member objects if needed. + resetTimeOfMemberObjects( ); + } + + //! Function to retrieve a partial w.r.t. a double parameter + /*! + * Function to retrieve a partial w.r.t. a double parameter. An error is thrown if there is no dependency w.r.t. + * the requested parameter. A warning is printed if the dependency exists, but has not yet been computed for the + * current time step. + * \param parameter Partial w.r.t. which a parameter is to be computed + * \param partialMatrix Partial of state derivative w.r.t. given parameter (return by 'reference', amking use of + * Eigen::Block architecture). + */ + void getCurrentParameterPartial( + const boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameter, + Eigen::Block< Eigen::MatrixXd > partialMatrix ) + { + // Check if dependecy is computed + if( currentDoubleParameterPartials_.count( parameter ) == 0 ) + { + // Check if dependecy exists at all + if( parameterDoublePartialFunctions_.count( parameter ) == 0 ) + { + throw std::runtime_error( + "Parameter of type " + + boost::lexical_cast< std::string >( parameter->getParameterName( ).first ) + ", " + + parameter->getParameterName( ).second.first + ", " + + parameter->getParameterName( ).second.second + ", " + + " not found in list of existing partials" ); + } + else + { + std::cerr<<"Warning, double partial should already be calculated"< > parameter, + Eigen::MatrixXd& parameterPartial ) + { + getCurrentParameterPartial( parameter, parameterPartial.block( 0, 0, accelerationSize_, 1 ) ); + } + + //! Function to retrieve a partial w.r.t. a vector parameter + /*! + * Function to retrieve a partial w.r.t. a vector parameter. An error is thrown if there is no dependency w.r.t. + * the requested parameter. A warning is printed if the dependency exists, but has not yet been computed for the + * current time step. + * \param parameter Partial w.r.t. which a parameter is to be computed + * \param partialMatrix Partial of state derivative w.r.t. given parameter (return by 'reference', amking use of + * Eigen::Block architecture). + */ + void getCurrentParameterPartial( + const boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > parameter, + Eigen::Block< Eigen::MatrixXd > partialMatrix ) + { + if( currentVectorParameterPartials_.count( parameter ) == 0 ) + { + if( parameterVectorPartialFunctions_.count( parameter ) == 0 ) + { + std::string errorMessage = "Parameter of type " + + boost::lexical_cast< std::string >( parameter->getParameterName( ).first ) + ", " + + boost::lexical_cast< std::string >( parameter->getParameterName( ).second.first ) + ", " + + boost::lexical_cast< std::string >( parameter->getParameterName( ).second.second ) + ", " + + " not found in list of existing partials"; + throw std::runtime_error( errorMessage ); + } + else + { + std::cerr<<"Warning, vector partial should already be calculated"< > parameter, + Eigen::MatrixXd& parameterPartial ) + { + getCurrentParameterPartial( parameter, parameterPartial.block( + 0, 0, accelerationSize_, parameter->getParameterSize( ) ) ); + } + + //! Function to set a dependency of this partial object w.r.t. a given double parameter. + /*! + * Function to set a dependency of this partial object w.r.t. a given double parameter. If a dependency exists, the given + * partial is recomputed on every call of updateParameterPartials. + * \param parameter Partial w.r.t. which dependency is to be checked and set. + * \return Size (number of columns) of parameter partial. Zero if no dependency, 1 otherwise. + */ + virtual int setParameterPartialUpdateFunction( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameter ) + { + // Get partial function. + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > parameterPartialFunction = + getParameterPartialFunction( parameter ); + + // If partial function found, add function to list of computations to be performed when calling + // updateParameterPartials. + if( parameterPartialFunction.second > 0 && parameterDoublePartialFunctions_.count( parameter ) == 0 ) + { + if( parameterDoublePartialFunctions_.count( parameter ) == 0 ) + { + parameterDoublePartialFunctions_[ parameter ] = parameterPartialFunction.first; + isCurrentDoubleParameterPartialSet_[ parameter ] = 0; + currentDoubleParameterPartials_[ parameter ] = Eigen::MatrixXd( accelerationSize_, 1 ); + } + } + + return parameterPartialFunction.second; + } + + //! Function to set a dependency of this partial object w.r.t. a given vector parameter. + /*! + * Function to set a dependency of this partial object w.r.t. a given vector parameter. If a dependency exists, the given + * partial is recomputed on every call of updateParameterPartials. + * \param parameter Partial w.r.t. which dependency is to be checked and set. + * \return Size (number of columns) of parameter partial. Zero if no dependency, size of parameter otherwise. + */ + virtual int setParameterPartialUpdateFunction( + boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > parameter ) + { + // Get partial function. + std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > parameterPartialFunction = + getParameterPartialFunction( parameter ); + + // If partial function found, add function to list of computations to be performed when calling + // updateParameterPartials. + if( parameterPartialFunction.second > 0 && currentVectorParameterPartials_.count( parameter ) == 0 ) + { + if( parameterVectorPartialFunctions_.count( parameter ) == 0 ) + { + parameterVectorPartialFunctions_[ parameter ] = parameterPartialFunction.first; + isCurrentVectorParameterPartialSet_[ parameter ] = 0; + currentVectorParameterPartials_[ parameter ] = + Eigen::MatrixXd( accelerationSize_, parameter->getParameterSize( ) ); + } + } + + return parameterPartialFunction.second; + } + + //! Function to update the values partial derivatives to current state and time. + /*! + * Function to update the values partial derivatives to current state and time. + */ + void updateParameterPartials( ) + { + // Update member object partials. + updateParameterPartialsOfMemberObjects( ); + + // Update double parameter partials + for( parameterDoublePartialFunctionIterator_ = parameterDoublePartialFunctions_.begin( ); + parameterDoublePartialFunctionIterator_ != parameterDoublePartialFunctions_.end( ); + parameterDoublePartialFunctionIterator_++ ) + { + if( isCurrentDoubleParameterPartialSet_.at( parameterDoublePartialFunctionIterator_->first ) == 0 ) + { + parameterDoublePartialFunctionIterator_->second( currentDoubleParameterPartials_[ parameterDoublePartialFunctionIterator_->first ] ); + isCurrentDoubleParameterPartialSet_[ parameterDoublePartialFunctionIterator_->first ] = 1; + } + } + + // Update vector parameter partials + for( parameterVectorPartialFunctionIterator_ = parameterVectorPartialFunctions_.begin( ); + parameterVectorPartialFunctionIterator_ != parameterVectorPartialFunctions_.end( ); + parameterVectorPartialFunctionIterator_++ ) + { + if( isCurrentVectorParameterPartialSet_.at( parameterVectorPartialFunctionIterator_->first ) == 0 ) + { + parameterVectorPartialFunctionIterator_->second( currentVectorParameterPartials_[ parameterVectorPartialFunctionIterator_->first ] ); + isCurrentVectorParameterPartialSet_[ parameterVectorPartialFunctionIterator_->first ] = 1; + } + } + } + +protected: + + //! Function to compute parameter partials of member objects. + /*! + * Function to compute parameter partials of member objects. By default (implemented here) no computations are + * performed. For certain derived classed (i.e. ThirdBodyGravityPartial), there are member StateDerivativePartial + * objects that need to be updated when calling updateParameterPartials, for which this function should be redefined. + */ + virtual void updateParameterPartialsOfMemberObjects( ) + { + + } + + //! Function to reset the member object to the current time + /*! + * Function to reset the member object to the current time. By default (implemented here) no computations are performed. + * For certain derived classed (i.e. ThirdBodyGravityPartial), there are member StateDerivativePartial objects that + * need to be updated when calling resetTime, for which this function should be redefined. + */ + virtual void resetTimeOfMemberObjects( ) + { + + } + + //! Function to define all current parameter partials as 'not computed' + void resetCurrentParameterValues( ) + { + for( isCurrentDoubleParameterPartialSetIterator_ = isCurrentDoubleParameterPartialSet_.begin( ); + isCurrentDoubleParameterPartialSetIterator_ != isCurrentDoubleParameterPartialSet_.end( ); + isCurrentDoubleParameterPartialSetIterator_++ ) + { + isCurrentDoubleParameterPartialSet_[ isCurrentDoubleParameterPartialSetIterator_->first ] = 0; + } + + for( isCurrentVectorParameterPartialSetIterator_ = isCurrentVectorParameterPartialSet_.begin( ); + isCurrentVectorParameterPartialSetIterator_ != isCurrentVectorParameterPartialSet_.end( ); + isCurrentVectorParameterPartialSetIterator_++ ) + { + isCurrentVectorParameterPartialSet_[ isCurrentVectorParameterPartialSetIterator_->first ] = 0; + } + } + + //! Type of state for which partials are to be computed. + propagators::IntegratedStateType integratedStateType_; + + //! Size of the single order state derivative model (i.e. 3 for translational dynamics). + int accelerationSize_; + + //! Identifier for body/reference point for which propagation is performed. + /*! + * Identifier for body/reference point for which propagation is performed. First entry represents the body being + * propagated. Second entry is empty for propagation of entire bodies (translational, rotational), but must be + * set for propagation of local dynamics (i.e. observer proper time). + */ + std::pair< std::string, std::string > integrationReferencePoint_; + + + //! List of booleans defining whether the partial w.r.t. the current double parameter (key) has been computed. + std::map< boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > >, bool > + isCurrentDoubleParameterPartialSet_; + + //! Iterator for list defining whether the partial w.r.t. the current double parameter (key) has been computed. + std::map< boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > >, bool >::iterator + isCurrentDoubleParameterPartialSetIterator_; + + + + //! List of current values of partials w.r.t. double parameter values (emptied at beginning of every time step). + std::map< boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > >, Eigen::MatrixXd > + currentDoubleParameterPartials_; + + //! List of functions to compute (return by reference) values of partials w.r.t. doule parameter partials + std::map< boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > >, + boost::function< void( Eigen::MatrixXd& ) > > parameterDoublePartialFunctions_; + + //! Iterator over list of functions to compute values of partials w.r.t. double parameter partials + std::map< boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > >, + boost::function< void( Eigen::MatrixXd& ) > >::iterator parameterDoublePartialFunctionIterator_; + + + + //! List of booleans defining whether the partial w.r.t. the current vector parameter (key) has been computed. + std::map< boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > >, bool > + isCurrentVectorParameterPartialSet_; + + //! Iterator for list defining whether the partial w.r.t. the current vector parameter (key) has been computed. + std::map< boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > >, bool >::iterator + isCurrentVectorParameterPartialSetIterator_; + + + + //! List of current values of partials w.r.t. double parameter values (emptied at beginning of every time step). + std::map< boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > >, + Eigen::MatrixXd > currentVectorParameterPartials_; + + //! List of functions to compute (return by reference) values of partials w.r.t. vector parameter partials + std::map< boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > >, + boost::function< void( Eigen::MatrixXd& ) > > parameterVectorPartialFunctions_; + + //! Iterator over list of functions to compute values of partials w.r.t. vector parameter partials + std::map< boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > >, + boost::function< void( Eigen::MatrixXd& ) > >::iterator parameterVectorPartialFunctionIterator_; + + + double currentTime_; + +}; + +//! Typedef for double vector of StateDerivativePartial objects. +/*! + * Typedef for double vector of StateDerivativePartial objects. First (outer) vector is typically the + * bodies undergoing 'acceleration (and being estimated), the second (inner) vector is the list of partials + * being exerted on a single body. + */ +typedef std::vector< std::vector< boost::shared_ptr< orbit_determination::StateDerivativePartial > > > +StateDerivativePartialsMap; + +//! Function to evaluate the negative value of a parameter partial. +/*! + * Function to evaluate the negative value of a parameter partial. + * \param parameterPartialFunction Function to compute the regular paramater partial (by reference). + * \param partial Negative value of partial computed by parameterPartialFunction (returned by reference). + */ +void evaluateNegativeParameterPartialFunction( + const boost::function< void( Eigen::MatrixXd& ) > parameterPartialFunction, + Eigen::MatrixXd& partial ); + +//! Function to evaluate the subtraction of two parameter partials. +/*! + * Function to evaluate the subtraction of two parameter partials. + * \param firstParameterPartialFunction Function to compute the first paramater partial (by reference), from which + * the value computed by parameterPartialFunctionToSubtract is subtracted. + * \param parameterPartialFunctionToSubtract Function to compute the paramater partial (by reference) that is to be + * subtracted. + * \param partial Value of partial returned by parameterPartialFunctionToSubtract, subtracted from value returned by + * firstParameterPartialFunction (returned by reference). + */ +void evaluateSubtractedParameterPartialFunction( + const boost::function< void( Eigen::MatrixXd& ) > firstParameterPartialFunction, + const boost::function< void( Eigen::MatrixXd& ) > parameterPartialFunctionToSubtract, + Eigen::MatrixXd& partial ); + +//! Function to evaluate the addition of two parameter partials. +/*! + * Function to evaluate the addition of two parameter partials. + * \param firstParameterPartialFunction Function to compute the first paramater partial (by reference) that is to be + * added. + * \param parameterPartialFunctionToAdd Function to compute the second paramater partial (by reference) that is to be + * added. + * \param partial Value of partial returned by firstParameterPartialFunction, added to value returned by + * parameterPartialFunctionToAdd (returned by reference). + */ +void evaluateAddedParameterPartialFunction( + const boost::function< void( Eigen::MatrixXd& ) > firstParameterPartialFunction, + const boost::function< void( Eigen::MatrixXd& ) > parameterPartialFunctionToAdd, + Eigen::MatrixXd& partial ); + +//! Create a parameter partial function obtained from the subtraction of two such function results. +/*! + * Create a parameter partial function, as returned by the StateDerivativePartial::getParameterPartialFunction function + * The partial created here is obtained from the subtraction of two such function results. The two input variables + * may be both empty, both define a function, or only one of them may define a function. + * \param partialFunctionOfAccelerationToAdd Function and associated parameter size (first and second of pair) that + * are to be added to the total partial. + * \param partialFunctionOfAccelerationToSubtract Function and associated parameter size (first and second of pair) that + * are to be subtracted from the total partial. + * \return Function and parameter size obtained from 'subtracting' partialFunctionOfAccelerationToSubtract from + * partialFunctionOfAccelerationToAdd + */ +std::pair< boost::function< void( Eigen::MatrixXd& ) >, int > createMergedParameterPartialFunction( + const std::pair< boost::function< void( Eigen::MatrixXd& ) >, int >& partialFunctionOfAccelerationToAdd, + const std::pair< boost::function< void( Eigen::MatrixXd& ) >, int >& partialFunctionOfAccelerationToSubtract ); + + +//! Function to create a parameter partial evaluation function, obtained by adding or subtracting a given partial +//! w.r.t. a double parameter from 2 state derivative partial models. +/*! + * Function to create a parameter partial evaluation function, obtained by adding or subtracting a given partial + * w.r.t. a double parameter from 2 state derivative partial models. Note that this function creates a merged + * function from two getCurrentDoubleParameterPartial functions of the two input partial objects. The automatic + * computation when updating the partial (done by call to setParameterPartialUpdateFunction) is not handled by + * this function. + * \param firstPartial First object for computing partial derivatives. + * \param secondPartial Second object for computing partial derivatives. + * \param parameterObject Parameter w.r.t. which a partial is to be taken. + * \param firstPartialSize Size of partial from firstPartial object (only 0 and 1 are valid). + * \param secondPartialSize Size of partial from secondPartial object (only 0 and 1 are valid). + * \param subtractPartials Boolean denoting whether the second parameter is to be subtracted or added to the + * total partial. + * \return Function computing and returning (by reference) the combined partial according to the required settings. + */ +boost::function< void( Eigen::MatrixXd& ) > getCombinedCurrentDoubleParameterFunction( + const boost::shared_ptr< StateDerivativePartial > firstPartial, + const boost::shared_ptr< StateDerivativePartial > secondPartial, + const boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameterObject, + const int firstPartialSize, const int secondPartialSize, + const bool subtractPartials = 0 ); + +//! Function to create a parameter partial evaluation function, obtained by adding or subtracting a given partial +//! w.r.t. a vector parameter from 2 state derivative partial models. +/*! + * Function to create a parameter partial evaluation function, obtained by adding or subtracting a given partial + * w.r.t. a vector parameter from 2 state derivative partial models. Note that this function creates a merged + * function from two getCurrentVectorParameterPartial functions of the two input partial objects. The automatic + * computation when updating the partial (done by call to setParameterPartialUpdateFunction) is not handled by + * this function. + * \param firstPartial First object for computing partial derivatives. + * \param secondPartial Second object for computing partial derivatives. + * \param parameterObject Parameter w.r.t. which a partial is to be taken. + * \param firstPartialSize Size of partial from firstPartial object + * \param secondPartialSize Size of partial from secondPartial object + * \param subtractPartials Boolean denoting whether the second parameter is to be subtracted or added to the + * total partial. + * \return Function computing and returning (by reference) the combined partial according to the required settings. + */ +boost::function< void( Eigen::MatrixXd& ) > getCombinedCurrentVectorParameterFunction( + const boost::shared_ptr< StateDerivativePartial > firstPartial, + const boost::shared_ptr< StateDerivativePartial > secondPartial, + const boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > parameterObject, + const int firstPartialSize, const int secondPartialSize, + const bool subtractPartials = 0 ); + +} // namespace orbit_determination + +} // namespace tudat + +#endif // TUDAT_STATEDERIVATIVEPARTIAL_H diff --git a/Tudat/Astrodynamics/Propagators/CMakeLists.txt b/Tudat/Astrodynamics/Propagators/CMakeLists.txt index e772e56427..efea7ebaf7 100644 --- a/Tudat/Astrodynamics/Propagators/CMakeLists.txt +++ b/Tudat/Astrodynamics/Propagators/CMakeLists.txt @@ -35,24 +35,27 @@ # Add source files. set(PROPAGATORS_SOURCES - "${SRCROOT}${PROPAGATORSDIR}/createEnvironmentUpdater.cpp" - "${SRCROOT}${PROPAGATORSDIR}/setNumericallyIntegratedStates.cpp" + "${SRCROOT}${PROPAGATORSDIR}/nBodyStateDerivative.cpp" + "${SRCROOT}${PROPAGATORSDIR}/nBodyEnckeStateDerivative.cpp" + "${SRCROOT}${PROPAGATORSDIR}/variationalEquations.cpp" + "${SRCROOT}${PROPAGATORSDIR}/stateTransitionMatrixInterface.cpp" + "${SRCROOT}${PROPAGATORSDIR}/environmentUpdateTypes.cpp" + "${SRCROOT}${PROPAGATORSDIR}/singleStateTypeDerivative.cpp" ) # Add header files. set(PROPAGATORS_HEADERS - "${SRCROOT}${PROPAGATORSDIR}/createStateDerivativeModel.h" - "${SRCROOT}${PROPAGATORSDIR}/createEnvironmentUpdater.h" "${SRCROOT}${PROPAGATORSDIR}/centralBodyData.h" "${SRCROOT}${PROPAGATORSDIR}/nBodyStateDerivative.h" "${SRCROOT}${PROPAGATORSDIR}/nBodyCowellStateDerivative.h" - "${SRCROOT}${PROPAGATORSDIR}/propagationSettings.h" + "${SRCROOT}${PROPAGATORSDIR}/nBodyEnckeStateDerivative.h" "${SRCROOT}${PROPAGATORSDIR}/dynamicsStateDerivativeModel.h" "${SRCROOT}${PROPAGATORSDIR}/singleStateTypeDerivative.h" - "${SRCROOT}${PROPAGATORSDIR}/setNumericallyIntegratedStates.h" - "${SRCROOT}${PROPAGATORSDIR}/dynamicsSimulator.h" - "${SRCROOT}${PROPAGATORSDIR}/environmentUpdater.h" "${SRCROOT}${PROPAGATORSDIR}/integrateEquations.h" + "${SRCROOT}${PROPAGATORSDIR}/bodyMassStateDerivative.h" + "${SRCROOT}${PROPAGATORSDIR}/variationalEquations.h" + "${SRCROOT}${PROPAGATORSDIR}/stateTransitionMatrixInterface.h" + "${SRCROOT}${PROPAGATORSDIR}/environmentUpdateTypes.h" ) # Add static libraries. @@ -60,20 +63,50 @@ add_library(tudat_propagators STATIC ${PROPAGATORS_SOURCES} ${PROPAGATORS_HEADER setup_tudat_library_target(tudat_propagators "${SRCROOT}${PROPAGATORSDIR}") # Add unit tests. -add_executable(test_CentralBodyData - "${SRCROOT}${PROPAGATORSDIR}/UnitTests/unitTestCentralBodyData.cpp") +add_executable(test_CentralBodyData "${SRCROOT}${PROPAGATORSDIR}/UnitTests/unitTestCentralBodyData.cpp") setup_custom_test_program(test_CentralBodyData "${SRCROOT}${PROPAGATORSDIR}") -target_link_libraries(test_CentralBodyData tudat_propagators tudat_basic_mathematics - ${Boost_LIBRARIES}) +target_link_libraries(test_CentralBodyData tudat_propagators tudat_basic_mathematics ${Boost_LIBRARIES}) if(USE_CSPICE) -add_executable(test_CowellStateDerivative - "${SRCROOT}${PROPAGATORSDIR}/UnitTests/unitTestCowellStateDerivative.cpp") + +add_executable(test_CowellStateDerivative "${SRCROOT}${PROPAGATORSDIR}/UnitTests/unitTestCowellStateDerivative.cpp") setup_custom_test_program(test_CowellStateDerivative "${SRCROOT}${PROPAGATORSDIR}") target_link_libraries(test_CowellStateDerivative ${TUDAT_PROPAGATION_LIBRARIES} ${Boost_LIBRARIES}) -add_executable(test_EnvironmentModelUpdater - "${SRCROOT}${PROPAGATORSDIR}/UnitTests/unitTestEnvironmentUpdater.cpp") +add_executable(test_EnckeStateDerivative "${SRCROOT}${PROPAGATORSDIR}/UnitTests/unitTestEnckeStateDerivative.cpp") +setup_custom_test_program(test_EnckeStateDerivative "${SRCROOT}${PROPAGATORSDIR}") +target_link_libraries(test_EnckeStateDerivative ${TUDAT_PROPAGATION_LIBRARIES} ${Boost_LIBRARIES}) + + +add_executable(test_SequentialVariationEquationIntegration "${SRCROOT}${PROPAGATORSDIR}/UnitTests/unitTestSequentialVariationalEquationIntegration.cpp") +setup_custom_test_program(test_SequentialVariationEquationIntegration "${SRCROOT}${PROPAGATORSDIR}") +target_link_libraries(test_SequentialVariationEquationIntegration ${TUDAT_ESTIMATION_LIBRARIES} ${Boost_LIBRARIES}) + +add_executable(test_VariationalEquations "${SRCROOT}${PROPAGATORSDIR}/UnitTests/unitTestVariationalEquationPropagation.cpp") +setup_custom_test_program(test_VariationalEquations "${SRCROOT}${PROPAGATORSDIR}") +target_link_libraries(test_VariationalEquations ${TUDAT_ESTIMATION_LIBRARIES} ${Boost_LIBRARIES}) + +add_executable(test_EnvironmentModelUpdater "${SRCROOT}${PROPAGATORSDIR}/UnitTests/unitTestEnvironmentUpdater.cpp") setup_custom_test_program(test_EnvironmentModelUpdater "${SRCROOT}${PROPAGATORSDIR}") - target_link_libraries(test_EnvironmentModelUpdater ${TUDAT_PROPAGATION_LIBRARIES} ${Boost_LIBRARIES}) -endif() +target_link_libraries(test_EnvironmentModelUpdater ${TUDAT_PROPAGATION_LIBRARIES} ${Boost_LIBRARIES}) + + +add_executable(test_BodyMassPropagation "${SRCROOT}${PROPAGATORSDIR}/UnitTests/unitTestBodyMassPropagation.cpp") +setup_custom_test_program(test_BodyMassPropagation "${SRCROOT}${PROPAGATORSDIR}") +target_link_libraries(test_BodyMassPropagation ${TUDAT_PROPAGATION_LIBRARIES} ${Boost_LIBRARIES}) + + +add_executable(test_MultiTypeStatePropagation "${SRCROOT}${PROPAGATORSDIR}/UnitTests/unitTestMultiTypeStatePropagation.cpp") +setup_custom_test_program(test_MultiTypeStatePropagation "${SRCROOT}${PROPAGATORSDIR}") +target_link_libraries(test_MultiTypeStatePropagation ${TUDAT_PROPAGATION_LIBRARIES} ${Boost_LIBRARIES}) + + +add_executable(test_DependentVariableOutput "${SRCROOT}${PROPAGATORSDIR}/UnitTests/unitTestDependentVariableOutput.cpp") +setup_custom_test_program(test_DependentVariableOutput "${SRCROOT}${PROPAGATORSDIR}") +target_link_libraries(test_DependentVariableOutput ${TUDAT_PROPAGATION_LIBRARIES} ${Boost_LIBRARIES}) + +add_executable(test_StoppingConditions "${SRCROOT}${PROPAGATORSDIR}/UnitTests/unitTestStoppingConditions.cpp") +setup_custom_test_program(test_StoppingConditions "${SRCROOT}${PROPAGATORSDIR}") +target_link_libraries(test_StoppingConditions ${TUDAT_PROPAGATION_LIBRARIES} ${Boost_LIBRARIES}) + +endif( ) diff --git a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestBodyMassPropagation.cpp b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestBodyMassPropagation.cpp new file mode 100644 index 0000000000..78f0ab314f --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestBodyMassPropagation.cpp @@ -0,0 +1,155 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#define BOOST_TEST_MAIN + +#include + +#include +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/massRateModel.h" +#include "Tudat/SimulationSetup/PropagationSetup//propagationSettings.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" +#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h" + + +namespace tudat +{ + +namespace unit_tests +{ + +using namespace tudat::propagators; +using namespace tudat::simulation_setup; +using namespace tudat::numerical_integrators; + +BOOST_AUTO_TEST_SUITE( test_body_mass_propagation ) + +double getDummyMassRate1( + const NamedBodyMap bodyMap ) +{ + return ( bodyMap.at( "Vehicle1" )->getBodyMass( ) + 2.0 * bodyMap.at( "Vehicle2" )->getBodyMass( ) ) / 1.0E4; +} + +double getDummyMassRate2( + const NamedBodyMap bodyMap ) +{ + return ( 3.0 * bodyMap.at( "Vehicle1" )->getBodyMass( ) + 2.0 * bodyMap.at( "Vehicle2" )->getBodyMass( ) ) / 1.0E4; +} + +// Test mass rate of single body, linearly decreasing with time +BOOST_AUTO_TEST_CASE( testBodyMassPropagation ) +{ + // Crate bodyMap + NamedBodyMap bodyMap; + bodyMap[ "Vehicle" ] = boost::make_shared< Body >( ); + + // Create mass rate model. + std::map< std::string, boost::shared_ptr< basic_astrodynamics::MassRateModel > > massRateModels; + massRateModels[ "Vehicle" ] = boost::make_shared< basic_astrodynamics::CustomMassRateModel >( + boost::lambda::constant( -0.01 ) ); + + // Create settings for propagation + Eigen::VectorXd initialMass = Eigen::VectorXd( 1 ); + initialMass( 0 ) = 500.0; + boost::shared_ptr< PropagatorSettings< double > > propagatorSettings = + boost::make_shared< MassPropagatorSettings< double > >( + boost::assign::list_of( "Vehicle" ), massRateModels, initialMass, + boost::make_shared< PropagationTimeTerminationSettings >( 1000.0 ) ); + + // Define numerical integrator settings. + boost::shared_ptr< IntegratorSettings< > > integratorSettings = + boost::make_shared< IntegratorSettings< > >( rungeKutta4, 0.0, 1.0 ); + + // Create dynamics simulation object. + SingleArcDynamicsSimulator< double, double > dynamicsSimulator( + bodyMap, integratorSettings, propagatorSettings, true, false, false ); + + // Test propagated solution. + std::map< double, Eigen::VectorXd > integratedState = dynamicsSimulator.getEquationsOfMotionNumericalSolution( ); + for( std::map< double, Eigen::VectorXd >::const_iterator stateIterator = integratedState.begin( ); + stateIterator != integratedState.end( ); stateIterator++ ) + { + BOOST_CHECK_CLOSE_FRACTION( stateIterator->second( 0 ), 500.0 - 0.01 * stateIterator->first, 1.0E-13 ); + } +} + +// Test coupled mass rate of two bodies. Model ius unphysical, but has an analytical solution, and allows the internal +// workings of the mass propagation to be more rigorously tested. +BOOST_AUTO_TEST_CASE( testTwoBodyMassPropagation ) +{ + // Crate bodyMap + NamedBodyMap bodyMap; + bodyMap[ "Vehicle1" ] = boost::make_shared< Body >( ); + bodyMap[ "Vehicle2" ] = boost::make_shared< Body >( ); + + // Create mass rate models. + std::map< std::string, boost::shared_ptr< basic_astrodynamics::MassRateModel > > massRateModels; + massRateModels[ "Vehicle1" ] = boost::make_shared< basic_astrodynamics::CustomMassRateModel >( + boost::bind( &getDummyMassRate1, bodyMap ) ); + massRateModels[ "Vehicle2" ] = boost::make_shared< basic_astrodynamics::CustomMassRateModel >( + boost::bind( &getDummyMassRate2, bodyMap ) ); + bodyMap[ "Earth" ] = boost::make_shared< Body >( ); + bodyMap[ "Earth" ]->setEphemeris( boost::make_shared< ephemerides::ConstantEphemeris >( + boost::lambda::constant( basic_mathematics::Vector6d::Zero( ) ) ) ); + bodyMap[ "Vehicle1" ]->setEphemeris( boost::make_shared< ephemerides::ConstantEphemeris >( + boost::lambda::constant( basic_mathematics::Vector6d::Zero( ) ), "Earth" ) ); + bodyMap[ "Vehicle2" ]->setEphemeris( boost::make_shared< ephemerides::ConstantEphemeris >( + boost::lambda::constant( basic_mathematics::Vector6d::Zero( ) ), "Earth" ) ); + + // Create settings for propagation + Eigen::VectorXd initialMass = Eigen::VectorXd( 2 ); + initialMass( 0 ) = 500.0; + initialMass( 1 ) = 1000.0; + boost::shared_ptr< PropagatorSettings< double > > propagatorSettings = + boost::make_shared< MassPropagatorSettings< double > >( + boost::assign::list_of( "Vehicle1" )( "Vehicle2" ), massRateModels, initialMass, + boost::make_shared< PropagationTimeTerminationSettings >( 1000.0 ) ); + + // Define numerical integrator settings. + boost::shared_ptr< IntegratorSettings< > > integratorSettings = + boost::make_shared< IntegratorSettings< > >( rungeKutta4, 0.0, 1.0 ); + + // Create dynamics simulation object. + SingleArcDynamicsSimulator< double, double > dynamicsSimulator( + bodyMap, integratorSettings, propagatorSettings, true, false, true ); + + // Test propagated solution. + std::map< double, Eigen::VectorXd > integratedState = dynamicsSimulator.getEquationsOfMotionNumericalSolution( ); + for( std::map< double, Eigen::VectorXd >::const_iterator stateIterator = integratedState.begin( ); + stateIterator != integratedState.end( ); stateIterator++ ) + { + // Test directly + BOOST_CHECK_CLOSE_FRACTION( + stateIterator->second( 0 ), + 100.0 * ( -std::exp( -stateIterator->first / 1E4 ) + + 6.0 * std::exp( 4.0 * stateIterator->first / 1E4 ) ), 1.0E-13 ); + BOOST_CHECK_CLOSE_FRACTION( + stateIterator->second( 1 ), + 100.0 * ( std::exp( -stateIterator->first / 1E4 ) + + 9.0 * std::exp( 4.0 * stateIterator->first / 1E4 ) ), 1.0E-13 ); + + // Test reset mass solution of vehicles. + bodyMap[ "Vehicle1" ]->updateMass( stateIterator->first ); + bodyMap[ "Vehicle2" ]->updateMass( stateIterator->first ); + + BOOST_CHECK_CLOSE_FRACTION( stateIterator->second( 0 ), bodyMap[ "Vehicle1" ]->getBodyMass( ), + std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( stateIterator->second( 1 ), bodyMap[ "Vehicle2" ]->getBodyMass( ), + std::numeric_limits< double >::epsilon( ) ); + } +} + +BOOST_AUTO_TEST_SUITE_END( ) + +} // namespace unit_tests + +} // namespace tudat diff --git a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestCentralBodyData.cpp b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestCentralBodyData.cpp index 9e01e3e13d..1d9f7a1c18 100644 --- a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestCentralBodyData.cpp +++ b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestCentralBodyData.cpp @@ -16,7 +16,7 @@ #include #include "Tudat/Astrodynamics/Propagators/nBodyStateDerivative.h" -#include "Tudat/SimulationSetup/body.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" namespace tudat { diff --git a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestCowellStateDerivative.cpp b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestCowellStateDerivative.cpp index 5f17cfd07f..1bc9eb92b2 100644 --- a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestCowellStateDerivative.cpp +++ b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestCowellStateDerivative.cpp @@ -27,13 +27,13 @@ #include "Tudat/InputOutput/basicInputOutput.h" #include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h" -#include "Tudat/SimulationSetup/body.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" #include "Tudat/Astrodynamics/Propagators/nBodyCowellStateDerivative.h" -#include "Tudat/Astrodynamics/Propagators/dynamicsSimulator.h" +#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h" #include "Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h" -#include "Tudat/SimulationSetup/createBodies.h" -#include "Tudat/SimulationSetup/createAccelerationModels.h" -#include "Tudat/SimulationSetup/defaultBodies.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h" +#include "Tudat/SimulationSetup/PropagationSetup/createNumericalSimulator.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h" namespace tudat @@ -83,125 +83,130 @@ BOOST_AUTO_TEST_CASE( testCowellPopagatorCentralBodies ) // Create bodies needed in simulation std::map< std::string, boost::shared_ptr< BodySettings > > bodySettings = - getDefaultBodySettings( bodyNames, initialEphemerisTime - buffer, - finalEphemerisTime + buffer ); + getDefaultBodySettings( bodyNames, initialEphemerisTime - buffer, finalEphemerisTime + buffer ); bodySettings[ "Mars" ]->ephemerisSettings->resetFrameOrigin( "Earth" ); bodySettings[ "Earth" ]->ephemerisSettings->resetFrameOrigin( "Sun" ); bodySettings[ "Moon" ]->ephemerisSettings->resetFrameOrigin( "Earth" ); - std::map< std::string, boost::shared_ptr< Body > > bodyMap = createBodies( bodySettings ); + NamedBodyMap bodyMap = createBodies( bodySettings ); setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" ); - // Set accelerations between bodies that are to be taken into account (mutual point mass gravity - // between all bodies). + // Set accelerations between bodies that are to be taken into account (mutual point mass gravity between all bodies). SelectedAccelerationMap accelerationMap; - std::map< std::string, - std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfEarth; - accelerationsOfEarth[ "Sun" ].push_back( - boost::make_shared< AccelerationSettings >( central_gravity ) ); - accelerationsOfEarth[ "Moon" ].push_back( - boost::make_shared< AccelerationSettings >( central_gravity ) ); - accelerationsOfEarth[ "Mars" ].push_back( - boost::make_shared< AccelerationSettings >( central_gravity ) ); + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfEarth; + accelerationsOfEarth[ "Sun" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationsOfEarth[ "Moon" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationsOfEarth[ "Mars" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); accelerationMap[ "Earth" ] = accelerationsOfEarth; - std::map< std::string, - std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfSun; - accelerationsOfSun[ "Moon" ].push_back( - boost::make_shared< AccelerationSettings >( central_gravity ) ); - accelerationsOfSun[ "Earth" ].push_back( - boost::make_shared< AccelerationSettings >( central_gravity ) ); - accelerationsOfSun[ "Mars" ].push_back( - boost::make_shared< AccelerationSettings >( central_gravity ) ); + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfSun; + accelerationsOfSun[ "Moon" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationsOfSun[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationsOfSun[ "Mars" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); accelerationMap[ "Sun" ] = accelerationsOfSun; - std::map< std::string, - std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfMoon; - accelerationsOfMoon[ "Sun" ].push_back( - boost::make_shared< AccelerationSettings >( central_gravity ) ); - accelerationsOfMoon[ "Earth" ].push_back( - boost::make_shared< AccelerationSettings >( central_gravity ) ); - accelerationsOfMoon[ "Mars" ].push_back( - boost::make_shared< AccelerationSettings >( central_gravity ) ); + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfMoon; + accelerationsOfMoon[ "Sun" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationsOfMoon[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationsOfMoon[ "Mars" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); accelerationMap[ "Moon" ] = accelerationsOfMoon; - std::map< std::string, - std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfMars; - accelerationsOfMars[ "Sun" ].push_back( - boost::make_shared< AccelerationSettings >( central_gravity ) ); - accelerationsOfMars[ "Earth" ].push_back( - boost::make_shared< AccelerationSettings >( central_gravity ) ); - accelerationsOfMars[ "Moon" ].push_back( - boost::make_shared< AccelerationSettings >( central_gravity ) ); + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfMars; + accelerationsOfMars[ "Sun" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationsOfMars[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationsOfMars[ "Moon" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); accelerationMap[ "Mars" ] = accelerationsOfMars; // Define list of bodies to propagate - std::vector< std::string > bodiesToPropagate; - bodiesToPropagate.push_back( "Earth" ); - bodiesToPropagate.push_back( "Sun" ); - bodiesToPropagate.push_back( "Moon" ); - bodiesToPropagate.push_back( "Mars" ); - unsigned int numberOfNumericalBodies = bodiesToPropagate.size( ); + std::vector< std::string > bodiesToIntegrate; + bodiesToIntegrate.push_back( "Earth" ); + bodiesToIntegrate.push_back( "Sun" ); + bodiesToIntegrate.push_back( "Moon" ); + bodiesToIntegrate.push_back( "Mars" ); + unsigned int numberOfNumericalBodies = bodiesToIntegrate.size( ); // Define numerical integrator settings. boost::shared_ptr< IntegratorSettings< > > integratorSettings = boost::make_shared< IntegratorSettings< > > - ( rungeKutta4, initialEphemerisTime, finalEphemerisTime, 240.0 ); + ( rungeKutta4, initialEphemerisTime, 200.0 ); // Define central bodies to use in propagation (all w.r.t SSB). std::vector< std::string > centralBodies; + std::map< std::string, std::string > centralBodyMap; centralBodies.resize( numberOfNumericalBodies ); for( unsigned int i = 0; i < numberOfNumericalBodies; i++ ) { centralBodies[ i ] = "SSB"; + centralBodyMap[ bodiesToIntegrate[ i ] ] = centralBodies[ i ]; } // Get initial state vector as input to integration. Eigen::VectorXd systemInitialState = getInitialStatesOfBodies( - bodiesToPropagate, centralBodies, bodyMap, initialEphemerisTime ); + bodiesToIntegrate, centralBodies, bodyMap, initialEphemerisTime ); // Create acceleration models and propagation settings. AccelerationMap accelerationModelMap = createAccelerationModelsMap( - bodyMap, accelerationMap, bodiesToPropagate, centralBodies ); + bodyMap, accelerationMap, centralBodyMap ); boost::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings = boost::make_shared< TranslationalStatePropagatorSettings< double > > - ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState ); + ( centralBodies, accelerationModelMap, bodiesToIntegrate, systemInitialState, finalEphemerisTime ); // Create simulation object and propagate dynamics. SingleArcDynamicsSimulator< > dynamicsSimulator( bodyMap, integratorSettings, propagatorSettings, true, false, false ); + std::map< double, Eigen::VectorXd > solutionSet1 = dynamicsSimulator.getEquationsOfMotionNumericalSolution( ); // Define new central bodies (hierarchical system) centralBodies[ 0 ] = "Sun"; centralBodies[ 1 ] = "SSB"; centralBodies[ 2 ] = "Earth"; centralBodies[ 3 ] = "Sun"; + for( unsigned int i = 0; i < numberOfNumericalBodies; i++ ) + { + centralBodyMap[ bodiesToIntegrate[ i ] ] = centralBodies[ i ]; + } systemInitialState = getInitialStatesOfBodies( - bodiesToPropagate, centralBodies, bodyMap, initialEphemerisTime ); + bodiesToIntegrate, centralBodies, bodyMap, initialEphemerisTime ); // Create new acceleration models and propagation settings. AccelerationMap accelerationModelMap2 = createAccelerationModelsMap( - bodyMap, accelerationMap, bodiesToPropagate, centralBodies ); + bodyMap, accelerationMap, centralBodyMap ); boost::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings2 = boost::make_shared< TranslationalStatePropagatorSettings< double > > - ( centralBodies, accelerationModelMap2, bodiesToPropagate, systemInitialState ); + ( centralBodies, accelerationModelMap2, bodiesToIntegrate, systemInitialState, finalEphemerisTime ); + + // Create new simulation object and propagate dynamics. SingleArcDynamicsSimulator< > dynamicsSimulator2( bodyMap, integratorSettings, propagatorSettings2, true, false ); + std::map< double, Eigen::VectorXd > solutionSet2 = dynamicsSimulator2.getEquationsOfMotionNumericalSolution( ); + + // Create integration and propagation settings for reverse in time propagation + std::map< double, Eigen::VectorXd >::iterator solutionSetIterator = (--solutionSet2.end( ) ); + Eigen::VectorXd systemFinalState = solutionSetIterator->second; + boost::shared_ptr< IntegratorSettings< > > integratorSettings2 = + boost::make_shared< IntegratorSettings< > > + ( rungeKutta4, solutionSetIterator->first, -200.0 ); + boost::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings3 = + boost::make_shared< TranslationalStatePropagatorSettings< double > > + ( centralBodies, accelerationModelMap2, bodiesToIntegrate, systemFinalState, initialEphemerisTime ); - // Retrieve dynamics solution for the two different central body settings and create - // interpolators. - std::map< double, Eigen::VectorXd > solutionSet1 - = dynamicsSimulator.getEquationsOfMotionNumericalSolution( ); - std::map< double, Eigen::VectorXd > solutionSet2 - = dynamicsSimulator2.getEquationsOfMotionNumericalSolution( ); + // Create new simulation object and propagate dynamics backwards in time. + SingleArcDynamicsSimulator< > dynamicsSimulator3( + bodyMap, integratorSettings2, propagatorSettings3, true, false, false ); + std::map< double, Eigen::VectorXd > solutionSet3 = dynamicsSimulator3.getEquationsOfMotionNumericalSolution( ); - LagrangeInterpolator< double, Eigen::VectorXd > interpolator1( solutionSet1, 8 ); - LagrangeInterpolator< double, Eigen::VectorXd > interpolator2( solutionSet2, 8 ); + // Create interpolators from three numerical solutions (first one is inertial; second and third are non-inertial) + boost::shared_ptr< LagrangeInterpolator< double, Eigen::VectorXd > > interpolator1 = + boost::make_shared< LagrangeInterpolator< double, Eigen::VectorXd > >( solutionSet1, 8 ); + boost::shared_ptr< LagrangeInterpolator< double, Eigen::VectorXd > > interpolator2 = + boost::make_shared< LagrangeInterpolator< double, Eigen::VectorXd > >( solutionSet2, 8 ); + boost::shared_ptr< LagrangeInterpolator< double, Eigen::VectorXd > > interpolator3 = + boost::make_shared< LagrangeInterpolator< double, Eigen::VectorXd > >( solutionSet3, 8 ); // Define step size to be out of sync with integration step size. double stepSize = 2001.1 + mathematical_constants::PI; @@ -210,87 +215,97 @@ BOOST_AUTO_TEST_CASE( testCowellPopagatorCentralBodies ) std::map< double, Eigen::VectorXd > analyticalSolutions; // Define maps to retrieve propagated orbits from interpolator. - Eigen::VectorXd currentInertialSolution - = Eigen::VectorXd::Zero( 6 * numberOfNumericalBodies ); - Eigen::VectorXd currentNonInertialSolution - = Eigen::VectorXd::Zero( 6 * numberOfNumericalBodies ); + Eigen::VectorXd currentInertialSolution = Eigen::VectorXd::Zero( 6 * numberOfNumericalBodies ); + Eigen::VectorXd currentNonInertialSolution = Eigen::VectorXd::Zero( 6 * numberOfNumericalBodies ); // Define map to put inertial orbit reconstructed from non-inertial orbits. - Eigen::VectorXd reconstructedInertialSolution - = Eigen::VectorXd::Zero( 6 * numberOfNumericalBodies ); + Eigen::VectorXd reconstructedInertialSolution = Eigen::VectorXd::Zero( 6 * numberOfNumericalBodies ); // Define error maps. Eigen::VectorXd stateDifference = Eigen::VectorXd::Zero( 6 * numberOfNumericalBodies ); // Test numerical output against results with SSB as origin for ech body, - boost::shared_ptr< ephemerides::Ephemeris > sunEphemeris - = bodyMap[ "Sun" ]->getEphemeris( ); + boost::shared_ptr< ephemerides::Ephemeris > sunEphemeris = bodyMap[ "Sun" ]->getEphemeris( ); + boost::shared_ptr< ephemerides::Ephemeris > earthEphemeris = bodyMap[ "Earth" ]->getEphemeris( ); + boost::shared_ptr< ephemerides::Ephemeris > marsEphemeris = bodyMap[ "Mars" ]->getEphemeris( ); + boost::shared_ptr< ephemerides::Ephemeris > moonEphemeris = bodyMap[ "Moon" ]->getEphemeris( ); + + boost::shared_ptr< LagrangeInterpolator< double, Eigen::VectorXd > > currentInterpolator; + while( currentTime < finalEphemerisTime - stepSize ) { // Retrieve data from interpolators; transform to inertial frames and compare. - currentInertialSolution = interpolator1.interpolate( currentTime ); - currentNonInertialSolution = interpolator2.interpolate( currentTime ); - reconstructedInertialSolution.segment( 0, 6 ) = currentNonInertialSolution.segment( 0, 6 ) - + sunEphemeris->getCartesianStateFromEphemeris( currentTime ); - reconstructedInertialSolution.segment( 6, 6 ) = currentNonInertialSolution.segment( 6, 6 ); - reconstructedInertialSolution.segment( 12, 6 ) - = currentNonInertialSolution.segment( 12, 6 ) - + reconstructedInertialSolution.segment( 0, 6 ); - reconstructedInertialSolution.segment( 18, 6 ) - = currentNonInertialSolution.segment( 18, 6 ) - + sunEphemeris->getCartesianStateFromEphemeris( currentTime ); + currentInertialSolution = interpolator1->interpolate( currentTime ); - // Compare states. - stateDifference = reconstructedInertialSolution - currentInertialSolution; - for( unsigned j = 0; j < 4; j++ ) + + for( unsigned int k = 0; k < 2; k++ ) { - if( j != 2 ) + if( k == 0 ) { - BOOST_CHECK_SMALL( std::fabs( stateDifference( 0 + 6 * j ) ), 1.0E-2 ); - BOOST_CHECK_SMALL( std::fabs( stateDifference( 1 + 6 * j ) ), 1.0E-2 ); - BOOST_CHECK_SMALL( std::fabs( stateDifference( 2 + 6 * j ) ), 1.0E-2 ); - - BOOST_CHECK_SMALL( std::fabs( stateDifference( 3 + 6 * j ) ), 5.0E-9 ); - BOOST_CHECK_SMALL( std::fabs( stateDifference( 4 + 6 * j ) ), 5.0E-9 ); - BOOST_CHECK_SMALL( std::fabs( stateDifference( 5 + 6 * j ) ), 5.0E-9 ); + currentInterpolator = interpolator2; } else { - BOOST_CHECK_SMALL( std::fabs( stateDifference( 0 + 6 * j ) ), 1.0E-1 ); - BOOST_CHECK_SMALL( std::fabs( stateDifference( 1 + 6 * j ) ), 1.0E-1 ); - BOOST_CHECK_SMALL( std::fabs( stateDifference( 2 + 6 * j ) ), 1.0E-1 ); + currentInterpolator = interpolator3; + } - BOOST_CHECK_SMALL( std::fabs( stateDifference( 3 + 6 * j ) ), 2.0E-7 ); - BOOST_CHECK_SMALL( std::fabs( stateDifference( 4 + 6 * j ) ), 2.0E-7 ); - BOOST_CHECK_SMALL( std::fabs( stateDifference( 5 + 6 * j ) ), 2.0E-7 ); + currentNonInertialSolution = currentInterpolator->interpolate( currentTime ); + + reconstructedInertialSolution.segment( 0, 6 ) = currentNonInertialSolution.segment( 0, 6 ) + + currentNonInertialSolution.segment( 6, 6 ); + reconstructedInertialSolution.segment( 6, 6 ) = currentNonInertialSolution.segment( 6, 6 ); + reconstructedInertialSolution.segment( 12, 6 )= currentNonInertialSolution.segment( 12, 6 ) + + reconstructedInertialSolution.segment( 0, 6 ); + reconstructedInertialSolution.segment( 18, 6 ) = currentNonInertialSolution.segment( 18, 6 ) + + reconstructedInertialSolution.segment( 6, 6 ); + + // Compare states. + stateDifference = reconstructedInertialSolution - currentInertialSolution; + + for( unsigned j = 0; j < 4; j++ ) + { + if( j != 2 ) + { + BOOST_CHECK_SMALL( std::fabs( stateDifference( 0 + 6 * j ) ), 1.0E-2 ); + BOOST_CHECK_SMALL( std::fabs( stateDifference( 1 + 6 * j ) ), 1.0E-2 ); + BOOST_CHECK_SMALL( std::fabs( stateDifference( 2 + 6 * j ) ), 1.0E-2 ); + + BOOST_CHECK_SMALL( std::fabs( stateDifference( 3 + 6 * j ) ), 5.0E-9 ); + BOOST_CHECK_SMALL( std::fabs( stateDifference( 4 + 6 * j ) ), 5.0E-9 ); + BOOST_CHECK_SMALL( std::fabs( stateDifference( 5 + 6 * j ) ), 5.0E-9 ); + } + else + { + BOOST_CHECK_SMALL( std::fabs( stateDifference( 0 + 6 * j ) ), 1.0E-1 ); + BOOST_CHECK_SMALL( std::fabs( stateDifference( 1 + 6 * j ) ), 1.0E-1 ); + BOOST_CHECK_SMALL( std::fabs( stateDifference( 2 + 6 * j ) ), 1.0E-1 ); + + BOOST_CHECK_SMALL( std::fabs( stateDifference( 3 + 6 * j ) ), 2.0E-7 ); + BOOST_CHECK_SMALL( std::fabs( stateDifference( 4 + 6 * j ) ), 2.0E-7 ); + BOOST_CHECK_SMALL( std::fabs( stateDifference( 5 + 6 * j ) ), 2.0E-7 ); + } } } - currentTime += stepSize; - } - // Test whether ephemeris objects have been properly reset, i.e. whether all states have been - // properly transformed to the ephemeris frame. - boost::shared_ptr< ephemerides::Ephemeris > earthEphemeris = bodyMap[ "Earth" ]->getEphemeris( ); - boost::shared_ptr< ephemerides::Ephemeris > marsEphemeris = bodyMap[ "Mars" ]->getEphemeris( ); - boost::shared_ptr< ephemerides::Ephemeris > moonEphemeris = bodyMap[ "Moon" ]->getEphemeris( ); - while( currentTime < finalEphemerisTime - stepSize ) - { + + // Test whether ephemeris objects have been properly reset, i.e. whether all states have been properly transformed to the + // ephemeris frame. + // Retrieve data from interpolators; transform to inertial frames and compare. - currentInertialSolution = interpolator1.interpolate( currentTime ); - - reconstructedInertialSolution.segment( 0, 6 ) - = earthEphemeris->getCartesianStateFromEphemeris( currentTime ) - + sunEphemeris->getCartesianStateFromEphemeris( currentTime ); - reconstructedInertialSolution.segment( 6, 6 ) - = sunEphemeris->getCartesianStateFromEphemeris( currentTime ); - reconstructedInertialSolution.segment( 12, 6 ) - = moonEphemeris->getCartesianStateFromEphemeris( currentTime ) - + earthEphemeris->getCartesianStateFromEphemeris( currentTime ) - + sunEphemeris->getCartesianStateFromEphemeris( currentTime ); - reconstructedInertialSolution.segment( 18, 6 ) - = marsEphemeris->getCartesianStateFromEphemeris( currentTime ) - + sunEphemeris->getCartesianStateFromEphemeris( currentTime ); + currentInertialSolution = interpolator1->interpolate( currentTime ); + + reconstructedInertialSolution.segment( 0, 6 ) = earthEphemeris->getCartesianStateFromEphemeris( currentTime ) + + sunEphemeris->getCartesianStateFromEphemeris( currentTime ); + reconstructedInertialSolution.segment( 6, 6 ) = sunEphemeris->getCartesianStateFromEphemeris( currentTime ); + reconstructedInertialSolution.segment( 12, 6 ) = + moonEphemeris->getCartesianStateFromEphemeris( currentTime ) + + earthEphemeris->getCartesianStateFromEphemeris( currentTime ) + + sunEphemeris->getCartesianStateFromEphemeris( currentTime ); + reconstructedInertialSolution.segment( 18, 6 ) = + marsEphemeris->getCartesianStateFromEphemeris( currentTime ) + + earthEphemeris->getCartesianStateFromEphemeris( currentTime ) + + sunEphemeris->getCartesianStateFromEphemeris( currentTime ); // Compare states. stateDifference = reconstructedInertialSolution - currentInertialSolution; @@ -325,7 +340,8 @@ BOOST_AUTO_TEST_CASE( testCowellPopagatorCentralBodies ) //! Test to ensure that a point-mass acceleration on a body produces a Kepler orbit (to within //! numerical error bounds). -BOOST_AUTO_TEST_CASE( testCowellPopagatorKeplerCompare ) +template< typename TimeType, typename StateScalarType > +void testCowellPropagationOfKeplerOrbit( ) { //Load spice kernels. std::string kernelsPath = input_output::getSpiceKernelPath( ); @@ -348,46 +364,49 @@ BOOST_AUTO_TEST_CASE( testCowellPopagatorKeplerCompare ) // Create bodies needed in simulation std::map< std::string, boost::shared_ptr< BodySettings > > bodySettings = - getDefaultBodySettings( bodyNames, - initialEphemerisTime - buffer, finalEphemerisTime + buffer ); + getDefaultBodySettings( bodyNames, initialEphemerisTime - buffer, finalEphemerisTime + buffer ); + + if( std::is_same< long double, StateScalarType >::value ) + { + boost::dynamic_pointer_cast< InterpolatedSpiceEphemerisSettings >( + bodySettings[ "Moon" ]->ephemerisSettings )->setUseLongDoubleStates( 1 ); + } // Change ephemeris settings of Moon and Earth to make test results analysis more transparent. - boost::dynamic_pointer_cast< InterpolatedSpiceEphemerisSettings > - ( bodySettings[ "Moon" ]->ephemerisSettings )->resetFrameOrigin( "Earth" ); + boost::dynamic_pointer_cast< InterpolatedSpiceEphemerisSettings >( bodySettings[ "Moon" ]->ephemerisSettings )-> + resetFrameOrigin( "Earth" ); bodySettings[ "Earth" ]->ephemerisSettings = boost::make_shared< ConstantEphemerisSettings >( basic_mathematics::Vector6d::Zero( ), "SSB", "ECLIPJ2000" ); - std::map< std::string, boost::shared_ptr< Body > > bodyMap = createBodies( bodySettings ); + NamedBodyMap bodyMap = createBodies( bodySettings ); setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" ); // Set accelerations between bodies that are to be taken into account. SelectedAccelerationMap accelerationMap; - std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > - accelerationsOfMoon; - accelerationsOfMoon[ "Earth" ].push_back( boost::make_shared< AccelerationSettings > - ( central_gravity ) ); + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfMoon; + accelerationsOfMoon[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); accelerationMap[ "Moon" ] = accelerationsOfMoon; // Propagate the moon only - std::vector< std::string > bodiesToPropagate; - bodiesToPropagate.push_back( "Moon" ); - unsigned int numberOfNumericalBodies = bodiesToPropagate.size( ); + std::vector< std::string > bodiesToIntegrate; + bodiesToIntegrate.push_back( "Moon" ); + unsigned int numberOfNumericalBodies = bodiesToIntegrate.size( ); // Define settings for numerical integrator. boost::shared_ptr< IntegratorSettings< > > integratorSettings = boost::make_shared< IntegratorSettings< > > - ( rungeKutta4, initialEphemerisTime, finalEphemerisTime, 120.0 ); + ( rungeKutta4, initialEphemerisTime, 120.0 ); // Run test where Moon gravity is/is not taken into account. for( unsigned testCase = 0; testCase < 2; testCase++ ) { // Get initial kepler elements - double effectiveGravitationalParameter; + StateScalarType effectiveGravitationalParameter; if( testCase == 0 ) { - effectiveGravitationalParameter - = bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( ) - + bodyMap.at( "Moon" )->getGravityFieldModel( )->getGravitationalParameter( ); + effectiveGravitationalParameter = + bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( ) + + bodyMap.at( "Moon" )->getGravityFieldModel( )->getGravitationalParameter( ); } else { @@ -397,6 +416,7 @@ BOOST_AUTO_TEST_CASE( testCowellPopagatorKeplerCompare ) // Define central bodies for integration. std::vector< std::string > centralBodies; + std::map< std::string, std::string > centralBodyMap; if( testCase == 0 ) { @@ -412,42 +432,50 @@ BOOST_AUTO_TEST_CASE( testCowellPopagatorKeplerCompare ) bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( ); centralBodies.push_back( "SSB" ); } + centralBodyMap[ bodiesToIntegrate[ 0 ] ] = centralBodies[ 0 ]; + // Create system initial state. - Eigen::VectorXd systemInitialState = Eigen::VectorXd( bodiesToPropagate.size( ) * 6 ); + Eigen::Matrix< StateScalarType, 6, 1 > systemInitialState = + Eigen::Matrix< StateScalarType, 6, 1 >( bodiesToIntegrate.size( ) * 6 ); for( unsigned int i = 0; i < numberOfNumericalBodies ; i++ ) { systemInitialState.segment( i * 6 , 6 ) = spice_interface::getBodyCartesianStateAtEpoch( - bodiesToPropagate[ i ], "Earth", "ECLIPJ2000", "NONE", initialEphemerisTime ); + bodiesToIntegrate[ i ], "Earth", "ECLIPJ2000", "NONE", initialEphemerisTime ). + template cast< StateScalarType >( ); } // Create acceleration models and propagation settings. AccelerationMap accelerationModelMap = createAccelerationModelsMap( - bodyMap, accelerationMap, bodiesToPropagate, centralBodies ); - boost::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings = - boost::make_shared< TranslationalStatePropagatorSettings< double > > - ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState ); + bodyMap, accelerationMap, bodiesToIntegrate, centralBodies ); + boost::shared_ptr< TranslationalStatePropagatorSettings< StateScalarType > > propagatorSettings = + boost::make_shared< TranslationalStatePropagatorSettings< StateScalarType > > + ( centralBodies, accelerationModelMap, bodiesToIntegrate, systemInitialState, finalEphemerisTime ); // Create dynamics simulation object. - SingleArcDynamicsSimulator< > dynamicsSimulator( + SingleArcDynamicsSimulator< StateScalarType, TimeType > dynamicsSimulator( bodyMap, integratorSettings, propagatorSettings, true, false ); - basic_mathematics::Vector6d initialKeplerElements = - orbital_element_conversions::convertCartesianToKeplerianElements( - basic_mathematics::Vector6d( systemInitialState ), effectiveGravitationalParameter ); + Eigen::Matrix< StateScalarType, 6, 1 > initialKeplerElements = + orbital_element_conversions::convertCartesianToKeplerianElements< StateScalarType >( + Eigen::Matrix< StateScalarType, 6, 1 >( systemInitialState ), effectiveGravitationalParameter ); + // Compare numerical state and kepler orbit at each time step. boost::shared_ptr< Ephemeris > moonEphemeris = bodyMap.at( "Moon" )->getEphemeris( ); double currentTime = initialEphemerisTime + buffer; while( currentTime < finalEphemerisTime - buffer ) { - basic_mathematics::Vector6d stateDifference - = orbital_element_conversions::convertKeplerianToCartesianElements( - propagateKeplerOrbit( initialKeplerElements, currentTime - initialEphemerisTime, + + Eigen::VectorXd stateDifference + = ( orbital_element_conversions::convertKeplerianToCartesianElements( + propagateKeplerOrbit< StateScalarType >( initialKeplerElements, currentTime - initialEphemerisTime, effectiveGravitationalParameter ), effectiveGravitationalParameter ) - - moonEphemeris->getCartesianStateFromEphemeris( currentTime ); + - moonEphemeris->template getTemplatedStateFromEphemeris< StateScalarType >( currentTime ) ). + template cast< double >( ); + for( int i = 0; i < 3; i++ ) { BOOST_CHECK_SMALL( stateDifference( i ), 1E-3 ); @@ -458,10 +486,16 @@ BOOST_AUTO_TEST_CASE( testCowellPopagatorKeplerCompare ) } } } +BOOST_AUTO_TEST_CASE( testCowellPopagatorKeplerCompare ) +{ + testCowellPropagationOfKeplerOrbit< double, double >( ); + testCowellPropagationOfKeplerOrbit< double, long double >( ); + +} BOOST_AUTO_TEST_SUITE_END( ) -} // namespace unit_tests +} -} // namespace tudat +} diff --git a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestDependentVariableOutput.cpp b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestDependentVariableOutput.cpp new file mode 100644 index 0000000000..241c52ccec --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestDependentVariableOutput.cpp @@ -0,0 +1,280 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#define BOOST_TEST_MAIN + +#include +#include +#include +#include + +#include +#include +#include +#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h" +#include +#include +#include +#include "Tudat/SimulationSetup/PropagationSetup/createNumericalSimulator.h" +#include +#include + +#include +#include +#include + +#include + +namespace tudat +{ + +namespace unit_tests +{ + +BOOST_AUTO_TEST_SUITE( test_dependent_variable_output ) + +//! Propagate entry of Apollo capsule, and save a list of dependent variables during entry. The saved dependent variables +//! are compred against theoretical/manual values in this test. +BOOST_AUTO_TEST_CASE( testDependentVariableOutput ) +{ + using namespace tudat; + using namespace ephemerides; + using namespace interpolators; + using namespace numerical_integrators; + using namespace spice_interface; + using namespace simulation_setup; + using namespace basic_astrodynamics; + using namespace orbital_element_conversions; + using namespace propagators; + using namespace aerodynamics; + using namespace basic_mathematics; + using namespace input_output; + + // Load Spice kernels. + spice_interface::loadSpiceKernelInTudat( input_output::getSpiceKernelPath( ) + "pck00009.tpc" ); + spice_interface::loadSpiceKernelInTudat( input_output::getSpiceKernelPath( ) + "de-403-masses.tpc" ); + spice_interface::loadSpiceKernelInTudat( input_output::getSpiceKernelPath( ) + "de421.bsp" ); + + + // Set simulation start epoch. + const double simulationStartEpoch = 0.0; + + // Set simulation end epoch. + const double simulationEndEpoch = 3300.0; + + // Set numerical integration fixed step size. + const double fixedStepSize = 1.0; + + + // Set Keplerian elements for Capsule. + Vector6d apolloInitialStateInKeplerianElements; + apolloInitialStateInKeplerianElements( semiMajorAxisIndex ) = spice_interface::getAverageRadius( "Earth" ) + 120.0E3; + apolloInitialStateInKeplerianElements( eccentricityIndex ) = 0.005; + apolloInitialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 85.3 ); + apolloInitialStateInKeplerianElements( argumentOfPeriapsisIndex ) + = unit_conversions::convertDegreesToRadians( 235.7 ); + apolloInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex ) + = unit_conversions::convertDegreesToRadians( 23.4 ); + apolloInitialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 139.87 ); + + // Convert apollo state from Keplerian elements to Cartesian elements. + const Vector6d apolloInitialState = convertKeplerianToCartesianElements( + apolloInitialStateInKeplerianElements, + getBodyGravitationalParameter( "Earth" ) ); + + // Define simulation body settings. + std::map< std::string, boost::shared_ptr< BodySettings > > bodySettings = + getDefaultBodySettings( { "Earth", "Moon" }, simulationStartEpoch - 10.0 * fixedStepSize, + simulationEndEpoch + 10.0 * fixedStepSize ); + bodySettings[ "Earth" ]->gravityFieldSettings = + boost::make_shared< simulation_setup::GravityFieldSettings >( central_spice ); + + // Create Earth object + simulation_setup::NamedBodyMap bodyMap = simulation_setup::createBodies( bodySettings ); + + // Create vehicle objects. + bodyMap[ "Apollo" ] = boost::make_shared< simulation_setup::Body >( ); + + // Create vehicle aerodynamic coefficients + bodyMap[ "Apollo" ]->setAerodynamicCoefficientInterface( + unit_tests::getApolloCoefficientInterface( ) ); + bodyMap[ "Apollo" ]->setConstantBodyMass( 5.0E3 ); + bodyMap[ "Apollo" ]->setEphemeris( + boost::make_shared< ephemerides::TabulatedCartesianEphemeris< > >( + boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, basic_mathematics::Vector6d > >( ), + "Earth" ) ); + + // Finalize body creation. + setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" ); + + // Define propagator settings variables. + SelectedAccelerationMap accelerationMap; + std::vector< std::string > bodiesToPropagate; + std::vector< std::string > centralBodies; + + // Define acceleration model settings. + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfApollo; + accelerationsOfApollo[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationsOfApollo[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( aerodynamic ) ); + accelerationsOfApollo[ "Moon" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationMap[ "Apollo" ] = accelerationsOfApollo; + + bodiesToPropagate.push_back( "Apollo" ); + centralBodies.push_back( "Earth" ); + + // Set initial state + basic_mathematics::Vector6d systemInitialState = apolloInitialState; + + // Define list of dependent variables to save. + std::vector< boost::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables; + dependentVariables.push_back( + boost::make_shared< SingleDependentVariableSaveSettings >( mach_number_dependent_variable, "Apollo" ) ); + dependentVariables.push_back( + boost::make_shared< SingleDependentVariableSaveSettings >( altitude_dependent_variable, + "Apollo", "Earth" ) ); + dependentVariables.push_back( + boost::make_shared< SingleDependentVariableSaveSettings >( relative_distance_dependent_variable, + "Apollo", "Earth" ) ); + dependentVariables.push_back( + boost::make_shared< SingleDependentVariableSaveSettings >( relative_speed_dependent_variable, + "Apollo", "Earth" ) ); + dependentVariables.push_back( + boost::make_shared< SingleAccelerationDependentVariableSaveSettings >( + central_gravity, "Apollo", "Earth", 1 ) ); + + + dependentVariables.push_back( + boost::make_shared< SingleDependentVariableSaveSettings >( relative_position_dependent_variable, + "Apollo", "Earth" ) ); + dependentVariables.push_back( + boost::make_shared< SingleDependentVariableSaveSettings >( relative_velocity_dependent_variable, + "Apollo", "Earth" ) ); + dependentVariables.push_back( + boost::make_shared< SingleAccelerationDependentVariableSaveSettings >( + central_gravity, "Apollo", "Earth", 0 ) ); + dependentVariables.push_back( + boost::make_shared< SingleDependentVariableSaveSettings >( + total_acceleration_dependent_variable, "Apollo" ) ); + dependentVariables.push_back( + boost::make_shared< SingleDependentVariableSaveSettings >( + aerodynamic_moment_coefficients_dependent_variable, "Apollo" ) ); + + // Create acceleration models and propagation settings. + basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodyMap, accelerationMap, bodiesToPropagate, centralBodies ); + + setTrimmedConditions( bodyMap.at( "Apollo" ) ); + + boost::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings = + boost::make_shared< TranslationalStatePropagatorSettings< double > > + ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, + boost::make_shared< propagators::PropagationTimeTerminationSettings >( 3200.0 ), cowell, + boost::make_shared< DependentVariableSaveSettings >( dependentVariables ) ); + boost::shared_ptr< IntegratorSettings< > > integratorSettings = + boost::make_shared< IntegratorSettings< > > + ( rungeKutta4, simulationStartEpoch, fixedStepSize ); + + // Create simulation object and propagate dynamics. + SingleArcDynamicsSimulator< > dynamicsSimulator( + bodyMap, integratorSettings, propagatorSettings, true, false, false ); + + // Retrieve numerical solutions for state and dependent variables + std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > > numericalSolution = + dynamicsSimulator.getEquationsOfMotionNumericalSolution( ); + std::map< double, Eigen::VectorXd > dependentVariableSoution = + dynamicsSimulator.getDependentVariableHistory( ); + + // Iterate over results for dependent variables, and check against manually retrieved values. + basic_mathematics::Vector6d currentStateDerivative; + Eigen::Vector3d manualCentralGravity; + for( std::map< double, Eigen::VectorXd >::iterator variableIterator = dependentVariableSoution.begin( ); + variableIterator != dependentVariableSoution.end( ); variableIterator++ ) + { + currentStateDerivative = dynamicsSimulator.getDynamicsStateDerivative( )->computeStateDerivative( + variableIterator->first, numericalSolution.at( variableIterator->first ) ); + + // Manually compute central gravity. + manualCentralGravity = + -bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( ) * + variableIterator->second.segment( 5, 3 ) / + std::pow( variableIterator->second.segment( 5, 3 ).norm( ), 3 ); + + // Check output time consistency + BOOST_CHECK_EQUAL( numericalSolution.count( variableIterator->first ), 1 ); + + // Check relative position and velocity against state + for( unsigned int i = 0; i < 3; i++ ) + { + BOOST_CHECK_SMALL( + std::fabs( numericalSolution.at( variableIterator->first )( i ) - + variableIterator->second( 5 + i ) ), 2.0E-5 ); + BOOST_CHECK_SMALL( + std::fabs( numericalSolution.at( variableIterator->first )( 3 + i ) - + variableIterator->second( 8 + i ) ), 5.0E-11 ); + } + + // Check central gravity acceleration + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + manualCentralGravity.segment( 0, 3 ), + variableIterator->second.segment( 11, 3 ), ( 5.0 * std::numeric_limits< double >::epsilon( ) ) ); + + // Check total acceleration (tolerance is not epsilon due to numerical root finding for trim) + for( unsigned int i = 0; i < 3; i++ ) + { + BOOST_CHECK_SMALL( + std::fabs( currentStateDerivative( 3 + i ) - + variableIterator->second( 14 + i ) ), 1.0E-13 ); + } + + // Check relative position and velocity norm. + BOOST_CHECK_SMALL( + std::fabs( ( numericalSolution.at( variableIterator->first ).segment( 0, 3 ) ).norm( ) - + variableIterator->second( 2 ) ), 2.0E-5 ); + BOOST_CHECK_SMALL( + std::fabs( ( numericalSolution.at( variableIterator->first ).segment( 3, 3 ) ).norm( ) - + variableIterator->second( 3 ) ), 2.0E-11 ); + + // Check central gravity acceleration norm + BOOST_CHECK_CLOSE_FRACTION( + manualCentralGravity.norm( ), + variableIterator->second( 4 ), 5.0 * std::numeric_limits< double >::epsilon( ) ); + + // Check Mach number + BOOST_CHECK_CLOSE_FRACTION( + bodyMap.at( "Apollo" )->getFlightConditions( )->getCurrentAirspeed( ) / + bodyMap.at( "Apollo" )->getFlightConditions( )->getCurrentSpeedOfSound( ), + variableIterator->second( 0 ), std::numeric_limits< double >::epsilon( ) ); + + // Check altitude. + BOOST_CHECK_CLOSE_FRACTION( + bodyMap.at( "Apollo" )->getFlightConditions( )->getCurrentAltitude( ), + variableIterator->second( 1 ), std::numeric_limits< double >::epsilon( ) ); + + // Check trimmed condition (y-term)/symmetric vehicle shape (x- and z-term). + BOOST_CHECK_SMALL( + std::fabs( variableIterator->second( 17 ) ), 1.0E-14 ); + BOOST_CHECK_SMALL( + std::fabs( variableIterator->second( 18 ) ), 1.0E-10 ); + BOOST_CHECK_SMALL( + std::fabs( variableIterator->second( 19 ) ), 1.0E-14 ); + + + } + +} + +BOOST_AUTO_TEST_SUITE_END( ) + + +} + +} + + diff --git a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestEnckeStateDerivative.cpp b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestEnckeStateDerivative.cpp new file mode 100644 index 0000000000..fdf5dc67e1 --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestEnckeStateDerivative.cpp @@ -0,0 +1,438 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#define BOOST_TEST_MAIN + +#include +#include +#include +#include + +#include +#include +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h" +#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h" + +#include "Tudat/External/SpiceInterface/spiceInterface.h" +#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" +#include "Tudat/InputOutput/basicInputOutput.h" +#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h" +#include "Tudat/SimulationSetup/PropagationSetup/createNumericalSimulator.h" +#include "Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h" + +namespace tudat +{ +namespace unit_tests +{ + +BOOST_AUTO_TEST_SUITE( test_encke_propagator ) + +// Test Encke propagator for point mass central body. +BOOST_AUTO_TEST_CASE( testEnckePopagatorForPointMassCentralBodies ) +{ + // Test simulation for different central body cases + for( unsigned int simulationCase = 0; simulationCase < 2; simulationCase++ ) + { + //Using declarations. + using namespace tudat::interpolators; + using namespace tudat::numerical_integrators; + using namespace tudat::spice_interface; + using namespace tudat::simulation_setup; + using namespace tudat::basic_astrodynamics; + using namespace tudat::orbital_element_conversions; + using namespace tudat::propagators; + + + //Load spice kernels. + std::string kernelsPath = input_output::getSpiceKernelPath( ); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de-403-masses.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "naif0009.tls"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "pck00009.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de421.bsp"); + + // Define bodies in simulation. + unsigned int totalNumberOfBodies = 7; + std::vector< std::string > bodyNames; + bodyNames.resize( totalNumberOfBodies ); + bodyNames[ 0 ] = "Earth"; + bodyNames[ 1 ] = "Mars"; + bodyNames[ 2 ] = "Sun"; + bodyNames[ 3 ] = "Venus"; + bodyNames[ 4 ] = "Moon"; + bodyNames[ 5 ] = "Mercury"; + bodyNames[ 6 ] = "Jupiter"; + + double initialEphemerisTime = 1.0E7; + double finalEphemerisTime = 2.0E7; + double maximumTimeStep = 3600.0; + double buffer = 5.0 * maximumTimeStep; + + // Create bodies needed in simulation + NamedBodyMap bodyMap = createBodies( + getDefaultBodySettings( bodyNames, initialEphemerisTime - buffer, finalEphemerisTime + buffer ) ); + setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" ); + + // Set accelerations between bodies that are to be taken into account. + SelectedAccelerationMap accelerationMap; + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfEarth; + accelerationsOfEarth[ "Sun" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationsOfEarth[ "Moon" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationsOfEarth[ "Jupiter" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationMap[ "Earth" ] = accelerationsOfEarth; + + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfMars; + accelerationsOfMars[ "Sun" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationsOfMars[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationsOfMars[ "Jupiter" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationMap[ "Mars" ] = accelerationsOfMars; + + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfMoon; + accelerationsOfMoon[ "Sun" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationsOfMoon[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationsOfMoon[ "Jupiter" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationMap[ "Moon" ] = accelerationsOfMoon; + + // Propagate Earth, Mars and Moon + std::vector< std::string > bodiesToPropagate; + bodiesToPropagate.push_back( "Earth" ); + bodiesToPropagate.push_back( "Mars" ); + bodiesToPropagate.push_back( "Moon" ); + + unsigned int numberOfNumericalBodies = bodiesToPropagate.size( ); + + // Define central bodies: all Sun for simulationCase = 0, Earth and Mars: Sun, Moon: Earth for simulationCase = 1 + std::vector< std::string > centralBodies; + std::map< std::string, std::string > centralBodyMap; + centralBodies.resize( numberOfNumericalBodies ); + for( int i = 0; i < 3; i++ ) + { + if( i == 2 && simulationCase == 1 ) + { + centralBodies[ i ] = "Earth"; + } + else + { + centralBodies[ i ] = "Sun"; + } + centralBodyMap[ bodiesToPropagate[ i ] ] = centralBodies[ i ]; + } + + + // Get initial states for bodies. + Eigen::VectorXd systemInitialState = Eigen::VectorXd( bodiesToPropagate.size( ) * 6 ); + for( unsigned int i = 0; i < numberOfNumericalBodies ; i++ ) + { + systemInitialState.segment( i * 6 , 6 ) = + bodyMap[ bodiesToPropagate[ i ] ]->getStateInBaseFrameFromEphemeris( initialEphemerisTime ) - + bodyMap[ centralBodies[ i ] ]->getStateInBaseFrameFromEphemeris( initialEphemerisTime ); + } + + // Create acceleratiuon models. + AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodyMap, accelerationMap, centralBodyMap ); + + // Create integrator settings. + boost::shared_ptr< IntegratorSettings< > > integratorSettings = + boost::make_shared< IntegratorSettings< > > + ( rungeKutta4, + initialEphemerisTime, 250.0 ); + + // Create propagation settings (Cowell) + boost::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings = + boost::make_shared< TranslationalStatePropagatorSettings< double > > + ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, finalEphemerisTime ); + + // Propagate orbit with Cowell method + SingleArcDynamicsSimulator< double > dynamicsSimulator2( + bodyMap, integratorSettings, propagatorSettings, true ); + + // Define ephemeris interrogation settings. + double initialTestTime = initialEphemerisTime + 10.0 * maximumTimeStep; + double finalTestTime = finalEphemerisTime - 10.0 * maximumTimeStep; + double testTimeStep = 1.0E4; + + // Get resutls of Cowell integration at given times. + double currentTestTime = initialTestTime; + std::map< double, Eigen::Matrix< double, 18, 1 > > cowellIntegrationResults; + while( currentTestTime < finalTestTime ) + { + cowellIntegrationResults[ currentTestTime ].segment( 0, 6 ) = + bodyMap[ "Earth" ]->getStateInBaseFrameFromEphemeris( currentTestTime ); + cowellIntegrationResults[ currentTestTime ].segment( 6, 6 ) = + bodyMap[ "Mars" ]->getStateInBaseFrameFromEphemeris( currentTestTime ); + cowellIntegrationResults[ currentTestTime ].segment( 12, 6 ) = + bodyMap[ "Moon" ]->getStateInBaseFrameFromEphemeris( currentTestTime ); + + currentTestTime += testTimeStep; + } + + // Create propagation settings (Encke) + propagatorSettings = boost::make_shared< TranslationalStatePropagatorSettings< double > > + ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, finalEphemerisTime, encke ); + + // Propagate orbit with Encke method + SingleArcDynamicsSimulator< double > dynamicsSimulator( + bodyMap, integratorSettings, propagatorSettings, true ); + + // Get resutls of Encke integration at given times. + currentTestTime = initialTestTime; + std::map< double, Eigen::Matrix< double, 18, 1 > > enckeIntegrationResults; + while( currentTestTime < finalTestTime ) + { + enckeIntegrationResults[ currentTestTime ].segment( 0, 6 ) = + bodyMap[ "Earth" ]->getStateInBaseFrameFromEphemeris( currentTestTime ); + enckeIntegrationResults[ currentTestTime ].segment( 6, 6 ) = + bodyMap[ "Mars" ]->getStateInBaseFrameFromEphemeris( currentTestTime ); + enckeIntegrationResults[ currentTestTime ].segment( 12, 6 ) = + bodyMap[ "Moon" ]->getStateInBaseFrameFromEphemeris( currentTestTime ); + currentTestTime += testTimeStep; + } + + // Compare results of Cowell and Encke propagations + std::map< double, Eigen::Matrix< double, 18, 1 > >::iterator enckeIterator = enckeIntegrationResults.begin( ); + std::map< double, Eigen::Matrix< double, 18, 1 > >::iterator cowellIterator = cowellIntegrationResults.begin( ); + for( unsigned int i = 0; i < enckeIntegrationResults.size( ); i++ ) + { + for( int j= 0; j< 3; j++ ) + { + BOOST_CHECK_SMALL( ( enckeIterator->second - cowellIterator->second ).segment( j, 1 )( 0 ), 0.01 ); + } + + for( int j = 6; j < 9; j++ ) + { + BOOST_CHECK_SMALL( ( enckeIterator->second - cowellIterator->second ).segment( j, 1 )( 0 ), 0.01 ); + } + + for( int j = 12; j < 15; j++ ) + { + BOOST_CHECK_SMALL( ( enckeIterator->second - cowellIterator->second ).segment( j, 1 )( 0 ), 0.075 ); + } + + for( int j = 3; j < 6; j++ ) + { + BOOST_CHECK_SMALL( ( enckeIterator->second - cowellIterator->second ).segment( j, 1 )( 0 ), 1.0E-8 ); + } + + for( int j = 9; j < 12; j++ ) + { + BOOST_CHECK_SMALL( ( enckeIterator->second - cowellIterator->second ).segment( j, 1 )( 0 ), 1.0E-8 ); + + } + + for( int j = 15; j < 18; j++ ) + { + BOOST_CHECK_SMALL( ( enckeIterator->second - cowellIterator->second ).segment( j, 1 )( 0 ), 1.0E-6 ); + + } + enckeIterator++; + cowellIterator++; + } + } +} + +// Test Encke propagator for point mass, and spherical harmonics central body. +BOOST_AUTO_TEST_CASE( testEnckePopagatorForSphericalHarmonicCentralBodies ) +{ + for( unsigned int simulationCase = 0; simulationCase < 4; simulationCase++ ) + { + using namespace tudat; + using namespace simulation_setup; + using namespace propagators; + using namespace numerical_integrators; + using namespace orbital_element_conversions; + using namespace basic_mathematics; + using namespace gravitation; + using namespace numerical_integrators; + + // Load Spice kernels. + spice_interface::loadSpiceKernelInTudat( input_output::getSpiceKernelPath( ) + "pck00009.tpc" ); + spice_interface::loadSpiceKernelInTudat( input_output::getSpiceKernelPath( ) + "de-403-masses.tpc" ); + spice_interface::loadSpiceKernelInTudat( input_output::getSpiceKernelPath( ) + "de421.bsp" ); + + // Set simulation time settings. + const double simulationStartEpoch = 0.0; + const double simulationEndEpoch = tudat::physical_constants::JULIAN_DAY; + + // 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" ); + + // Create body objects. + std::map< std::string, boost::shared_ptr< BodySettings > > bodySettings = + getDefaultBodySettings( bodiesToCreate, simulationStartEpoch - 300.0, simulationEndEpoch + 300.0 ); + for( unsigned int i = 0; i < bodiesToCreate.size( ); i++ ) + { + bodySettings[ bodiesToCreate.at( i ) ]->ephemerisSettings->resetFrameOrientation( "J2000" ); + bodySettings[ bodiesToCreate.at( i ) ]->rotationModelSettings->resetOriginalFrame( "J2000" ); + } + NamedBodyMap bodyMap = createBodies( bodySettings ); + + // Create spacecraft object. + bodyMap[ "Vehicle" ] = boost::make_shared< simulation_setup::Body >( ); + bodyMap[ "Vehicle" ]->setConstantBodyMass( 400.0 ); + bodyMap[ "Vehicle" ]->setEphemeris( boost::make_shared< ephemerides::TabulatedCartesianEphemeris< > >( + boost::shared_ptr< interpolators::OneDimensionalInterpolator + < double, Vector6d > >( ), "Earth", "J2000" ) ); + boost::shared_ptr< RadiationPressureInterfaceSettings > vehicleRadiationPressureSettings = + boost::make_shared< CannonBallRadiationPressureInterfaceSettings >( + "Sun", 4.0, 1.2, boost::assign::list_of( "Earth" )( "Moon" ) ); + bodyMap[ "Vehicle" ]->setRadiationPressureInterface( + "Sun", createRadiationPressureInterface( + vehicleRadiationPressureSettings, "Vehicle", bodyMap ) ); + + + // Finalize body creation. + setGlobalFrameBodyEphemerides( bodyMap, "SSB", "J2000" ); + + // 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< boost::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle; + + // Use only central gravity for Earth + if( simulationCase < 2 ) + { + accelerationsOfVehicle[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( + basic_astrodynamics::central_gravity ) ); + } + // Use spherical harmonics for Earth + else + { + accelerationsOfVehicle[ "Earth" ].push_back( + boost::make_shared< SphericalHarmonicAccelerationSettings >( 5, 5 ) ); + + } + + // Use perturbations other than Earth gravity + if( simulationCase % 2 == 0 ) + { + accelerationsOfVehicle[ "Sun" ].push_back( boost::make_shared< AccelerationSettings >( + basic_astrodynamics::central_gravity ) ); + accelerationsOfVehicle[ "Moon" ].push_back( boost::make_shared< AccelerationSettings >( + basic_astrodynamics::central_gravity ) ); + accelerationsOfVehicle[ "Mars" ].push_back( boost::make_shared< AccelerationSettings >( + basic_astrodynamics::central_gravity ) ); + accelerationsOfVehicle[ "Venus" ].push_back( boost::make_shared< AccelerationSettings >( + basic_astrodynamics::central_gravity ) ); + accelerationsOfVehicle[ "Sun" ].push_back( boost::make_shared< AccelerationSettings >( + basic_astrodynamics::cannon_ball_radiation_pressure ) ); + } + accelerationMap[ "Vehicle" ] = accelerationsOfVehicle; + bodiesToPropagate.push_back( "Vehicle" ); + centralBodies.push_back( "Earth" ); + basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodyMap, accelerationMap, bodiesToPropagate, centralBodies ); + + // Set Keplerian elements for Vehicle. + Vector6d vehicleInitialStateInKeplerianElements; + vehicleInitialStateInKeplerianElements( semiMajorAxisIndex ) = 8000.0E3; + vehicleInitialStateInKeplerianElements( eccentricityIndex ) = 0.1; + vehicleInitialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 85.3 ); + vehicleInitialStateInKeplerianElements( argumentOfPeriapsisIndex ) + = unit_conversions::convertDegreesToRadians( 235.7 ); + vehicleInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex ) + = unit_conversions::convertDegreesToRadians( 23.4 ); + vehicleInitialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 139.87 ); + + double earthGravitationalParameter = bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( ); + const Vector6d vehicleInitialState = convertKeplerianToCartesianElements( + vehicleInitialStateInKeplerianElements, earthGravitationalParameter ); + + // Define propagator settings (Cowell) + boost::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings = + boost::make_shared< TranslationalStatePropagatorSettings< double > > + ( centralBodies, accelerationModelMap, bodiesToPropagate, vehicleInitialState, simulationEndEpoch ); + + // Define integrator settings. + const double fixedStepSize = 5.0; + boost::shared_ptr< IntegratorSettings< > > integratorSettings = + boost::make_shared< IntegratorSettings< > > + ( rungeKutta4, 0.0, fixedStepSize ); + + // Propagate orbit with Cowell method + SingleArcDynamicsSimulator< double > dynamicsSimulator2( + bodyMap, integratorSettings, propagatorSettings, true ); + + // Define ephemeris interrogation settings. + double initialTestTime = simulationStartEpoch + 10.0 * fixedStepSize; + double finalTestTime = simulationEndEpoch - 10.0 * fixedStepSize; + double testTimeStep = 1.0E4; + + // Get resutls of Cowell integration at given times. + double currentTestTime = initialTestTime; + std::map< double, Eigen::Matrix< double, 6, 1 > > cowellIntegrationResults; + while( currentTestTime < finalTestTime ) + { + cowellIntegrationResults[ currentTestTime ].segment( 0, 6 ) = + bodyMap[ "Vehicle" ]->getEphemeris( )->getCartesianStateFromEphemeris( currentTestTime ); + + currentTestTime += testTimeStep; + } + + // Create propagation settings (Encke) + propagatorSettings = boost::make_shared< TranslationalStatePropagatorSettings< double > > + ( centralBodies, accelerationModelMap, bodiesToPropagate, vehicleInitialState, simulationEndEpoch, encke ); + + // Propagate orbit with Encke method + SingleArcDynamicsSimulator< double > dynamicsSimulator( + bodyMap, integratorSettings, propagatorSettings, true ); + + // Get resutls of Encke integration at given times. + currentTestTime = initialTestTime; + std::map< double, Eigen::Matrix< double, 6, 1 > > enckeIntegrationResults; + while( currentTestTime < finalTestTime ) + { + enckeIntegrationResults[ currentTestTime ].segment( 0, 6 ) = + bodyMap[ "Vehicle" ]->getEphemeris( )->getCartesianStateFromEphemeris( currentTestTime ); + currentTestTime += testTimeStep; + } + + // Compare results of Cowell and Encke propagations + std::map< double, Eigen::Matrix< double, 6, 1 > >::iterator enckeIterator = enckeIntegrationResults.begin( ); + std::map< double, Eigen::Matrix< double, 6, 1 > >::iterator cowellIterator = cowellIntegrationResults.begin( ); + for( unsigned int i = 0; i < enckeIntegrationResults.size( ); i++ ) + { + for( int j= 0; j< 3; j++ ) + { + BOOST_CHECK_SMALL( ( enckeIterator->second - cowellIterator->second )( j ), 0.02 ); + } + + for( int j = 3; j < 6; j++ ) + { + BOOST_CHECK_SMALL( ( enckeIterator->second - cowellIterator->second )( j ), 1.0E-5 ); + + } + enckeIterator++; + cowellIterator++; + } + } +} +BOOST_AUTO_TEST_SUITE_END( ) + + +} + +} + + diff --git a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestEnvironmentUpdater.cpp b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestEnvironmentUpdater.cpp index 527726bb72..bd599a135b 100644 --- a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestEnvironmentUpdater.cpp +++ b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestEnvironmentUpdater.cpp @@ -22,15 +22,15 @@ #include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h" #include "Tudat/Astrodynamics/Ephemerides/approximatePlanetPositions.h" #include "Tudat/Astrodynamics/Ephemerides/tabulatedEphemeris.h" -#include "Tudat/Astrodynamics/Propagators/propagationSettings.h" -#include "Tudat/Astrodynamics/Propagators/dynamicsSimulator.h" +#include "Tudat/SimulationSetup/PropagationSetup/propagationSettings.h" +#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h" #include "Tudat/Basics/testMacros.h" #include "Tudat/External/SpiceInterface/spiceEphemeris.h" #include "Tudat/InputOutput/basicInputOutput.h" #include "Tudat/Mathematics/Interpolators/linearInterpolator.h" -#include "Tudat/SimulationSetup/createAccelerationModels.h" -#include "Tudat/SimulationSetup/createBodies.h" -#include "Tudat/SimulationSetup/defaultBodies.h" +#include "Tudat/SimulationSetup/PropagationSetup/createNumericalSimulator.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h" #include "Tudat/Astrodynamics/Aerodynamics/UnitTests/testApolloCapsuleCoefficients.h" namespace tudat @@ -52,6 +52,7 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) { double initialTime = 86400.0; + double finalTime = 2.0 * 86400.0; // Load Spice kernels const std::string kernelsPath = input_output::getSpiceKernelPath( ); @@ -90,9 +91,9 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) 3.088038821491940e-7, 0.0, 0.0, -9.436980733957690e-8, -3.233531925405220e-7, -2.149554083060460e-7, 4.980705501023510e-8, -6.693799351801650e-7 ).finished( ); - bodySettings[ "Earth" ]->gravityFieldSettings - = boost::make_shared< SphericalHarmonicsGravityFieldSettings >( - gravitationalParameter, 6378.0E3, cosineCoefficients, sineCoefficients, "IAU_Earth" ); + bodySettings[ "Earth" ]->gravityFieldSettings = boost::make_shared< SphericalHarmonicsGravityFieldSettings >( + gravitationalParameter, 6378.0E3, cosineCoefficients, sineCoefficients, + "IAU_Earth" ); // Create bodies NamedBodyMap bodyMap = createBodies( bodySettings ); @@ -100,17 +101,17 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) // Define variables used in tests. SelectedAccelerationMap accelerationSettingsMap; + std::map< std::string, std::string > centralBodies; std::vector< std::string > propagatedBodyList; std::vector< std::string > centralBodyList; - std::map< IntegratedStateType, Eigen::VectorXd > integratedStateToSet; + std::unordered_map< IntegratedStateType, Eigen::VectorXd > integratedStateToSet; double testTime = 0.0; { // Define (arbitrary) test state. - Eigen::VectorXd testState = ( Eigen::VectorXd( 6 ) << 1.44E6, 2.234E8, -3343.246E7, - 1.2E4, 1.344E3, -22.343E3 ).finished( ); + Eigen::VectorXd testState = ( Eigen::VectorXd( 6 ) << 1.44E6, 2.234E8, -3343.246E7, 1.2E4, 1.344E3, -22.343E3 ).finished( ); integratedStateToSet[ transational_state ] = testState; testTime = 2.0 * 86400.0; @@ -123,18 +124,19 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) boost::make_shared< AccelerationSettings >( central_gravity ) ); // Define origin of integration to be barycenter. + centralBodies[ "Moon" ] = "SSB"; propagatedBodyList.push_back( "Moon" ); - centralBodyList.push_back( "SSB" ); + centralBodyList.push_back( centralBodies[ "Moon" ] ); // Create accelerations AccelerationMap accelerationsMap = createAccelerationModelsMap( - bodyMap, accelerationSettingsMap, propagatedBodyList, centralBodyList ); + bodyMap, accelerationSettingsMap, centralBodies ); // Create environment update settings. boost::shared_ptr< PropagatorSettings< double > > propagatorSettings = boost::make_shared< TranslationalStatePropagatorSettings< double > >( centralBodyList, accelerationsMap, propagatedBodyList, getInitialStateOfBody( - "Moon", centralBodyList.at( 0 ), bodyMap, initialTime ) ); + "Moon", centralBodies[ "Moon" ], bodyMap, initialTime ), finalTime ); std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > environmentModelsToUpdate = createEnvironmentUpdaterSettings< double >( propagatorSettings, bodyMap ); @@ -143,7 +145,6 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) BOOST_CHECK_EQUAL( environmentModelsToUpdate.count( body_transational_state_update ), 1 ); BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( body_transational_state_update ).size( ), 2 ); - // Create and call updater. boost::shared_ptr< propagators::EnvironmentUpdater< double, double > > updater = createEnvironmentUpdaterForDynamicalEquations< double, double >( @@ -153,13 +154,11 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) // Test if Earth, Sun and Moon are updated TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Earth" )->getState( ), - bodyMap.at( "Earth" )->getEphemeris( )-> - getCartesianStateFromEphemeris( testTime ), + bodyMap.at( "Earth" )->getEphemeris( )->getCartesianStateFromEphemeris( testTime ), std::numeric_limits< double >::epsilon( ) ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Sun" )->getState( ), - bodyMap.at( "Sun" )->getEphemeris( )-> - getCartesianStateFromEphemeris( testTime ), + bodyMap.at( "Sun" )->getEphemeris( )->getCartesianStateFromEphemeris( testTime ), std::numeric_limits< double >::epsilon( ) ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Moon" )->getState( ), testState, @@ -172,22 +171,19 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) // Update environment to new time, and state from environment. updater->updateEnvironment( - 0.5 * testTime, std::map< IntegratedStateType, Eigen::VectorXd >( ), + 0.5 * testTime, std::unordered_map< IntegratedStateType, Eigen::VectorXd >( ), boost::assign::list_of( transational_state ) ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Earth" )->getState( ), - bodyMap.at( "Earth" )->getEphemeris( )-> - getCartesianStateFromEphemeris( 0.5 * testTime ), + bodyMap.at( "Earth" )->getEphemeris( )->getCartesianStateFromEphemeris( 0.5 * testTime ), std::numeric_limits< double >::epsilon( ) ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Sun" )->getState( ), - bodyMap.at( "Sun" )->getEphemeris( )-> - getCartesianStateFromEphemeris( 0.5 * testTime ), + bodyMap.at( "Sun" )->getEphemeris( )->getCartesianStateFromEphemeris( 0.5 * testTime ), std::numeric_limits< double >::epsilon( ) ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Moon" )->getState( ), - bodyMap.at( "Moon" )->getEphemeris( )-> - getCartesianStateFromEphemeris( 0.5 * testTime ), + bodyMap.at( "Moon" )->getEphemeris( )->getCartesianStateFromEphemeris( 0.5 * testTime ), std::numeric_limits< double >::epsilon( ) ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Mars" )->getState( ), basic_mathematics::Vector6d::Zero( ), @@ -197,6 +193,7 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) // Test third body acceleration updates. { accelerationSettingsMap.clear( ); + centralBodies.clear( ); propagatedBodyList.clear( ); centralBodyList.clear( ); @@ -207,25 +204,26 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) boost::make_shared< AccelerationSettings >( central_gravity ) ); // Define origin of integration + centralBodies[ "Moon" ] = "Earth"; propagatedBodyList.push_back( "Moon" ); - centralBodyList.push_back( "Earth"); + centralBodyList.push_back( centralBodies[ "Moon" ] ); // Create accelerations AccelerationMap accelerationsMap = createAccelerationModelsMap( - bodyMap, accelerationSettingsMap, propagatedBodyList, centralBodyList ); + bodyMap, accelerationSettingsMap, centralBodies ); // Create environment update settings. boost::shared_ptr< PropagatorSettings< double > > propagatorSettings = boost::make_shared< TranslationalStatePropagatorSettings< double > >( centralBodyList, accelerationsMap, propagatedBodyList, getInitialStateOfBody( - "Moon", centralBodyList.at( 0 ), bodyMap, initialTime ) ); + "Moon", centralBodies[ "Moon" ], bodyMap, initialTime ), finalTime ); std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > environmentModelsToUpdate = createEnvironmentUpdaterSettings< double >( propagatorSettings, bodyMap ); // Test update settings BOOST_CHECK_EQUAL( environmentModelsToUpdate.size( ), 1 ); BOOST_CHECK_EQUAL( environmentModelsToUpdate.count( body_transational_state_update ), 1 ); - BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( body_transational_state_update ).size( ), 3 ); + BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( body_transational_state_update ).size( ), 3 ); // Create and call updater. boost::shared_ptr< propagators::EnvironmentUpdater< double, double > > updater = @@ -236,21 +234,18 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) // Test if Earth, Sun, Mars and Moon are updated TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Earth" )->getState( ), - bodyMap.at( "Earth" )->getEphemeris( )-> - getCartesianStateFromEphemeris( testTime ), + bodyMap.at( "Earth" )->getEphemeris( )->getCartesianStateFromEphemeris( testTime ), std::numeric_limits< double >::epsilon( ) ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Sun" )->getState( ), - bodyMap.at( "Sun" )->getEphemeris( )-> - getCartesianStateFromEphemeris( testTime ), + bodyMap.at( "Sun" )->getEphemeris( )->getCartesianStateFromEphemeris( testTime ), std::numeric_limits< double >::epsilon( ) ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Moon" )->getState( ), testState, std::numeric_limits< double >::epsilon( ) ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Mars" )->getState( ), - bodyMap.at( "Mars" )->getEphemeris( )-> - getCartesianStateFromEphemeris( testTime ), + bodyMap.at( "Mars" )->getEphemeris( )->getCartesianStateFromEphemeris( testTime ), std::numeric_limits< double >::epsilon( ) ); // Test if Venus is not updated @@ -260,7 +255,7 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) // Update environment to new time, and state from environment. updater->updateEnvironment( - 0.5 * testTime, std::map< IntegratedStateType, Eigen::VectorXd >( ), + 0.5 * testTime, std::unordered_map< IntegratedStateType, Eigen::VectorXd >( ), boost::assign::list_of( transational_state ) ); } @@ -268,6 +263,7 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) // Test spherical harmonic acceleration update { accelerationSettingsMap.clear( ); + centralBodies.clear( ); propagatedBodyList.clear( ); centralBodyList.clear( ); @@ -280,17 +276,19 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) boost::make_shared< AccelerationSettings >( central_gravity ) ); // Define origin of integration + centralBodies[ "Moon" ] = "Earth"; propagatedBodyList.push_back( "Moon" ); - centralBodyList.push_back( "Earth" ); + centralBodyList.push_back( centralBodies[ "Moon" ] ); + // Create accelerations AccelerationMap accelerationsMap = createAccelerationModelsMap( - bodyMap, accelerationSettingsMap, propagatedBodyList, centralBodyList ); + bodyMap, accelerationSettingsMap, centralBodies ); // Create environment update settings. boost::shared_ptr< PropagatorSettings< double > > propagatorSettings = boost::make_shared< TranslationalStatePropagatorSettings< double > >( centralBodyList, accelerationsMap, propagatedBodyList, getInitialStateOfBody( - "Moon", centralBodyList.at( 0 ), bodyMap, initialTime ) ); + "Moon", centralBodies[ "Moon" ], bodyMap, initialTime ), finalTime ); std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > environmentModelsToUpdate = createEnvironmentUpdaterSettings< double >( propagatorSettings, bodyMap ); @@ -311,21 +309,18 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) // Test if Earth, Sun, Mars and Moon are updated TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Earth" )->getState( ), - bodyMap.at( "Earth" )->getEphemeris( )-> - getCartesianStateFromEphemeris( testTime ), + bodyMap.at( "Earth" )->getEphemeris( )->getCartesianStateFromEphemeris( testTime ), std::numeric_limits< double >::epsilon( ) ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Sun" )->getState( ), - bodyMap.at( "Sun" )->getEphemeris( )-> - getCartesianStateFromEphemeris( testTime ), + bodyMap.at( "Sun" )->getEphemeris( )->getCartesianStateFromEphemeris( testTime ), std::numeric_limits< double >::epsilon( ) ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Moon" )->getState( ), testState, std::numeric_limits< double >::epsilon( ) ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Mars" )->getState( ), - bodyMap.at( "Mars" )->getEphemeris( )-> - getCartesianStateFromEphemeris( testTime ), + bodyMap.at( "Mars" )->getEphemeris( )->getCartesianStateFromEphemeris( testTime ), std::numeric_limits< double >::epsilon( ) ); // Test if Venus is not updated @@ -336,23 +331,19 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) // Test if Earth rotation is updated TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Earth" )->getCurrentRotationToGlobalFrame( ).toRotationMatrix( ), - bodyMap.at( "Earth" )->getRotationalEphemeris( )-> - getRotationToBaseFrame( testTime ).toRotationMatrix( ), + bodyMap.at( "Earth" )->getRotationalEphemeris( )->getRotationToBaseFrame( testTime ).toRotationMatrix( ), std::numeric_limits< double >::epsilon( ) ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Earth" )->getCurrentRotationToLocalFrame( ).toRotationMatrix( ), - bodyMap.at( "Earth" )->getRotationalEphemeris( )-> - getRotationToTargetFrame( testTime ).toRotationMatrix( ), + bodyMap.at( "Earth" )->getRotationalEphemeris( )->getRotationToTargetFrame( testTime ).toRotationMatrix( ), std::numeric_limits< double >::epsilon( ) ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Earth" )->getCurrentRotationMatrixDerivativeToGlobalFrame( ), - bodyMap.at( "Earth" )->getRotationalEphemeris( )-> - getDerivativeOfRotationToBaseFrame( testTime ), + bodyMap.at( "Earth" )->getRotationalEphemeris( )->getDerivativeOfRotationToBaseFrame( testTime ), std::numeric_limits< double >::epsilon( ) ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Earth" )->getCurrentRotationMatrixDerivativeToLocalFrame( ), - bodyMap.at( "Earth" )->getRotationalEphemeris( )-> - getDerivativeOfRotationToTargetFrame( testTime ), + bodyMap.at( "Earth" )->getRotationalEphemeris( )->getDerivativeOfRotationToTargetFrame( testTime ), std::numeric_limits< double >::epsilon( ) ); // Test if Mars rotation is not updated @@ -371,7 +362,7 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) // Update environment to new time, and state from environment. updater->updateEnvironment( - 0.5 * testTime, std::map< IntegratedStateType, Eigen::VectorXd >( ), + 0.5 * testTime, std::unordered_map< IntegratedStateType, Eigen::VectorXd >( ), boost::assign::list_of( transational_state ) ); } @@ -388,6 +379,7 @@ double getBodyMass( const double time ) BOOST_AUTO_TEST_CASE( test_NonConservativeForceEnvironmentUpdate ) { double initialTime = 86400.0; + double finalTime = 2.0 * 86400.0; using namespace tudat::simulation_setup; using namespace tudat; @@ -424,10 +416,9 @@ BOOST_AUTO_TEST_CASE( test_NonConservativeForceEnvironmentUpdate ) // Define test time and state. double testTime = 2.0 * 86400.0; - std::map< IntegratedStateType, Eigen::VectorXd > integratedStateToSet; - Eigen::VectorXd testState = 1.1 * bodyMap[ "Vehicle" ]->getEphemeris( )-> - getCartesianStateFromEphemeris( testTime ) - + bodyMap.at( "Earth" )->getEphemeris( )->getCartesianStateFromEphemeris( testTime ); + std::unordered_map< IntegratedStateType, Eigen::VectorXd > integratedStateToSet; + Eigen::VectorXd testState = 1.1 * bodyMap[ "Vehicle" ]->getEphemeris( )->getCartesianStateFromEphemeris( testTime ) + + bodyMap.at( "Earth" )->getEphemeris( )->getCartesianStateFromEphemeris( testTime ); integratedStateToSet[ transational_state ] = testState; { @@ -437,21 +428,22 @@ BOOST_AUTO_TEST_CASE( test_NonConservativeForceEnvironmentUpdate ) boost::make_shared< AccelerationSettings >( cannon_ball_radiation_pressure ) ); // Define origin of integration - + std::map< std::string, std::string > centralBodies; + centralBodies[ "Vehicle" ] = "Earth"; std::vector< std::string > propagatedBodyList; propagatedBodyList.push_back( "Vehicle" ); std::vector< std::string > centralBodyList; - centralBodyList.push_back( "Earth" ); + centralBodyList.push_back( centralBodies[ "Vehicle" ] ); // Create accelerations AccelerationMap accelerationsMap = createAccelerationModelsMap( - bodyMap, accelerationSettingsMap, propagatedBodyList, centralBodyList ); + bodyMap, accelerationSettingsMap, centralBodies ); // Create environment update settings. boost::shared_ptr< PropagatorSettings< double > > propagatorSettings = boost::make_shared< TranslationalStatePropagatorSettings< double > >( centralBodyList, accelerationsMap, propagatedBodyList, getInitialStateOfBody( - "Vehicle", centralBodyList.at( 0 ), bodyMap, initialTime ) ); + "Vehicle", centralBodies[ "Vehicle" ], bodyMap, initialTime ), finalTime ); std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > environmentModelsToUpdate = createEnvironmentUpdaterSettings< double >( propagatorSettings, bodyMap ); @@ -484,10 +476,8 @@ BOOST_AUTO_TEST_CASE( test_NonConservativeForceEnvironmentUpdate ) updater->updateEnvironment( - 0.5 * testTime, std::map< IntegratedStateType, Eigen::VectorXd >( ), + 0.5 * testTime, std::unordered_map< IntegratedStateType, Eigen::VectorXd >( ), boost::assign::list_of( transational_state ) ); - - } { @@ -499,14 +489,16 @@ BOOST_AUTO_TEST_CASE( test_NonConservativeForceEnvironmentUpdate ) boost::make_shared< AccelerationSettings >( aerodynamic ) ); // Define origin of integration + std::map< std::string, std::string > centralBodies; + centralBodies[ "Vehicle" ] = "Earth"; std::vector< std::string > propagatedBodyList; propagatedBodyList.push_back( "Vehicle" ); std::vector< std::string > centralBodyList; - centralBodyList.push_back( "Earth" ); + centralBodyList.push_back( centralBodies[ "Vehicle" ] ); // Create accelerations AccelerationMap accelerationsMap = createAccelerationModelsMap( - bodyMap, accelerationSettingsMap, propagatedBodyList, centralBodyList ); + bodyMap, accelerationSettingsMap, centralBodies ); // Define orientation angles. @@ -523,7 +515,7 @@ BOOST_AUTO_TEST_CASE( test_NonConservativeForceEnvironmentUpdate ) boost::shared_ptr< PropagatorSettings< double > > propagatorSettings = boost::make_shared< TranslationalStatePropagatorSettings< double > >( centralBodyList, accelerationsMap, propagatedBodyList, getInitialStateOfBody( - "Vehicle", centralBodyList.at( 0 ), bodyMap, initialTime ) ); + "Vehicle", centralBodies[ "Vehicle" ], bodyMap, initialTime ), finalTime ); std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > environmentModelsToUpdate = createEnvironmentUpdaterSettings< double >( propagatorSettings, bodyMap ); @@ -536,11 +528,9 @@ BOOST_AUTO_TEST_CASE( test_NonConservativeForceEnvironmentUpdate ) BOOST_CHECK_EQUAL( environmentModelsToUpdate.count( body_mass_update ), 1 ); BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( body_mass_update ).size( ), 1 ); BOOST_CHECK_EQUAL( environmentModelsToUpdate.count( body_rotational_state_update ), 1 ); - BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( - body_rotational_state_update ).size( ), 1 ); + BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( body_rotational_state_update ).size( ), 1 ); BOOST_CHECK_EQUAL( environmentModelsToUpdate.count( vehicle_flight_conditions_update ), 1 ); - BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( - vehicle_flight_conditions_update ).size( ), 1 ); + BOOST_CHECK_EQUAL( environmentModelsToUpdate.at( vehicle_flight_conditions_update ).size( ), 1 ); // Create and call updater. boost::shared_ptr< propagators::EnvironmentUpdater< double, double > > updater = @@ -551,13 +541,11 @@ BOOST_AUTO_TEST_CASE( test_NonConservativeForceEnvironmentUpdate ) // Test if Earth, Sun and Vehicle states are updated. TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Earth" )->getState( ), - bodyMap.at( "Earth" )->getEphemeris( )-> - getCartesianStateFromEphemeris( testTime ), + bodyMap.at( "Earth" )->getEphemeris( )->getCartesianStateFromEphemeris( testTime ), std::numeric_limits< double >::epsilon( ) ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Sun" )->getState( ), - bodyMap.at( "Sun" )->getEphemeris( )-> - getCartesianStateFromEphemeris( testTime ), + bodyMap.at( "Sun" )->getEphemeris( )->getCartesianStateFromEphemeris( testTime ), std::numeric_limits< double >::epsilon( ) ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Vehicle" )->getState( ), testState, @@ -566,13 +554,11 @@ BOOST_AUTO_TEST_CASE( test_NonConservativeForceEnvironmentUpdate ) // Test if Earth rotation is updated. TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Earth" )->getCurrentRotationToGlobalFrame( ).toRotationMatrix( ), - bodyMap.at( "Earth" )->getRotationalEphemeris( )-> - getRotationToBaseFrame( testTime ).toRotationMatrix( ), + bodyMap.at( "Earth" )->getRotationalEphemeris( )->getRotationToBaseFrame( testTime ).toRotationMatrix( ), std::numeric_limits< double >::epsilon( ) ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( bodyMap.at( "Earth" )->getCurrentRotationToLocalFrame( ).toRotationMatrix( ), - bodyMap.at( "Earth" )->getRotationalEphemeris( )-> - getRotationToTargetFrame( testTime ).toRotationMatrix( ), + bodyMap.at( "Earth" )->getRotationalEphemeris( )->getRotationToTargetFrame( testTime ).toRotationMatrix( ), std::numeric_limits< double >::epsilon( ) ); // Test if body mass is updated @@ -582,33 +568,28 @@ BOOST_AUTO_TEST_CASE( test_NonConservativeForceEnvironmentUpdate ) // Check if flight conditions update has been called BOOST_CHECK_EQUAL( - ( vehicleFlightConditions->getCurrentAirspeed( ) - == vehicleFlightConditions->getCurrentAirspeed( ) ), 1 ); + ( vehicleFlightConditions->getCurrentAirspeed( ) == vehicleFlightConditions->getCurrentAirspeed( ) ), 1 ); BOOST_CHECK_EQUAL( - ( vehicleFlightConditions->getCurrentAltitude( ) - == vehicleFlightConditions->getCurrentAltitude( ) ), 1 ); + ( vehicleFlightConditions->getCurrentAltitude( ) == vehicleFlightConditions->getCurrentAltitude( ) ), 1 ); BOOST_CHECK_EQUAL( - ( vehicleFlightConditions->getCurrentDensity( ) - == vehicleFlightConditions->getCurrentDensity( ) ), 1 ); + ( vehicleFlightConditions->getCurrentDensity( ) == vehicleFlightConditions->getCurrentDensity( ) ), 1 ); BOOST_CHECK_EQUAL( vehicleFlightConditions->getCurrentTime( ), testTime ); // Check if radiation pressure update is updated. - boost::shared_ptr< electro_magnetism::RadiationPressureInterface > radiationPressureInterface - = bodyMap.at( "Vehicle" )->getRadiationPressureInterfaces( ).at( "Sun" ); + boost::shared_ptr< electro_magnetism::RadiationPressureInterface > radiationPressureInterface = + bodyMap.at( "Vehicle" )->getRadiationPressureInterfaces( ).at( "Sun" ); BOOST_CHECK_EQUAL( - ( radiationPressureInterface->getCurrentTime( ) - == radiationPressureInterface->getCurrentTime( ) ), 1 ); + ( radiationPressureInterface->getCurrentTime( ) == radiationPressureInterface->getCurrentTime( ) ), 1 ); BOOST_CHECK_EQUAL( - ( radiationPressureInterface->getCurrentRadiationPressure( ) - == radiationPressureInterface->getCurrentRadiationPressure( ) ), 1 ); + ( radiationPressureInterface->getCurrentRadiationPressure( ) == + radiationPressureInterface->getCurrentRadiationPressure( ) ), 1 ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( radiationPressureInterface->getCurrentSolarVector( ), - ( bodyMap.at( "Sun" )->getPosition( ) - - bodyMap.at( "Vehicle" )->getPosition( ) ), + ( bodyMap.at( "Sun" )->getPosition( ) - bodyMap.at( "Vehicle" )->getPosition( ) ), std::numeric_limits< double >::epsilon( ) ); updater->updateEnvironment( - 0.5 * testTime, std::map< IntegratedStateType, Eigen::VectorXd >( ), + 0.5 * testTime, std::unordered_map< IntegratedStateType, Eigen::VectorXd >( ), boost::assign::list_of( transational_state ) ); diff --git a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestMultiTypeStatePropagation.cpp b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestMultiTypeStatePropagation.cpp new file mode 100644 index 0000000000..268a92991a --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestMultiTypeStatePropagation.cpp @@ -0,0 +1,253 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#define BOOST_TEST_MAIN + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h" +#include +#include +#include +#include "Tudat/SimulationSetup/PropagationSetup/createNumericalSimulator.h" + + + +namespace tudat +{ +namespace unit_tests +{ + +using namespace mathematical_constants; + +BOOST_AUTO_TEST_SUITE( test_hybrid_state_derivative_model ) + +std::map< double, Eigen::VectorXd > propagateKeplerOrbitAndMassState( + const int simulationCase ) +{ + using namespace simulation_setup; + using namespace propagators; + using namespace numerical_integrators; + using namespace orbital_element_conversions; + using namespace basic_mathematics; + using namespace gravitation; + using namespace numerical_integrators; + using namespace unit_conversions; + + // Load Spice kernels. + spice_interface::loadSpiceKernelInTudat( input_output::getSpiceKernelPath( ) + "pck00009.tpc" ); + spice_interface::loadSpiceKernelInTudat( input_output::getSpiceKernelPath( ) + "de-403-masses.tpc" ); + spice_interface::loadSpiceKernelInTudat( input_output::getSpiceKernelPath( ) + "de421.bsp" ); + + // Set simulation end epoch. + const double simulationEndEpoch = tudat::physical_constants::JULIAN_DAY; + + // Set numerical integration fixed step size. + const double fixedStepSize = 60.0; + + // Define body settings for simulation. + std::map< std::string, boost::shared_ptr< BodySettings > > bodySettings; + bodySettings[ "Earth" ] = boost::make_shared< BodySettings >( ); + bodySettings[ "Earth" ]->ephemerisSettings = boost::make_shared< ConstantEphemerisSettings >( + basic_mathematics::Vector6d::Zero( ), "SSB", "J2000" ); + bodySettings[ "Earth" ]->gravityFieldSettings = boost::make_shared< GravityFieldSettings >( central_spice ); + + // Create Earth object + NamedBodyMap bodyMap = createBodies( bodySettings ); + + // Create spacecraft object. + bodyMap[ "Asterix" ] = boost::make_shared< simulation_setup::Body >( ); + bodyMap[ "Asterix" ]->setEphemeris( boost::make_shared< ephemerides::TabulatedCartesianEphemeris< > >( + boost::shared_ptr< interpolators::OneDimensionalInterpolator + < double, Vector6d > >( ), "Earth", "J2000" ) ); + + // Finalize body creation. + setGlobalFrameBodyEphemerides( bodyMap, "SSB", "J2000" ); + + // 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< boost::shared_ptr< AccelerationSettings > > > accelerationsOfAsterix; + accelerationsOfAsterix[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( + basic_astrodynamics::central_gravity ) ); + accelerationMap[ "Asterix" ] = accelerationsOfAsterix; + bodiesToPropagate.push_back( "Asterix" ); + centralBodies.push_back( "Earth" ); + + // Create acceleration models and propagation settings. + basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodyMap, accelerationMap, bodiesToPropagate, centralBodies ); + + + // Set Keplerian elements for Asterix. + Vector6d asterixInitialStateInKeplerianElements; + asterixInitialStateInKeplerianElements( semiMajorAxisIndex ) = 7500.0E3; + asterixInitialStateInKeplerianElements( eccentricityIndex ) = 0.1; + asterixInitialStateInKeplerianElements( inclinationIndex ) = convertDegreesToRadians( 85.3 ); + asterixInitialStateInKeplerianElements( argumentOfPeriapsisIndex ) + = convertDegreesToRadians( 235.7 ); + asterixInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex ) + = convertDegreesToRadians( 23.4 ); + asterixInitialStateInKeplerianElements( trueAnomalyIndex ) = convertDegreesToRadians( 139.87 ); + + // Convert Asterix state from Keplerian elements to Cartesian elements. + double earthGravitationalParameter = bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( ); + Eigen::VectorXd systemInitialState = convertKeplerianToCartesianElements( + asterixInitialStateInKeplerianElements, + earthGravitationalParameter ); + + + boost::shared_ptr< PropagatorSettings< double > > translationalPropagatorSettings = + boost::make_shared< TranslationalStatePropagatorSettings< double > > + ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, + boost::make_shared< PropagationTimeTerminationSettings >( simulationEndEpoch ) ); + + // Create mass rate model and mass propagation settings + std::map< std::string, boost::shared_ptr< basic_astrodynamics::MassRateModel > > massRateModels; + massRateModels[ "Vehicle" ] = boost::make_shared< basic_astrodynamics::CustomMassRateModel >( + boost::lambda::constant( -0.01 ) ); + Eigen::VectorXd initialMass = Eigen::VectorXd( 1 ); + initialMass( 0 ) = 500.0; + boost::shared_ptr< PropagatorSettings< double > > massPropagatorSettings = + boost::make_shared< MassPropagatorSettings< double > >( + boost::assign::list_of( "Asterix" ), massRateModels, initialMass, + boost::make_shared< PropagationTimeTerminationSettings >( simulationEndEpoch ) ); + + // Create total propagator settings, depending on current case. + boost::shared_ptr< PropagatorSettings< double > > propagatorSettings; + if( ( simulationCase % 3 ) == 0 ) + { + propagatorSettings = translationalPropagatorSettings; + } + else if( ( simulationCase % 3 ) == 1 ) + { + propagatorSettings = massPropagatorSettings; + } + else if( ( simulationCase % 3 ) == 2 ) + { + std::vector< boost::shared_ptr< PropagatorSettings< double > > > propagatorSettingsList; + propagatorSettingsList.push_back( translationalPropagatorSettings ); + propagatorSettingsList.push_back( massPropagatorSettings ); + + propagatorSettings = boost::make_shared< MultiTypePropagatorSettings< double > >( + propagatorSettingsList, + boost::make_shared< PropagationTimeTerminationSettings >( simulationEndEpoch ) ); + } + + + + boost::shared_ptr< IntegratorSettings< > > integratorSettings = + boost::make_shared< IntegratorSettings< > > + ( rungeKutta4, 0.0, fixedStepSize ); + + // Create simulation object and propagate dynamics. + SingleArcDynamicsSimulator< > dynamicsSimulator( + bodyMap, integratorSettings, propagatorSettings, true, false, true ); + + // Return propagated dynamics (if simulationCase < 3) or interpolated dynamics (else) + if( simulationCase < 3 ) + { + return dynamicsSimulator.getEquationsOfMotionNumericalSolution( ); + } + else + { + std::map< double, Eigen::VectorXd > returnMap; + double currentTime = 4.0 * fixedStepSize; + + double interpolationTimeStep = 32.1; + + // Interpolate dynamics for range of times. + while( currentTime < simulationEndEpoch - 4.0 * fixedStepSize ) + { + + // Interpolate propagated dynamics type + if( simulationCase == 3 ) + { + returnMap[ currentTime ] = bodyMap[ "Asterix" ]->getEphemeris( )->getCartesianStateFromEphemeris( currentTime ); + } + else if( simulationCase == 4 ) + { + returnMap[ currentTime ] = Eigen::VectorXd::Zero( 1 ); + returnMap[ currentTime ]( 0 ) = bodyMap[ "Asterix" ]->getBodyMassFunction( )( currentTime ); + } + else if( simulationCase == 5 ) + { + returnMap[ currentTime ] = Eigen::VectorXd::Zero( 7 ); + returnMap[ currentTime ].segment( 0, 6 ) = bodyMap[ "Asterix" ]->getEphemeris( )->getCartesianStateFromEphemeris( currentTime ); + returnMap[ currentTime ]( 6 ) = bodyMap[ "Asterix" ]->getBodyMassFunction( )( currentTime ); + + } + + // Increment time (non-resonant with integration time step). + currentTime += interpolationTimeStep; + } + + return returnMap; + } + +} +//! Test if conversion from Keplerian elements to Cartesian elements is working correctly. +BOOST_AUTO_TEST_CASE( testHybridStateDerivativeModel ) +{ + // Compare separate and multitype (independent) propagation directly from propagation (useCase = 0) and + // interpolated from reset dynamics (useCase = 1) + for( int useCase = 0; useCase < 2; useCase++ ) + { + int simulationCaseToAdd = 3; + if( useCase == 0 ) + { + simulationCaseToAdd = 0; + } + + // Propagate dynamics for translational, mass and combined state. + std::map< double, Eigen::VectorXd > translationalState = propagateKeplerOrbitAndMassState( 0 + simulationCaseToAdd ); + std::map< double, Eigen::VectorXd > massState = propagateKeplerOrbitAndMassState( 1 + simulationCaseToAdd ); + std::map< double, Eigen::VectorXd > combinedState = propagateKeplerOrbitAndMassState( 2 + simulationCaseToAdd ); + + std::map< double, Eigen::VectorXd >::const_iterator stateIterator = translationalState.begin( ); + std::map< double, Eigen::VectorXd >::const_iterator massIterator = massState.begin( ); + std::map< double, Eigen::VectorXd >::const_iterator combinedIterator = combinedState.begin( ); + + // Compare separate and multitype dynamics of each type. + for( unsigned int i = 0; i < translationalState.size( ); i++ ) + { + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( stateIterator->second, combinedIterator->second.segment( 0, 6 ), + std::numeric_limits< double >::epsilon( ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( massIterator->second, combinedIterator->second.segment( 6, 1 ), + std::numeric_limits< double >::epsilon( ) ); + stateIterator++; + massIterator++; + combinedIterator++; + } + } +} + +BOOST_AUTO_TEST_SUITE_END( ) + +} + +} diff --git a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestSequentialVariationalEquationIntegration.cpp b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestSequentialVariationalEquationIntegration.cpp new file mode 100644 index 0000000000..93ee18c967 --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestSequentialVariationalEquationIntegration.cpp @@ -0,0 +1,206 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#define BOOST_TEST_MAIN + +#include +#include +#include +#include + +#include +#include + +#include "Tudat/Basics/testMacros.h" +#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h" +#include "Tudat/Astrodynamics/Ephemerides/tabulatedEphemeris.h" + +#include "Tudat/InputOutput/basicInputOutput.h" +#include "Tudat/External/SpiceInterface/spiceInterface.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" + +#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h" +#include "Tudat/SimulationSetup/PropagationSetup/createNumericalSimulator.h" +#include "Tudat/SimulationSetup/EstimationSetup/createEstimatableParameters.h" +#include "Tudat/SimulationSetup/PropagationSetup/variationalEquationsSolver.h" + +namespace tudat +{ + +namespace unit_tests +{ + +//Using declarations. +using namespace tudat::interpolators; +using namespace tudat::numerical_integrators; +using namespace tudat::spice_interface; +using namespace tudat::simulation_setup; +using namespace tudat::basic_astrodynamics; +using namespace tudat::estimatable_parameters; +using namespace tudat::orbital_element_conversions; +using namespace tudat::ephemerides; +using namespace tudat::propagators; + + +BOOST_AUTO_TEST_SUITE( test_sequential_variational_equation_integration ) + + +std::pair< boost::shared_ptr< CombinedStateTransitionAndSensitivityMatrixInterface >, boost::shared_ptr< Ephemeris > > +integrateEquations( const bool performIntegrationsSequentially ) +{ + std::string kernelsPath = input_output::getSpiceKernelPath( ); + + //Load spice kernels. + spice_interface::loadSpiceKernelInTudat( kernelsPath + "naif0009.tls"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "pck00009.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de-403-masses.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de421.bsp"); + + std::vector< std::string > bodyNames; + bodyNames.push_back( "Earth" ); + bodyNames.push_back( "Sun" ); + bodyNames.push_back( "Moon" ); + + // Specify initial time + double initialEphemerisTime = 1.0E7; + double finalEphemerisTime = initialEphemerisTime + 14.0 * 86400.0; + double maximumTimeStep = 600.0; + + double numberOfTimeStepBuffer = 6.0; + double buffer = numberOfTimeStepBuffer * maximumTimeStep; + + // Create bodies needed in simulation + std::map< std::string, boost::shared_ptr< BodySettings > > bodySettings = + getDefaultBodySettings( bodyNames, initialEphemerisTime - buffer, finalEphemerisTime + buffer ); + NamedBodyMap bodyMap = + createBodies( bodySettings ); + boost::shared_ptr< Body > lageos = boost::make_shared< Body >( ); + bodyMap[ "LAGEOS" ] = lageos; + + // Create body initial state + basic_mathematics::Vector6d lageosKeplerianElements; + lageosKeplerianElements[ semiMajorAxisIndex ] = 8000.0E3; + lageosKeplerianElements[ eccentricityIndex ] = 0.0044; + lageosKeplerianElements[ inclinationIndex ] = 109.89 * mathematical_constants::PI / 180.0; + lageosKeplerianElements[ argumentOfPeriapsisIndex ] = 259.35 * mathematical_constants::PI / 180.0; + lageosKeplerianElements[ longitudeOfAscendingNodeIndex ] = 31.56 * mathematical_constants::PI / 180.0; + lageosKeplerianElements[ trueAnomalyIndex ] = 1.0; + basic_mathematics::Vector6d lageosState = convertKeplerianToCartesianElements( + lageosKeplerianElements, getBodyGravitationalParameter("Earth" ) ); + + lageos->setEphemeris( boost::make_shared< TabulatedCartesianEphemeris< double, double > >( + boost::shared_ptr< interpolators::OneDimensionalInterpolator< + double, basic_mathematics::Vector6d > >( ), "Earth" ) ); + setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" ); + + // Set accelerations between bodies that are to be taken into account. + SelectedAccelerationMap accelerationMap; + + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfLageos; + //accelerationsOfLageos[ "Sun" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + //accelerationsOfLageos[ "Earth" ].push_back( boost::make_shared< RelativisticCorrectionSettings >( ) ); + //accelerationsOfLageos[ "Earth" ].push_back( boost::make_shared< SphericalHarmonicAccelerationSettings >( 8, 8 ) ); + accelerationsOfLageos[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationMap[ "LAGEOS" ] = accelerationsOfLageos; + + // Set bodies for which initial state is to be estimated and integrated. + std::vector< std::string > bodiesToIntegrate; + bodiesToIntegrate.push_back( "LAGEOS" ); + unsigned int numberOfNumericalBodies = bodiesToIntegrate.size( ); + + std::vector< std::string > centralBodies; + std::map< std::string, std::string > centralBodyMap; + + centralBodies.resize( numberOfNumericalBodies ); + for( unsigned int i = 0; i < numberOfNumericalBodies; i++ ) + { + centralBodies[ i ] = "Earth"; + centralBodyMap[ bodiesToIntegrate[ i ] ] = centralBodies[ i ]; + } + AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodyMap, accelerationMap, centralBodyMap ); + + // Set parameters that are to be included. + std::vector< boost::shared_ptr< EstimatableParameterSettings > > parameterNames; + parameterNames.push_back( boost::make_shared< InitialTranslationalStateEstimatableParameterSettings< double > >( + "LAGEOS", lageosState, "Earth" ) ); + parameterNames.push_back( boost::make_shared< EstimatableParameterSettings > + ( "Earth", gravitational_parameter ) ); + parameterNames.push_back( boost::make_shared< EstimatableParameterSettings > + ( "Moon", gravitational_parameter ) ); + boost::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parametersToEstimate = + createParametersToEstimate( parameterNames, bodyMap, accelerationModelMap ); + + // Define integrator settings. + boost::shared_ptr< IntegratorSettings< > > integratorSettings = + boost::make_shared< RungeKuttaVariableStepSizeSettings< > > + ( rungeKuttaVariableStepSize, initialEphemerisTime, 10.0, + RungeKuttaCoefficients::rungeKuttaFehlberg45, 0.01, 10.0, 1.0E-6, 1.0E-6 ); + + // Define propagator settings. + boost::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings = + boost::make_shared< TranslationalStatePropagatorSettings< double > > + ( centralBodies, accelerationModelMap, bodiesToIntegrate, lageosState, finalEphemerisTime ); + + // Perform requested propagation + boost::shared_ptr< SingleArcVariationalEquationsSolver< double, double, double > > variationalEquationSolver; + if( !performIntegrationsSequentially ) + { + variationalEquationSolver = boost::make_shared< SingleArcVariationalEquationsSolver< double, double, double > >( + bodyMap, integratorSettings, + propagatorSettings, parametersToEstimate ); + } + else + { + variationalEquationSolver = boost::make_shared< SingleArcVariationalEquationsSolver< double, double, double > >( + bodyMap, integratorSettings, + propagatorSettings, parametersToEstimate, 0, + integratorSettings ); + } + + return std::make_pair( variationalEquationSolver->getStateTransitionMatrixInterface( ), + bodyMap[ "LAGEOS" ]->getEphemeris( ) ); +} + +//! Test whether concurrent and sequential propagation of variational equations gives same results. +BOOST_AUTO_TEST_CASE( testSequentialVariationalEquationIntegration ) +{ + // Propagate concurrently. + std::pair< boost::shared_ptr< CombinedStateTransitionAndSensitivityMatrixInterface >, boost::shared_ptr< Ephemeris > > + concurrentResult = integrateEquations( 0 ); + + // Propagate sequentially. + std::pair< boost::shared_ptr< CombinedStateTransitionAndSensitivityMatrixInterface >, boost::shared_ptr< Ephemeris > > + sequentialResult = integrateEquations( 1 ); + + // Test variational equations solution. + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + concurrentResult.first->getCombinedStateTransitionAndSensitivityMatrix( 1.0E7 + 14.0 * 80000.0 ), + sequentialResult.first->getCombinedStateTransitionAndSensitivityMatrix( 1.0E7 + 14.0 * 80000.0 ), 2.0E-6 ); + + // Test dynamics solution. + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + concurrentResult.second->getCartesianStateFromEphemeris( 1.0E7 + 14.0 * 80000.0 ), + sequentialResult.second->getCartesianStateFromEphemeris( 1.0E7 + 14.0 * 80000.0 ), + std::numeric_limits< double >::epsilon( ) ); + +} + +BOOST_AUTO_TEST_SUITE_END( ) + +} + +} + + + diff --git a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestStoppingConditions.cpp b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestStoppingConditions.cpp new file mode 100644 index 0000000000..0d6322ded0 --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestStoppingConditions.cpp @@ -0,0 +1,351 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#define BOOST_TEST_MAIN + +#include +#include +#include +#include + +#include +#include + +#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h" +#include +#include +#include +#include "Tudat/SimulationSetup/PropagationSetup/createNumericalSimulator.h" +#include +#include + +#include +#include +#include + +#include + +namespace tudat +{ + +namespace unit_tests +{ + +BOOST_AUTO_TEST_SUITE( test_propagation_stopping_conditions ) + +boost::shared_ptr< propagators::PropagationTerminationSettings > getTerminationSettings( const int testType ) +{ + + // Define stopping conditions, depending on test case. + boost::shared_ptr< propagators::PropagationTerminationSettings > terminationSettings; + switch( testType ) + { + // Stop at given time. + case 0: + terminationSettings = boost::make_shared< propagators::PropagationTimeTerminationSettings >( 3200.0 ); + break; + // Stop at given Mach number + case 1: + terminationSettings = boost::make_shared< propagators::PropagationDependentVariableTerminationSettings >( + boost::make_shared< propagators::SingleDependentVariableSaveSettings >( + propagators::mach_number_dependent_variable, "Apollo" ), 3.0, 1 ); + break; + // Stop at given altitude + case 2: + terminationSettings = boost::make_shared< propagators::PropagationDependentVariableTerminationSettings >( + boost::make_shared< propagators::SingleDependentVariableSaveSettings >( + propagators::altitude_dependent_variable, "Apollo" ), 10.0E3, 1 ); + break; + // Stop at given density + case 3: + terminationSettings = boost::make_shared< propagators::PropagationDependentVariableTerminationSettings >( + boost::make_shared< propagators::SingleDependentVariableSaveSettings >( + propagators::local_density_dependent_variable, "Apollo" ), 1.1, 0); + break; + // Stop when a single of the conditions 0-3 is fulfilled. + case 4: + { + std::vector< boost::shared_ptr< propagators::PropagationTerminationSettings > > constituentSettings; + constituentSettings.push_back( getTerminationSettings( 0 ) ); + constituentSettings.push_back( getTerminationSettings( 1 ) ); + constituentSettings.push_back( getTerminationSettings( 2 ) ); + constituentSettings.push_back( getTerminationSettings( 3 ) ); + + terminationSettings = boost::make_shared< propagators::PropagationHybridTerminationSettings >( + constituentSettings, 1 ); + break; + } + // Stop when all of the conditions 0-3 is fulfilled. + case 5: + { + std::vector< boost::shared_ptr< propagators::PropagationTerminationSettings > > constituentSettings; + constituentSettings.push_back( getTerminationSettings( 0 ) ); + constituentSettings.push_back( getTerminationSettings( 1 ) ); + constituentSettings.push_back( getTerminationSettings( 2 ) ); + constituentSettings.push_back( getTerminationSettings( 3 ) ); + + terminationSettings = boost::make_shared< propagators::PropagationHybridTerminationSettings >( + constituentSettings, 0 ); + break; + } + } + return terminationSettings; +} + +void performSimulation( const int testType ) +{ + using namespace ephemerides; + using namespace interpolators; + using namespace numerical_integrators; + using namespace spice_interface; + using namespace simulation_setup; + using namespace basic_astrodynamics; + using namespace orbital_element_conversions; + using namespace propagators; + using namespace aerodynamics; + using namespace basic_mathematics; + using namespace input_output; + + // Load Spice kernels. + spice_interface::loadSpiceKernelInTudat( input_output::getSpiceKernelPath( ) + "pck00009.tpc" ); + spice_interface::loadSpiceKernelInTudat( input_output::getSpiceKernelPath( ) + "de-403-masses.tpc" ); + spice_interface::loadSpiceKernelInTudat( input_output::getSpiceKernelPath( ) + "de421.bsp" ); + + + // Set simulation start epoch. + const double simulationStartEpoch = 0.0; + + // Set simulation end epoch. + const double simulationEndEpoch = 3300.0; + + // Set numerical integration fixed step size. + const double fixedStepSize = 1.0; + + + // Set Keplerian elements for Capsule. + Vector6d apolloInitialStateInKeplerianElements; + apolloInitialStateInKeplerianElements( semiMajorAxisIndex ) = spice_interface::getAverageRadius( "Earth" ) + 120.0E3; + apolloInitialStateInKeplerianElements( eccentricityIndex ) = 0.005; + apolloInitialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 85.3 ); + apolloInitialStateInKeplerianElements( argumentOfPeriapsisIndex ) + = unit_conversions::convertDegreesToRadians( 235.7 ); + apolloInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex ) + = unit_conversions::convertDegreesToRadians( 23.4 ); + apolloInitialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 139.87 ); + + // Convert apollo state from Keplerian elements to Cartesian elements. + const Vector6d apolloInitialState = convertKeplerianToCartesianElements( + apolloInitialStateInKeplerianElements, + getBodyGravitationalParameter( "Earth" ) ); + + + // Define simulation body settings. + std::map< std::string, boost::shared_ptr< BodySettings > > bodySettings = + getDefaultBodySettings( { "Earth" }, simulationStartEpoch - 10.0 * fixedStepSize, + simulationEndEpoch + 10.0 * fixedStepSize ); + bodySettings[ "Earth" ]->ephemerisSettings = boost::make_shared< simulation_setup::ConstantEphemerisSettings >( + basic_mathematics::Vector6d::Zero( ), "SSB", "J2000" ); + bodySettings[ "Earth" ]->gravityFieldSettings = + boost::make_shared< simulation_setup::GravityFieldSettings >( + central_spice ); + bodySettings[ "Earth" ]->rotationModelSettings->resetOriginalFrame( "J2000" ); + + // Create Earth object + simulation_setup::NamedBodyMap bodyMap = simulation_setup::createBodies( bodySettings ); + + // Define propagator settings variables. + SelectedAccelerationMap accelerationMap; + std::vector< std::string > bodiesToPropagate; + std::vector< std::string > centralBodies; + + // Create vehicle objects. + bodyMap[ "Apollo" ] = boost::make_shared< simulation_setup::Body >( ); + + // Create vehicle aerodynamic coefficients + bodyMap[ "Apollo" ]->setAerodynamicCoefficientInterface( + unit_tests::getApolloCoefficientInterface( ) ); + bodyMap[ "Apollo" ]->setConstantBodyMass( 5.0E3 ); + + // Finalize body creation. + setGlobalFrameBodyEphemerides( bodyMap, "SSB", "J2000" ); + + // Define acceleration model settings. + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfApollo; + accelerationsOfApollo[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationsOfApollo[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( aerodynamic ) ); + accelerationMap[ "Apollo" ] = accelerationsOfApollo; + + bodiesToPropagate.push_back( "Apollo" ); + centralBodies.push_back( "Earth" ); + + // Set initial state + basic_mathematics::Vector6d systemInitialState = apolloInitialState; + + // Create acceleration models and propagation settings, using current test case to retrieve stop settings.. + basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodyMap, accelerationMap, bodiesToPropagate, centralBodies ); + boost::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings = + boost::make_shared< TranslationalStatePropagatorSettings< double > > + ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, + getTerminationSettings( testType ) ); + boost::shared_ptr< IntegratorSettings< > > integratorSettings = + boost::make_shared< IntegratorSettings< > > + ( rungeKutta4, simulationStartEpoch, fixedStepSize ); + + // Create simulation object and propagate dynamics. + SingleArcDynamicsSimulator< > dynamicsSimulator( + bodyMap, integratorSettings, propagatorSettings, true, false, false ); + + std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > > numericalSolution = + dynamicsSimulator.getEquationsOfMotionNumericalSolution( ); + + // Check whether propagation has stopped at given conditions + switch( testType ) + { + // Check whether propagation stopped when passing t=3200.0 + case 0: + { + + BOOST_CHECK_EQUAL( ( (--( numericalSolution.end( ) ) )->first >= 3200.0 ) && + ( (--(--( numericalSolution.end( ) ) ) )->first < 3200.0 ), true ); + break; + } + case 1: + { + // Compute Mach number for last two time steps, and check whether last step was first to pass below Mach=3 + boost::shared_ptr< DynamicsStateDerivativeModel< double, double > > stateDerivativeModel = + dynamicsSimulator.getDynamicsStateDerivative( ); + stateDerivativeModel->computeStateDerivative( + (--(--( numericalSolution.end( ) ) ) )->first, (--(--( numericalSolution.end( ) ) ) )->second ); + boost::shared_ptr< FlightConditions > flightConditions = bodyMap.at( "Apollo" )->getFlightConditions( ); + double secondToLastMachNumber = flightConditions->getCurrentAirspeed( ) / flightConditions->getCurrentSpeedOfSound( ); + stateDerivativeModel->computeStateDerivative( + (--( numericalSolution.end( ) ) )->first, (--( numericalSolution.end( ) ) )->second ); + double lastMachNumber = flightConditions->getCurrentAirspeed( ) / flightConditions->getCurrentSpeedOfSound( ); + + BOOST_CHECK_EQUAL( ( lastMachNumber <= 3.0 ) && ( secondToLastMachNumber > 3.0 ), true ); + + break; + } + case 2: + { + // Compute altitude for last two time steps, and check whether last step was first to pass below altitude = 10 km + boost::shared_ptr< DynamicsStateDerivativeModel< double, double > > stateDerivativeModel = + dynamicsSimulator.getDynamicsStateDerivative( ); + stateDerivativeModel->computeStateDerivative( + (--(--( numericalSolution.end( ) ) ) )->first, (--(--( numericalSolution.end( ) ) ) )->second ); + boost::shared_ptr< FlightConditions > flightConditions = bodyMap.at( "Apollo" )->getFlightConditions( ); + double secondToLastAltitude = flightConditions->getCurrentAltitude( ); + stateDerivativeModel->computeStateDerivative( + (--( numericalSolution.end( ) ) )->first, (--( numericalSolution.end( ) ) )->second ); + double lastAltitude = flightConditions->getCurrentAltitude( ); + + BOOST_CHECK_EQUAL( ( lastAltitude <= 10.0E3 ) && ( secondToLastAltitude > 10.0E3 ), true ); + + break; + } + case 3: + { + // Compute density for last two time steps, and check whether last step was first to pass below density = 1.1 kg/m3 + boost::shared_ptr< DynamicsStateDerivativeModel< double, double > > stateDerivativeModel = + dynamicsSimulator.getDynamicsStateDerivative( ); + stateDerivativeModel->computeStateDerivative( + (--(--( numericalSolution.end( ) ) ) )->first, (--(--( numericalSolution.end( ) ) ) )->second ); + boost::shared_ptr< FlightConditions > flightConditions = bodyMap.at( "Apollo" )->getFlightConditions( ); + double secondToLastDensity = flightConditions->getCurrentDensity( ); + stateDerivativeModel->computeStateDerivative( + (--( numericalSolution.end( ) ) )->first, (--( numericalSolution.end( ) ) )->second ); + double lastDensity = flightConditions->getCurrentDensity( ); + + BOOST_CHECK_EQUAL( ( lastDensity >= 1.1 ) && ( secondToLastDensity < 1.1 ), true ); + + break; + } + // Check whether at least a single of the conditions for case 0-3 was first reached at last time step. + case 4: + { + boost::shared_ptr< DynamicsStateDerivativeModel< double, double > > stateDerivativeModel = + dynamicsSimulator.getDynamicsStateDerivative( ); + stateDerivativeModel->computeStateDerivative( + (--(--( numericalSolution.end( ) ) ) )->first, (--(--( numericalSolution.end( ) ) ) )->second ); + boost::shared_ptr< FlightConditions > flightConditions = bodyMap.at( "Apollo" )->getFlightConditions( ); + + double secondToLastMachNumber = flightConditions->getCurrentAirspeed( ) / flightConditions->getCurrentSpeedOfSound( ); + double secondToLastAltitude = flightConditions->getCurrentAltitude( ); + double secondToLastDensity = flightConditions->getCurrentDensity( ); + + stateDerivativeModel->computeStateDerivative( + (--( numericalSolution.end( ) ) )->first, (--( numericalSolution.end( ) ) )->second ); + + double lastMachNumber = flightConditions->getCurrentAirspeed( ) / flightConditions->getCurrentSpeedOfSound( ); + double lastAltitude = flightConditions->getCurrentAltitude( ); + double lastDensity = flightConditions->getCurrentDensity( ); + + BOOST_CHECK_EQUAL( ( ( (--( numericalSolution.end( ) ) )->first >= 3200.0 ) && ( (--(--( numericalSolution.end( ) ) ) )->first < 3200.0 ) ) || + ( ( lastMachNumber <= 3.0 ) && ( secondToLastMachNumber > 3.0 ) ) || + ( ( lastAltitude <= 10.0E3 ) && ( secondToLastAltitude > 10.0E3 ) ) || + ( ( lastDensity >= 1.1 ) && ( secondToLastDensity < 1.1 ) ), 1 ); + + break; + } + // Check whether all conditions for case 0-3 was first reached at last time step. + case 5: + { + boost::shared_ptr< DynamicsStateDerivativeModel< double, double > > stateDerivativeModel = + dynamicsSimulator.getDynamicsStateDerivative( ); + stateDerivativeModel->computeStateDerivative( + (--(--( numericalSolution.end( ) ) ) )->first, (--(--( numericalSolution.end( ) ) ) )->second ); + boost::shared_ptr< FlightConditions > flightConditions = bodyMap.at( "Apollo" )->getFlightConditions( ); + + double secondToLastMachNumber = flightConditions->getCurrentAirspeed( ) / flightConditions->getCurrentSpeedOfSound( ); + double secondToLastAltitude = flightConditions->getCurrentAltitude( ); + double secondToLastDensity = flightConditions->getCurrentDensity( ); + + stateDerivativeModel->computeStateDerivative( + (--( numericalSolution.end( ) ) )->first, (--( numericalSolution.end( ) ) )->second ); + + double lastMachNumber = flightConditions->getCurrentAirspeed( ) / flightConditions->getCurrentSpeedOfSound( ); + double lastAltitude = flightConditions->getCurrentAltitude( ); + double lastDensity = flightConditions->getCurrentDensity( ); + + BOOST_CHECK_EQUAL( ( ( (--( numericalSolution.end( ) ) )->first >= 3200.0 ) ) && + ( ( lastMachNumber <= 3.0 ) ) && + ( ( lastAltitude <= 10.0E3 ) ) && + ( ( lastDensity >= 1.1 ) ), 1 ); + + BOOST_CHECK_EQUAL( ( (--(--( numericalSolution.end( ) ) ) )->first < 3200.0 )|| + ( ( secondToLastMachNumber > 3.0 ) ) || + ( ( secondToLastAltitude > 10.0E3 ) ) || + ( ( secondToLastDensity < 1.1 ) ), 1 ); + + break; + } + + } +} + +//! Test to perform propagation of Apollo capsule for various stopping conditions, and checking whether the final state +//! corresponds to condition that was given. +BOOST_AUTO_TEST_CASE( testPropagationStoppingConditions ) +{ + for( unsigned int i = 0; i < 6; i++ ) + { + performSimulation( i ); + } +} + + +BOOST_AUTO_TEST_SUITE_END( ) + + +} + +} + diff --git a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestVariationalEquationPropagation.cpp b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestVariationalEquationPropagation.cpp new file mode 100644 index 0000000000..3b3f4bf663 --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestVariationalEquationPropagation.cpp @@ -0,0 +1,350 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#define BOOST_TEST_MAIN + +#include +#include +#include + +#include +#include + +#include "Tudat/Basics/testMacros.h" +#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h" + +#include "Tudat/External/SpiceInterface/spiceInterface.h" +#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" +#include "Tudat/InputOutput/basicInputOutput.h" + +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" +#include "Tudat/SimulationSetup/PropagationSetup/variationalEquationsSolver.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h" +#include "Tudat/SimulationSetup/PropagationSetup/createNumericalSimulator.h" +#include "Tudat/SimulationSetup/EstimationSetup/createEstimatableParameters.h" + +namespace tudat +{ + +namespace unit_tests +{ + +//Using declarations. +using namespace tudat::estimatable_parameters; +using namespace tudat::orbit_determination; +using namespace tudat::interpolators; +using namespace tudat::numerical_integrators; +using namespace tudat::spice_interface; +using namespace tudat::simulation_setup; +using namespace tudat::basic_astrodynamics; +using namespace tudat::orbital_element_conversions; +using namespace tudat::ephemerides; +using namespace tudat::propagators; + +BOOST_AUTO_TEST_SUITE( test_variational_equation_calculation ) + +template< typename TimeType = double , typename StateScalarType = double > + std::pair< std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > >, +std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > > +executeEarthMoonSimulation( + const std::vector< std::string > centralBodies, + const Eigen::Matrix< StateScalarType, 12, 1 > initialStateDifference = + Eigen::Matrix< StateScalarType, 12, 1 >::Zero( ), + const int propagationType = 0, + const Eigen::Vector3d parameterPerturbation = Eigen::Vector3d::Zero( ), + const bool propagateVariationalEquations = 1 ) +{ + + //Load spice kernels. + std::string kernelsPath = input_output::getSpiceKernelPath( ); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de-403-masses.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "naif0009.tls"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "pck00009.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de421.bsp"); + + // Define + std::vector< std::string > bodyNames; + bodyNames.push_back( "Earth" ); + bodyNames.push_back( "Sun" ); + bodyNames.push_back( "Moon" ); + bodyNames.push_back( "Mars" ); + + // Specify initial time + TimeType initialEphemerisTime = TimeType( 1.0E7 ); + TimeType finalEphemerisTime = initialEphemerisTime + 0.5E7; + double maximumTimeStep = 3600.0; + + double buffer = 10.0 * maximumTimeStep; + + // Create bodies needed in simulation + std::map< std::string, boost::shared_ptr< BodySettings > > bodySettings = + getDefaultBodySettings( bodyNames, initialEphemerisTime - buffer, finalEphemerisTime + buffer ); + + NamedBodyMap bodyMap = createBodies( bodySettings ); + setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" ); + + + // Set accelerations between bodies that are to be taken into account. + SelectedAccelerationMap accelerationMap; + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfEarth; + accelerationsOfEarth[ "Sun" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationsOfEarth[ "Moon" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationMap[ "Earth" ] = accelerationsOfEarth; + + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfMoon; + accelerationsOfMoon[ "Sun" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationsOfMoon[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + accelerationMap[ "Moon" ] = accelerationsOfMoon; + + // Set bodies for which initial state is to be estimated and integrated. + std::vector< std::string > bodiesToIntegrate; + bodiesToIntegrate.push_back( "Moon" ); + bodiesToIntegrate.push_back( "Earth" ); + + unsigned int numberOfNumericalBodies = bodiesToIntegrate.size( ); + + // Define propagator settings. + std::map< std::string, std::string > centralBodyMap; + + for( unsigned int i = 0; i < numberOfNumericalBodies; i++ ) + { + centralBodyMap[ bodiesToIntegrate[ i ] ] = centralBodies[ i ]; + } + + // Create acceleration models + AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodyMap, accelerationMap, centralBodyMap ); + + // Create integrator settings + boost::shared_ptr< IntegratorSettings< TimeType > > integratorSettings = + boost::make_shared< IntegratorSettings< TimeType > > + ( rungeKutta4, TimeType( initialEphemerisTime ), 1800.0 ); + + + // Set initial states of bodies to integrate. + TimeType initialIntegrationTime = initialEphemerisTime; + + // Set (perturbed) initial state. + Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > systemInitialState; + systemInitialState = getInitialStatesOfBodies< TimeType, StateScalarType >( + bodiesToIntegrate, centralBodies, bodyMap, initialIntegrationTime ); + systemInitialState += initialStateDifference; + + // Create propagator settings + boost::shared_ptr< TranslationalStatePropagatorSettings< StateScalarType > > propagatorSettings; + TranslationalPropagatorType propagatorType; + if( propagationType == 0 ) + { + propagatorType = cowell; + } + else if( propagationType == 1 ) + { + propagatorType = encke; + } + propagatorSettings = boost::make_shared< TranslationalStatePropagatorSettings< StateScalarType > > + ( centralBodies, accelerationModelMap, bodiesToIntegrate, systemInitialState, + TimeType( finalEphemerisTime ), propagatorType ); + + // Define parameters. + std::vector< boost::shared_ptr< EstimatableParameterSettings > > parameterNames; + { + parameterNames.push_back( + boost::make_shared< InitialTranslationalStateEstimatableParameterSettings< StateScalarType > >( + "Moon", propagators::getInitialStateOfBody< TimeType, StateScalarType >( + "Moon", centralBodies[ 0 ], bodyMap, TimeType( initialEphemerisTime ) ) + + initialStateDifference.segment( 0, 6 ), + centralBodies[ 0 ] ) ); + parameterNames.push_back( + boost::make_shared< InitialTranslationalStateEstimatableParameterSettings< StateScalarType > >( + "Earth", propagators::getInitialStateOfBody< TimeType, StateScalarType >( + "Earth", centralBodies[ 1 ], bodyMap, TimeType( initialEphemerisTime ) ) + + initialStateDifference.segment( 6, 6 ), + centralBodies[ 1 ] ) ); + parameterNames.push_back( boost::make_shared< EstimatableParameterSettings >( "Moon", gravitational_parameter ) ); + parameterNames.push_back( boost::make_shared< EstimatableParameterSettings >( "Earth", gravitational_parameter ) ); + parameterNames.push_back( boost::make_shared< EstimatableParameterSettings >( "Sun", gravitational_parameter ) ); + + } + + // Create parameters + boost::shared_ptr< estimatable_parameters::EstimatableParameterSet< StateScalarType > > parametersToEstimate = + createParametersToEstimate( parameterNames, bodyMap ); + + // Perturb parameters. + Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > parameterVector = + parametersToEstimate->template getFullParameterValues< StateScalarType >( ); + parameterVector.block( 12, 0, 3, 1 ) += parameterPerturbation; + parametersToEstimate->resetParameterValues( parameterVector ); + + std::pair< std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > >, + std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > > results; + + { + // Create dynamics simulator + SingleArcVariationalEquationsSolver< StateScalarType, TimeType, double > dynamicsSimulator = + SingleArcVariationalEquationsSolver< StateScalarType, TimeType, double >( + bodyMap, integratorSettings, propagatorSettings, parametersToEstimate, + 1, boost::shared_ptr< numerical_integrators::IntegratorSettings< double > >( ), 1, 0 ); + + // Propagate requested equations. + if( propagateVariationalEquations ) + { + dynamicsSimulator.integrateVariationalAndDynamicalEquations( propagatorSettings->getInitialStates( ), 1 ); + } + else + { + dynamicsSimulator.integrateDynamicalEquationsOfMotionOnly( propagatorSettings->getInitialStates( ) ); + } + + // Retrieve test data + double testEpoch = 1.4E7; + Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > testStates = + Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >::Zero( 12 ); + testStates.block( 0, 0, 6, 1 ) = bodyMap[ "Moon" ]->getStateInBaseFrameFromEphemeris( testEpoch ); + if( centralBodyMap[ "Moon" ] == "Earth" ) + { + testStates.block( 0, 0, 6, 1 ) -= bodyMap[ "Earth" ]->getStateInBaseFrameFromEphemeris( testEpoch ); + } + + testStates.block( 6, 0, 6, 1 ) = bodyMap[ "Earth" ]->getStateInBaseFrameFromEphemeris( testEpoch ); + + if( propagateVariationalEquations ) + { + results.first.push_back( dynamicsSimulator.getStateTransitionMatrixInterface( )-> + getCombinedStateTransitionAndSensitivityMatrix( testEpoch ) ); + } + results.second.push_back( testStates ); + } + return results; +} + +//! Test the state transition and sensitivity matrix computation against their numerical propagation. +/*! + * Test the state transition and sensitivity matrix computation against their numerical propagation. This unit test + * propagates the variational equations for the Earth and Moon, using both a barycentric origin and hierarchical origin. + * For the hierarchical origin, both an Encke and Cowell propagator are used. The results are compared against + * results obtained from numerical differentiation (first-order central difference). * + */ +BOOST_AUTO_TEST_CASE( testEarthMoonVariationalEquationCalculation ) +{ + std::pair< std::vector< Eigen::MatrixXd >, std::vector< Eigen::VectorXd > > currentOutput; + + std::vector< std::vector< std::string > > centralBodiesSet; + std::vector< std::string > centralBodies; + + // Define central bodt settings + centralBodies.resize( 2 ); + + centralBodies[ 0 ] = "SSB"; + centralBodies[ 1 ] = "SSB"; + centralBodiesSet.push_back( centralBodies ); + + centralBodies[ 0 ] = "Earth"; + centralBodies[ 1 ] = "Sun"; + centralBodiesSet.push_back( centralBodies ); + + + // Define variables for numerical differentiation + Eigen::Matrix< double, 12, 1> perturbedState; + Eigen::Vector3d perturbedParameter; + + Eigen::Matrix< double, 12, 1> statePerturbation; + Eigen::Vector3d parameterPerturbation; + + + for( unsigned int i = 0; i < centralBodiesSet.size( ); i++ ) + { + // Define parameter perturbation + parameterPerturbation = ( Eigen::Vector3d( ) << 1.0E10, 1.0E10, 1.0E14 ).finished( ); + + // Define state perturbation + if( i == 0 ) + { + statePerturbation = ( Eigen::Matrix< double, 12, 1>( )<< + 100000.0, 100000.0, 100000.0, 0.1, 0.1, 0.1, + 100000.0, 100000.0, 100000.0, 0.1, 0.1, 0.1 ).finished( ); + } + else if( i == 1 ) + { + statePerturbation = ( Eigen::Matrix< double, 12, 1>( )<< + 100000.0, 100000.0, 100000.0, 0.1, 0.1, 0.1, + 100000.0, 100000.0, 10000000.0, 0.1, 0.1, 10.0 ).finished( ); + } + + // Only perform Encke propagation is origin is not SSB + unsigned int maximumPropagatorType = 1; + if( i == 1 ) + { + maximumPropagatorType = 2; + } + + // Test for all requested propagator types. + for( unsigned int k = 0; k < maximumPropagatorType; k++ ) + { + // Compute state transition and sensitivity matrices + currentOutput = executeEarthMoonSimulation< double, double >( + centralBodiesSet[ i ], Eigen::Matrix< double, 12, 1 >::Zero( ), k ); + Eigen::MatrixXd stateTransitionAndSensitivityMatrixAtEpoch = currentOutput.first.at( 0 ); + Eigen::MatrixXd manualPartial = Eigen::MatrixXd::Zero( 12, 15 ); + + // Numerically compute state transition matrix + for( unsigned int j = 0; j < 12; j++ ) + { + Eigen::VectorXd upPerturbedState, downPerturbedState; + perturbedState.setZero( ); + perturbedState( j ) += statePerturbation( j ); + upPerturbedState = executeEarthMoonSimulation< double, double >( + centralBodiesSet[ i ], perturbedState, k, Eigen::Vector3d::Zero( ), 0 ).second.at( 0 ); + + perturbedState.setZero( ); + perturbedState( j ) -= statePerturbation( j ); + downPerturbedState = executeEarthMoonSimulation< double, double >( + centralBodiesSet[ i ], perturbedState, k, Eigen::Vector3d::Zero( ), 0 ).second.at( 0 ); + manualPartial.block( 0, j, 12, 1 ) = + ( upPerturbedState - downPerturbedState ) / ( 2.0 * statePerturbation( j ) ); + } + + // Numerically compute sensitivity matrix + for( unsigned int j = 0; j < 3; j ++ ) + { + Eigen::VectorXd upPerturbedState, downPerturbedState; + perturbedState.setZero( ); + Eigen::Vector3d upPerturbedParameter, downPerturbedParameter; + perturbedParameter.setZero( ); + perturbedParameter( j ) += parameterPerturbation( j ); + upPerturbedState = executeEarthMoonSimulation< double, double >( + centralBodiesSet[ i ], perturbedState, k, perturbedParameter, 0 ).second.at( 0 ); + + perturbedParameter.setZero( ); + perturbedParameter( j ) -= parameterPerturbation( j ); + downPerturbedState = executeEarthMoonSimulation< double, double >( + centralBodiesSet[ i ], perturbedState, k, perturbedParameter, 0 ).second.at( 0 ); + manualPartial.block( 0, j + 12, 12, 1 ) = + ( upPerturbedState - downPerturbedState ) / ( 2.0 * parameterPerturbation( j ) ); + } + + // Check results + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + stateTransitionAndSensitivityMatrixAtEpoch.block( 0, 0, 12, 15 ), manualPartial, 2.0E-4 ); + + } + + } +} + +BOOST_AUTO_TEST_SUITE_END( ) + +} + +} + diff --git a/Tudat/Astrodynamics/Propagators/bodyMassStateDerivative.h b/Tudat/Astrodynamics/Propagators/bodyMassStateDerivative.h new file mode 100644 index 0000000000..6b7e887371 --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/bodyMassStateDerivative.h @@ -0,0 +1,213 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_BODYMASSSTATEDERIVATIVE_H +#define TUDAT_BODYMASSSTATEDERIVATIVE_H + +#include +#include +#include + +#include +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/massRateModel.h" +#include "Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h" + + +namespace tudat +{ + +namespace propagators +{ + +//! Class for computing the derivative of the mass of a set of bodies. +/*! + * Class for computing the derivative of the mass of a set of bodies. Note that the local and global states are equal + * for mass propagation, both represent the physical body mass (in kg). + */ +template< typename StateScalarType = double, typename TimeType = double > +class BodyMassStateDerivative: public propagators::SingleStateTypeDerivative< StateScalarType, TimeType > +{ +public: + + //! Constructor + /*! + * Constructor, sets the mass rate models and the bodies that are to be propagated (single mass rate model per body). + * \param massRateModels Map of model per body that is to be used for the mass rate computation. + * \param bodiesToIntegrate List of bodies for which the mass is to be propagated. Note that this vector have + * more entries than the massRateModels map, as a body's mass can be 'propagated' with no rate model (i.e. constant + * mass). + */ + BodyMassStateDerivative( + const std::map< std::string, boost::shared_ptr< basic_astrodynamics::MassRateModel > >& massRateModels, + const std::vector< std::string >& bodiesToIntegrate ): + propagators::SingleStateTypeDerivative< StateScalarType, TimeType >( + propagators::body_mass_state ), + massRateModels_( massRateModels ), bodiesToIntegrate_( bodiesToIntegrate ){ } + + //! Constructor + /*! + * Constructor, sets the mass rate models and the bodies that are to be propagated. + * \param massRateModels Map of models per body that are to be used for the mass rate computation. + * \param bodiesToIntegrate List of bodies for which the mass is to be propagated. Note that this vector have + * more entries than the massRateModels map, as a body's mass can be 'propagated' with no rate model (i.e. constant + * mass). + */ + BodyMassStateDerivative( + const std::map< std::string, std::vector< boost::shared_ptr< basic_astrodynamics::MassRateModel > > >& + massRateModels, + const std::vector< std::string >& bodiesToIntegrate ): + propagators::SingleStateTypeDerivative< StateScalarType, TimeType >( + propagators::body_mass_state ), + massRateModels_( massRateModels ), bodiesToIntegrate_( bodiesToIntegrate ){ } + + + //! Destructor + virtual ~BodyMassStateDerivative( ){ } + + //! Calculates the state derivative of the system of equations for the mass dynamics + /*! + * Calculates the state derivative of the system of equations for the mass dynamics + * The environment and acceleration models (updateStateDerivativeModel) must be + * updated before calling this function. + * \param time Time at which the state derivative is to be calculated. + * \param stateOfSystemToBeIntegrated Current masses of the bodies that are propagated + * \param stateDerivative Mass rates of the bodies for which the mass is propagated, in the same order as + * bodiesToIntegrate_ + */ + void calculateSystemStateDerivative( + const TimeType time, + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& stateOfSystemToBeIntegrated, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > > stateDerivative ) + { + stateDerivative.setZero( ); + + // Iterate over all mass rate models, retrieve value and put into corresponding entry. + int currentIndex = 0; + for( massRateModelIterator_ = massRateModels_.begin( ); + massRateModelIterator_ != massRateModels_.end( ); + massRateModelIterator_++ ) + { + stateDerivative( currentIndex, 0 ) = 0.0; + for( unsigned int i = 0; i < massRateModelIterator_->second.size( ); i++ ) + { + stateDerivative( currentIndex, 0 ) += static_cast< StateScalarType >( + massRateModelIterator_->second.at ( i )->getMassRate( ) ); + + currentIndex++; + } + + } + } + + //! Function to clear reference/cached values of body mass state derivative model + /*! + * Function to clear reference/cached values of body mass state derivative model. All mass rate models' current times + * are reset to ensure that they are all recalculated. + */ + void clearStateDerivativeModel( ) + { + // Reset all mass rate times (to allow multiple evaluations at same time, e.g. stage 2 and 3 in RK4 integrator) + for( massRateModelIterator_ = massRateModels_.begin( ); + massRateModelIterator_ != massRateModels_.end( ); + massRateModelIterator_++ ) + { + for( unsigned int i = 0; i < massRateModelIterator_->second.size( ); i++ ) + { + massRateModelIterator_->second.at ( i )->resetTime( TUDAT_NAN ); + } + } + } + + //! Function to update the mass state derivative model to the current time. + /*! + * Function to update the mass state derivative model to the urrent time. + * cNote that this function only updates the state derivative model itself, the + * environment models must be updated before calling this function + * \param currentTime Time to which the mass state derivative is to be updated. + */ + void updateStateDerivativeModel( const TimeType currentTime ) + { + + + // Update local variables of mass rate model objects. + for( massRateModelIterator_ = massRateModels_.begin( ); + massRateModelIterator_ != massRateModels_.end( ); + massRateModelIterator_++ ) + { + for( unsigned int i = 0; i < massRateModelIterator_->second.size( ); i++ ) + { + massRateModelIterator_->second.at ( i )->updateMembers( static_cast< double >( currentTime ) ); + } + } + } + + //! Function included for compatibility purposes with base class, local and global representation is equal for mass rate + //! model. Function returns (by reference) input internalSolution. + void convertCurrentStateToGlobalRepresentation( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& internalSolution, const TimeType& time, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > currentCartesianLocalSoluton ) + { + currentCartesianLocalSoluton = internalSolution; + } + + //! Function included for compatibility purposes with base class, input and output representation is equal for mass rate + //! model. Function returns input outputSolution. + virtual Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > convertFromOutputSolution( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& outputSolution, const TimeType& time ) + { + return outputSolution; + } + + //! Function included for compatibility purposes with base class, input and output representation is equal for mass rate + //! model. Function returns (by reference) input internalSolution. + void convertToOutputSolution( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& internalSolution, + const TimeType& time, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > currentCartesianLocalSoluton ) + { + currentCartesianLocalSoluton = internalSolution; + } + + //! Function to get the total size of the state of propagated masses. + /*! + * Function to get the total size of the state of propagated masses. Equal to number of bodies for which the mass + * is propagated. + * \return Size of propagated mass state. + */ + virtual int getStateSize( ) + { + return bodiesToIntegrate_.size( ); + } + +private: + + //! Map of models per body that are to be used for the mass rate computation. + std::map< std::string, std::vector< boost::shared_ptr< basic_astrodynamics::MassRateModel > > > massRateModels_; + + //! Predefined iterator to save (de-)allocation time. + std::map< std::string, std::vector< boost::shared_ptr< basic_astrodynamics::MassRateModel > > >::const_iterator massRateModelIterator_; + + //! List of bodies for which the mass is to be propagated. + /*! + * List of bodies for which the mass is to be propagated. Note that this vector have + * more entries than the massRateModels map, as a body's mass can be 'propagated' with no rate model (i.e. constant + * mass). + */ + std::vector< std::string > bodiesToIntegrate_; + +}; + +} // namespace propagators + +} // namespace tudat + +#endif // TUDAT_BODYMASSSTATEDERIVATIVE_H diff --git a/Tudat/Astrodynamics/Propagators/centralBodyData.h b/Tudat/Astrodynamics/Propagators/centralBodyData.h index f396b3f453..5fbc4b8a0b 100644 --- a/Tudat/Astrodynamics/Propagators/centralBodyData.h +++ b/Tudat/Astrodynamics/Propagators/centralBodyData.h @@ -162,6 +162,8 @@ class CentralBodyData updateOrder_[ currentUpdateIndex ] = numericalBodies_.at( i ); currentUpdateIndex++; } + + localInternalState_.resize( 6 * bodiesToIntegrate.size( ), 1 ); } @@ -174,33 +176,36 @@ class CentralBodyData * size of bodiesToIntegrate, with entries in the order of the bodies in the bodiesToIntegrate * vector. * \param time Current time (used for retrieving states from ephemerides) - * \param areInputStateLocal True if the internalState vector is given in the local frames of - * the integrated bodies, or the global frame. - * \return Vector of states of the reference frame origins for each body. + * \param referenceFrameOriginStates Vector of states of the reference frame origins for each body + * (returned by reference). + * \param areInputStateLocal True if the internalState vector is given in the local frames of the integrated + * bodies, or the global frame. */ - std::vector< Eigen::Matrix< StateScalarType, 6, 1 > > getReferenceFrameOriginInertialStates( - Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > internalState, const TimeType time, + void getReferenceFrameOriginInertialStates( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& internalState, const TimeType time, + std::vector< Eigen::Matrix< StateScalarType, 6, 1 > >& referenceFrameOriginStates, const bool areInputStateLocal = true ) { - std::vector< Eigen::Matrix< StateScalarType, 6, 1 > > referenceFrameOriginStates_; - referenceFrameOriginStates_.resize( updateOrder_.size( ) ); + localInternalState_ = internalState; + if( referenceFrameOriginStates.size( ) != updateOrder_.size( ) ) + { + referenceFrameOriginStates.resize( updateOrder_.size( ) ); + } // Update state in correct order. for( unsigned int i = 0; i < updateOrder_.size( ); i++ ) { - referenceFrameOriginStates_[ updateOrder_[ i ] ] = - getSingleReferenceFrameOriginInertialState( internalState, - time, updateOrder_[ i ] ); + getSingleReferenceFrameOriginInertialState( + localInternalState_, time, updateOrder_.at( i ), + referenceFrameOriginStates.at( updateOrder_.at( i ) )); // Modify current input state to global frame if input is local (in propagation frame). if( areInputStateLocal ) { - internalState.segment( 6 * updateOrder_[ i ], 6 ) - += referenceFrameOriginStates_[ updateOrder_[ i ] ]; + localInternalState_.segment( 6 * updateOrder_.at( i ), 6 ) += + referenceFrameOriginStates.at( updateOrder_.at( i ) ); } } - - return referenceFrameOriginStates_; } @@ -250,6 +255,9 @@ class CentralBodyData //! as propagation origin std::map< int, int > centralBodiesFromIntegration_; + //! State of propagated bodies, with the origins translated to the global origin + Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > localInternalState_; + //! Function to get the global origin of the propagation center of a single body. /*! * Function to get the global origin of the propagation center of a single body, where the @@ -261,20 +269,19 @@ class CentralBodyData * \sa updateOrder * \param time Current time. * \param bodyIndex Index of the body for which the global origin state is to be retrieved - * \return Global origin state of the requested body. + * \param originState Global origin state of the requested body (returned by reference). */ - Eigen::Matrix< StateScalarType, 6, 1 > getSingleReferenceFrameOriginInertialState( - const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > internalSolution, + void getSingleReferenceFrameOriginInertialState( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& internalSolution, const TimeType time, - const int bodyIndex ) + const int bodyIndex, + Eigen::Matrix< StateScalarType, 6, 1 >& originState ) { - Eigen::Matrix< StateScalarType, 6, 1 > originState - = Eigen::Matrix< StateScalarType, 6, 1 >::Zero( ); - // Check origin type. switch( bodyOriginType_[ bodyIndex ] ) { case inertial: + originState.setZero( ); break; case from_ephemeris: originState @@ -289,7 +296,6 @@ class CentralBodyData boost::lexical_cast< std::string >( bodyOriginType_[ bodyIndex ] ) ); break; } - return originState; } }; diff --git a/Tudat/Astrodynamics/Propagators/dynamicsStateDerivativeModel.h b/Tudat/Astrodynamics/Propagators/dynamicsStateDerivativeModel.h index 21048f4236..bb418304ad 100644 --- a/Tudat/Astrodynamics/Propagators/dynamicsStateDerivativeModel.h +++ b/Tudat/Astrodynamics/Propagators/dynamicsStateDerivativeModel.h @@ -24,7 +24,8 @@ #include #include "Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h" -#include "Tudat/Astrodynamics/Propagators/environmentUpdater.h" +#include "Tudat/Astrodynamics/Propagators/nBodyStateDerivative.h" +#include "Tudat/Astrodynamics/Propagators/variationalEquations.h" namespace tudat { @@ -51,17 +52,20 @@ class DynamicsStateDerivativeModel /*! * Derivative model constructor. Takes state derivative model and environment * updater. Constructor checks whether all models use the same environment updater. - * \param stateDerivativeModels Vector of state derivative models, with one entry for each type - * of dynamical equation. - * \param environmentUpdater Object which is used to update time-dependent environment models - * to current time and state, - * must be consistent with member environment updaters of stateDerivativeModels entries. + * \param stateDerivativeModels Vector of state derivative models, with one entry for each type of dynamical equation. + * \param environmentUpdateFunction Function which is used to update time-dependent environment models to current time + * and state, must be consistent with member environment updaters of stateDerivativeModels entries. + * \param variationalEquations Object used for computing the state derivative in the variational equations */ DynamicsStateDerivativeModel( const std::vector< boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > > stateDerivativeModels, - const boost::shared_ptr< EnvironmentUpdater< StateScalarType, TimeType > > environmentUpdater ): - environmentUpdater_( environmentUpdater ) + const boost::function< void( + const TimeType, const std::unordered_map< IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >&, + const std::vector< IntegratedStateType > ) > environmentUpdateFunction, + const boost::shared_ptr< VariationalEquations > variationalEquations = + boost::shared_ptr< VariationalEquations >( ) ): + environmentUpdateFunction_( environmentUpdateFunction ), variationalEquations_( variationalEquations ) { std::vector< IntegratedStateType > stateTypeList; totalStateSize_ = 0; @@ -105,6 +109,11 @@ class DynamicsStateDerivativeModel // Set current model in member map. stateDerivativeModels_[ stateDerivativeModels.at( i )->getIntegratedStateType( ) ].push_back( stateDerivativeModels.at( i ) ); + + + currentStatesPerTypeInConventionalRepresentation_[ stateDerivativeModels.at( i )->getIntegratedStateType( ) ] = + Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >::Zero( + stateTypeSize_.at( stateDerivativeModels.at( i )->getIntegratedStateType( ) ), 1 ); } } @@ -123,23 +132,41 @@ class DynamicsStateDerivativeModel StateType computeStateDerivative( const TimeType time, const StateType& state ) { // Initialize state derivative - StateType stateDerivative = StateType::Zero( state.rows( ), state.cols( ) ); + if( stateDerivative_.rows( ) != state.rows( ) || stateDerivative_.cols( ) != state.cols( ) ) + { + stateDerivative_.resize( state.rows( ), state.cols( ) ); + } // If dynamical equations are integrated, update the environment with the current state. if( evaluateDynamicsEquations_ ) { - std::map< IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > - statesPerTypeInConventionalRepresentation = - convertCurrentStateToGlobalRepresentationPerType( state, time ); - environmentUpdater_->updateEnvironment( time, statesPerTypeInConventionalRepresentation, + // Iterate over all types of equations. + for( stateDerivativeModelsIterator_ = stateDerivativeModels_.begin( ); + stateDerivativeModelsIterator_ != stateDerivativeModels_.end( ); + stateDerivativeModelsIterator_++ ) + + { + for( unsigned int i = 0; i < stateDerivativeModelsIterator_->second.size( ); i++ ) + { + stateDerivativeModelsIterator_->second.at( i )->clearStateDerivativeModel( ); + } + } + + convertCurrentStateToGlobalRepresentationPerType( state, time, evaluateVariationalEquations_ ); + environmentUpdateFunction_( time, currentStatesPerTypeInConventionalRepresentation_, integratedStatesFromEnvironment_ ); } else { - environmentUpdater_->updateEnvironment( - time, std::map< IntegratedStateType, - Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >( ), - integratedStatesFromEnvironment_ ); + environmentUpdateFunction_( + time, std::unordered_map< + IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >( ), + integratedStatesFromEnvironment_ ); + } + + if( evaluateVariationalEquations_ ) + { + variationalEquations_->clearPartials( ); } // If dynamical equations are integrated, evaluate dynamics state derivatives. @@ -156,26 +183,54 @@ class DynamicsStateDerivativeModel { // Update state derivative models stateDerivativeModelsIterator_->second.at( i )->updateStateDerivativeModel( time ); + } + } + for( stateDerivativeModelsIterator_ = stateDerivativeModels_.begin( ); + stateDerivativeModelsIterator_ != stateDerivativeModels_.end( ); + stateDerivativeModelsIterator_++ ) + + { + for( unsigned int i = 0; i < stateDerivativeModelsIterator_->second.size( ); i++ ) + { // Evaluate and set current dynamical state derivative currentIndices = stateIndices_.at( stateDerivativeModelsIterator_->first ).at( i ); stateDerivativeModelsIterator_->second.at( i )->calculateSystemStateDerivative( - time, state.block( currentIndices.first, dynamicsStartColumn_, - currentIndices.second, 1 ) ); - - stateDerivative.block( currentIndices.first, dynamicsStartColumn_, - currentIndices.second, 1 ) = - stateDerivativeModelsIterator_->second.at( i )->calculateSystemStateDerivative( - time, state.block( currentIndices.first, dynamicsStartColumn_, - currentIndices.second, 1 ) ); + time, state.block( currentIndices.first, dynamicsStartColumn_, currentIndices.second, 1 ), + stateDerivative_.block( currentIndices.first, dynamicsStartColumn_, currentIndices.second, 1 ) ); + } } } - return stateDerivative; + + // If variational equations are to be integrated: evaluate and set. + if( evaluateVariationalEquations_ ) + { + variationalEquations_->updatePartials( time ); + + variationalEquations_->evaluateVariationalEquations< StateScalarType >( + time, state.block( 0, 0, totalStateSize_, variationalEquations_->getNumberOfParameterValues( ) ), + stateDerivative_.block( 0, 0, totalStateSize_, variationalEquations_->getNumberOfParameterValues( ) ) ); + } + + return stateDerivative_; } + //! Function to calculate the system state derivative with double precision, regardless of template arguments + /*! + * Function to calculate the system state derivative with double precision, regardless of template arguments + * \sa computeStateDerivative + * \param time Current time. + * \param state Current complete state. + * \return Calculated state derivative. + */ + Eigen::MatrixXd computeStateDoubleDerivative( + const double time, const Eigen::MatrixXd& state ) + { + return computeStateDerivative( static_cast< TimeType >( time ), state.template cast< StateScalarType >( ) ).template cast< double >( ); + } //! Function to convert the state in the conventional form to the propagator-specific form. /*! @@ -242,12 +297,11 @@ class DynamicsStateDerivativeModel stateDerivativeModelsIterator_->first ); for( unsigned int i = 0; i < stateDerivativeModelsIterator_->second.size( ); i++ ) { - outputState.segment( currentStateIndices.at( i ).first, - currentStateIndices.at( i ).second ) = - stateDerivativeModelsIterator_->second.at( i )->convertToOutputSolution( + stateDerivativeModelsIterator_->second.at( i )->convertToOutputSolution( internalSolution.segment( - currentStateIndices.at( i ).first, - currentStateIndices.at( i ).second ), time ); + currentStateIndices.at( i ).first, currentStateIndices.at( i ).second ), time, + outputState.block( currentStateIndices.at( i ).first, 0, + currentStateIndices.at( i ).second, 1 ) ); } } return outputState; @@ -282,21 +336,84 @@ class DynamicsStateDerivativeModel return convertedSolution; } + //! Function to add variational equations to the state derivative model + /*! + * Function to add variational equations to the state derivative model. + * \param variationalEquations Object used for computing the state derivative in the variational equations + */ + void addVariationalEquations( boost::shared_ptr< VariationalEquations > variationalEquations ) + { + variationalEquations_ = variationalEquations; + } + + //! Function to set which segments of the full state to propagate /*! * Function to set which segments of the full state to propagate, i.e. whether to propagate the * variational/dynamical equations, and which types of the dynamics to propagate. * \param stateTypesToNotIntegrate Types of dynamics to propagate - * \param evaluateDynamicsEquations Boolean to denote whether the dynamical equations are to be - * propagated or not + * \param evaluateDynamicsEquations Boolean to denote whether the dynamical equations are to be propagated or not + * \param evaluateVariationalEquations Boolean to denote whether the variational equations are to be propagated or not */ void setPropagationSettings( const std::vector< IntegratedStateType >& stateTypesToNotIntegrate, - const bool evaluateDynamicsEquations ) + const bool evaluateDynamicsEquations, + const bool evaluateVariationalEquations ) { integratedStatesFromEnvironment_ = stateTypesToNotIntegrate; evaluateDynamicsEquations_ = evaluateDynamicsEquations; - dynamicsStartColumn_ = 0; + evaluateVariationalEquations_ = evaluateVariationalEquations; + + if( evaluateVariationalEquations_ ) + { + dynamicsStartColumn_ = variationalEquations_->getNumberOfParameterValues( ); + } + else + { + dynamicsStartColumn_ = 0; + } + } + + //! Function to update the settings of the state derivative models with new initial states + /*! + * Function to update the settings of the state derivative models with new initial states. This function is + * called when using, for instance and Encke propagator for the translational dynamics, and the reference orbits + * are modified. + * \param initialBodyStates New initial state for the full propagated dynamics. + */ + void updateStateDerivativeModelSettings( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > initialBodyStates ) + { + // Iterate over all dynamics types + for( stateDerivativeModelsIterator_ = stateDerivativeModels_.begin( ); stateDerivativeModelsIterator_ != stateDerivativeModels_.end( ); + stateDerivativeModelsIterator_++ ) + { + switch( stateDerivativeModelsIterator_->first ) + { + case transational_state: + { + for( unsigned int i = 0; i < stateDerivativeModelsIterator_->second.size( ); i++ ) + { + boost::shared_ptr< NBodyStateDerivative< StateScalarType, TimeType > > currentTranslationalStateDerivative = + boost::dynamic_pointer_cast< NBodyStateDerivative< StateScalarType, TimeType > >( + stateDerivativeModelsIterator_->second.at( i ) ); + switch( currentTranslationalStateDerivative->getPropagatorType( ) ) + { + case cowell: + break; + default: + throw std::runtime_error( "Error when updating state derivative model settings, did not recognize translational propagator type" ); + break; + } + } + } + case body_mass_state: + break; + default: + throw std::runtime_error( "Error when updating state derivative model settings, did not recognize dynamics type" ); + break; + } + } } //! Function to get complete list of state derivative models, sorted per state type. @@ -304,7 +421,7 @@ class DynamicsStateDerivativeModel * Function to get complete list of state derivative models, sorted per state type. * \return Complete list of state derivative models, sorted per state type. */ - std::map< IntegratedStateType, std::vector< boost::shared_ptr + std::unordered_map< IntegratedStateType, std::vector< boost::shared_ptr < SingleStateTypeDerivative< StateScalarType, TimeType > > > > getStateDerivativeModels( ) { return stateDerivativeModels_; @@ -324,23 +441,30 @@ class DynamicsStateDerivativeModel //! Function to convert the to the conventional form in the global frame per dynamics type. /*! - * Function to convert the propagator-specific form of the state to the conventional form in the - * global frame, split by dynamics type. The conventional form is one that is typically used to - * represent the current state in the environment (e.g. Body class). For translational dynamics - * this is the Cartesian position and velocity). The inertial frame is typically the barycenter - * with J2000/ECLIPJ2000 orientation, but may differ depending on simulation settings. + * Function to convert the propagator-specific form of the state to the conventional form in the global frame, split + * by dynamics type. This function updates the currentStatesPerTypeInConventionalRepresentation_ to the current state + * and time. + * The conventional form is one that is typically used to represent the current state in the environment + * (e.g. Body class). For translational dynamics this is the Cartesian position and velocity). + * The inertial frame is typically the barycenter with J2000/ECLIPJ2000 orientation, but may differ depending on + * simulation settings * \param state State in propagator-specific form (i.e. form that is used in numerical integration). * \param time Current time at which the state is valid. - * \return State (state), converted to the 'conventional form' in inertial coordinates, - * that can for instance be set directly in the body object. + * \param stateIncludesVariationalState Boolean defining whether the stae includes the state transition/sensitivity + * matrices */ - std::map< IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > - convertCurrentStateToGlobalRepresentationPerType( const StateType& state, const TimeType& time ) + void convertCurrentStateToGlobalRepresentationPerType( + const StateType& state, const TimeType& time, const bool stateIncludesVariationalState ) { int startColumn = 0; - - std::map< IntegratedStateType, - Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > splitConventionalStates; + if( stateIncludesVariationalState ) + { + startColumn = variationalEquations_->getNumberOfParameterValues( ); + } + else + { + startColumn = 0; + } std::pair< int, int > currentIndices; @@ -348,10 +472,7 @@ class DynamicsStateDerivativeModel for( stateDerivativeModelsIterator_ = stateDerivativeModels_.begin( ); stateDerivativeModelsIterator_ != stateDerivativeModels_.end( ); stateDerivativeModelsIterator_++ ) - { - splitConventionalStates[ stateDerivativeModelsIterator_->first ] = - Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >::Zero( - stateTypeSize_.at( stateDerivativeModelsIterator_->first ), 1 ); + { int currentStateTypeSize = 0; // Iterate over all state derivative models of current type @@ -361,20 +482,24 @@ class DynamicsStateDerivativeModel currentIndices = stateIndices_.at( stateDerivativeModelsIterator_->first ).at( i ); // Set current block in split state (in global form) - splitConventionalStates[ stateDerivativeModelsIterator_->first ].block( - currentStateTypeSize, 0, currentIndices.second, 1 ) = - stateDerivativeModelsIterator_->second.at( i ) - ->convertCurrentStateToGlobalRepresentation( - state.block( currentIndices.first, startColumn, - currentIndices.second, 1 ), time ); + stateDerivativeModelsIterator_->second.at( i )->convertCurrentStateToGlobalRepresentation( + state.block( currentIndices.first, startColumn, currentIndices.second, 1 ), time, + currentStatesPerTypeInConventionalRepresentation_.at( + stateDerivativeModelsIterator_->first ).block( + currentStateTypeSize, 0, currentIndices.second, 1 ) ); + currentStateTypeSize += currentIndices.second; } } - return splitConventionalStates; } - //! Object used to update the environment to the current state and time. - boost::shared_ptr< EnvironmentUpdater< StateScalarType, TimeType > > environmentUpdater_; + boost::function< + void( const TimeType, const std::unordered_map< IntegratedStateType, + Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >&, + const std::vector< IntegratedStateType > ) > environmentUpdateFunction_; + + //! Object used for computing the state derivative in the variational equations + boost::shared_ptr< VariationalEquations > variationalEquations_; //! Map that denotes for each state derivative model the start index and size of the associated //! state in the full state vector. @@ -387,14 +512,12 @@ class DynamicsStateDerivativeModel std::map< IntegratedStateType, int > stateTypeStartIndex_; //! Complete list of state derivative models, sorted per state type. - std::map< IntegratedStateType, - std::vector< boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > > > - stateDerivativeModels_; + std::unordered_map< IntegratedStateType, + std::vector< boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > > > stateDerivativeModels_; //! Predefined iterator for computational efficiency. - typename std::map< IntegratedStateType, std::vector< boost::shared_ptr - < SingleStateTypeDerivative< StateScalarType, TimeType > > > >::iterator - stateDerivativeModelsIterator_; + typename std::unordered_map< IntegratedStateType, std::vector< boost::shared_ptr + < SingleStateTypeDerivative< StateScalarType, TimeType > > > >::iterator stateDerivativeModelsIterator_; //! Total length of state vector. int totalStateSize_; @@ -406,13 +529,154 @@ class DynamicsStateDerivativeModel //! Boolean denoting whether the equations of motion are to be propagated or not. bool evaluateDynamicsEquations_; + //! Boolean denoting whether the variational equations are to be propagated or not. + bool evaluateVariationalEquations_; + //! Start index in propagated matrix of the equations of motion (=0 if variational equations are //! not propagated). int dynamicsStartColumn_; + //! Current state derivative, as computed by computeStateDerivative. + StateType stateDerivative_; + //! Current state in 'conventional' representation, computed from current propagated state by + //! convertCurrentStateToGlobalRepresentationPerType + std::unordered_map< IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > + currentStatesPerTypeInConventionalRepresentation_; }; +//! Function to retrieve a single given acceleration model from a list of models +/*! + * Function to retrieve a single given acceleration model, determined by + * the body exerting and undergoing the acceleration, as well as the acceleration type, from a list of + * state derivative models. + * \param bodyUndergoingAcceleration Name of body undergoing the acceleration. + * \param bodyExertingAcceleration Name of body exerting the acceleration. + * \param stateDerivativeModels Complete list of state derivativ models + * \param accelerationModeType Type of acceleration model that is to be retrieved. + */ +template< typename TimeType = double, typename StateScalarType = double > +std::vector< boost::shared_ptr< basic_astrodynamics::AccelerationModel3d > > getAccelerationBetweenBodies( + const std::string bodyUndergoingAcceleration, + const std::string bodyExertingAcceleration, + const std::unordered_map< IntegratedStateType, + std::vector< boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > > > stateDerivativeModels, + const basic_astrodynamics::AvailableAcceleration accelerationModeType ) + +{ + std::vector< boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > > + listOfSuitableAccelerationModels; + + // Retrieve acceleration models + if( stateDerivativeModels.count( propagators::transational_state ) == 1 ) + { + basic_astrodynamics::AccelerationMap accelerationModelList = + boost::dynamic_pointer_cast< NBodyStateDerivative< StateScalarType, TimeType > >( + stateDerivativeModels.at( propagators::transational_state ).at( 0 ) )->getAccelerationsMap( ); + if( accelerationModelList.count( bodyUndergoingAcceleration ) == 0 ) + { + + std::string errorMessage = "Error when getting acceleration between bodies, no translational dynamics models acting on " + + bodyUndergoingAcceleration + " are found"; + throw std::runtime_error( errorMessage ); + } + else + { + // Retrieve accelerations acting on bodyUndergoingAcceleration + if( accelerationModelList.at( bodyUndergoingAcceleration ).count( bodyExertingAcceleration ) == 0 ) + { + std::string errorMessage = "Error when getting acceleration between bodies, no translational dynamics models by " + + bodyExertingAcceleration + " acting on " + bodyUndergoingAcceleration + " are found"; + throw std::runtime_error( errorMessage ); + } + else + { + // Retrieve required acceleration. + listOfSuitableAccelerationModels = basic_astrodynamics::getAccelerationModelsOfType( + accelerationModelList.at( bodyUndergoingAcceleration ).at( bodyExertingAcceleration ), accelerationModeType ); + } + } + } + else + { + std::string errorMessage = "Error when getting acceleration between bodies, no translational dynamics models found"; + throw std::runtime_error( errorMessage ); + } + return listOfSuitableAccelerationModels; +} + +//! Function to retrieve the state derivative models for translational dynamics of given body. +/*! + * Function to retrieve the state derivative models for translational dynamics (object of derived class from + * NBodyStateDerivative) of given body from full list of state derivative models + * \param bodyUndergoingAcceleration Name of body for which state derivative model is to be retrieved + * \param stateDerivativeModels Complete list of state derivativ models + */ +template< typename TimeType = double, typename StateScalarType = double > +boost::shared_ptr< NBodyStateDerivative< StateScalarType, TimeType > > getTranslationalStateDerivativeModelForBody( + const std::string bodyUndergoingAcceleration, + const std::unordered_map< IntegratedStateType, + std::vector< boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > > >& stateDerivativeModels ) + +{ + bool modelFound = 0; + boost::shared_ptr< NBodyStateDerivative< StateScalarType, TimeType > > modelForBody; + + // Check if translational state derivative models exists + if( stateDerivativeModels.count( propagators::transational_state ) > 0 ) + { + for( unsigned int i = 0; i < stateDerivativeModels.at( propagators::transational_state ).size( ); i++ ) + { + boost::shared_ptr< NBodyStateDerivative< StateScalarType, TimeType > > nBodyModel = + boost::dynamic_pointer_cast< NBodyStateDerivative< StateScalarType, TimeType > >( + stateDerivativeModels.at( propagators::transational_state ).at( i ) ); + std::vector< std::string > propagatedBodies = nBodyModel->getBodiesToBeIntegratedNumerically( ); + + // Check if bodyUndergoingAcceleration is propagated by bodyUndergoingAcceleration + if( std::find( propagatedBodies.begin( ), propagatedBodies.end( ), bodyUndergoingAcceleration ) + != propagatedBodies.end( ) ) + { + if( modelFound == true ) + { + std::string errorMessage = "Error when getting translational dynamics model for " + + bodyUndergoingAcceleration + ", multiple models found"; + throw std::runtime_error( errorMessage ); + } + else + { + modelForBody = nBodyModel; + modelFound = true; + } + } + } + } + else + { + std::string errorMessage = "Error when getting translational dynamics model for " + + bodyUndergoingAcceleration + " no translational dynamics models found"; + throw std::runtime_error( errorMessage ); + } + return modelForBody; +} + +template< typename TimeType = double, typename StateScalarType = double, + typename ConversionClassType = DynamicsStateDerivativeModel< TimeType, StateScalarType > > +std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > convertNumericalStateSolutionsToOutputSolutions( + const std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& rawSolution, + boost::shared_ptr< ConversionClassType > converterClass ) +{ + // Initialize converted solution. + std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > convertedSolution; + + // Iterate over all times. + for( typename std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >::const_iterator stateIterator = + rawSolution.begin( ); stateIterator != rawSolution.end( ); stateIterator++ ) + { + // Convert solution at this time to output (typically ephemeris frame of given body) solution + convertedSolution[ stateIterator->first ] = converterClass->convertToOutputSolution( stateIterator->second, stateIterator->first ); + } + return convertedSolution; +} } // namespace propagators } // namespace tudat diff --git a/Tudat/Astrodynamics/Propagators/environmentUpdateTypes.cpp b/Tudat/Astrodynamics/Propagators/environmentUpdateTypes.cpp new file mode 100644 index 0000000000..8f154fdd5c --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/environmentUpdateTypes.cpp @@ -0,0 +1,67 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include +#include "Tudat/Astrodynamics/Propagators/environmentUpdateTypes.h" + +namespace tudat +{ + +namespace propagators +{ + +//! Function to extend existing list of required environment update types +void addEnvironmentUpdates( std::map< propagators::EnvironmentModelsToUpdate, + std::vector< std::string > >& environmentUpdateList, + const std::map< propagators::EnvironmentModelsToUpdate, + std::vector< std::string > > updatesToAdd ) +{ + // Iterate over all environment update types. + for( std::map< propagators::EnvironmentModelsToUpdate, + std::vector< std::string > >::const_iterator + environmentUpdateIterator = updatesToAdd.begin( ); + environmentUpdateIterator != updatesToAdd.end( ); environmentUpdateIterator++ ) + { + bool addCurrentUpdate = 0; + + // Iterate over all updated bodies. + for( unsigned int i = 0; i < environmentUpdateIterator->second.size( ); i++ ) + { + addCurrentUpdate = 0; + + // Check if current update type exists. + if( environmentUpdateList.count( environmentUpdateIterator->first ) == 0 ) + { + addCurrentUpdate = 1; + } + // Check if current body exists for update type. + else if( std::find( environmentUpdateList.at( environmentUpdateIterator->first ).begin( ), + environmentUpdateList.at( environmentUpdateIterator->first ).end( ), + environmentUpdateIterator->second.at( i ) ) == + environmentUpdateList.at( environmentUpdateIterator->first ).end( ) ) + { + addCurrentUpdate = 1; + } + + // Add update type if required. + if( addCurrentUpdate ) + { + environmentUpdateList[ environmentUpdateIterator->first ].push_back( + environmentUpdateIterator->second.at( i ) ); + } + } + } +} + + +} + +} + diff --git a/Tudat/Astrodynamics/Propagators/environmentUpdateTypes.h b/Tudat/Astrodynamics/Propagators/environmentUpdateTypes.h new file mode 100644 index 0000000000..99c091d7e1 --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/environmentUpdateTypes.h @@ -0,0 +1,52 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + + +#ifndef TUDAT_ENVIRONMENTUPDATETYPES_H +#define TUDAT_ENVIRONMENTUPDATETYPES_H + +#include +#include +#include + +namespace tudat +{ + +namespace propagators +{ + +//! Enum defining types of environment model updates that can be done. +enum EnvironmentModelsToUpdate +{ + body_transational_state_update = 0, + body_rotational_state_update = 1, + body_mass_update = 2, + spherical_harmonic_gravity_field_update = 3, + vehicle_flight_conditions_update = 4, + radiation_pressure_interface_update = 5 +}; + +//! Function to extend existing list of required environment update types +/*! + * Function to extend existing list of required environment update types + * \param environmentUpdateList List of environment updates to extend + * (passed by reference and modified by function) + * \param updatesToAdd List of environment updates that are to be added to environmentUpdateList + */ +void addEnvironmentUpdates( + std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > >& + environmentUpdateList, + const std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > + updatesToAdd ); + +} // namespace propagators + +} // namespace tudat +#endif // TUDAT_ENVIRONMENTUPDATETYPES_H diff --git a/Tudat/Astrodynamics/Propagators/integrateEquations.cpp b/Tudat/Astrodynamics/Propagators/integrateEquations.cpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tudat/Astrodynamics/Propagators/integrateEquations.h b/Tudat/Astrodynamics/Propagators/integrateEquations.h index a71366cf57..a2bc0b5561 100644 --- a/Tudat/Astrodynamics/Propagators/integrateEquations.h +++ b/Tudat/Astrodynamics/Propagators/integrateEquations.h @@ -11,16 +11,14 @@ #ifndef TUDAT_INTEGRATEEQUATIONS_H #define TUDAT_INTEGRATEEQUATIONS_H -#include -#include - #include +#include + #include "Tudat/Mathematics/NumericalIntegrators/numericalIntegrator.h" #include "Tudat/Astrodynamics/BasicAstrodynamics/timeConversions.h" #include "Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h" -#include "Tudat/Astrodynamics/Propagators/environmentUpdater.h" #include "Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h" #include "Tudat/Mathematics/Interpolators/lagrangeInterpolator.h" @@ -32,98 +30,139 @@ namespace propagators //! Function to numerically integrate a given first order differential equation /*! - * Function to numerically integrate a given first order differential equation, with the state - * derivative a function of a single independent variable and the current state - * \param stateDerivativeFunction Function returning the state derivative from current time and state. - * \param initialState Initial state - * \param integratorSettings Settings for numerical integrator. + * Function to numerically integrate a given first order differential equation, with the state derivative a function of + * a single independent variable and the current state + * \param integrator Numerical integrator used for propagation + * \param initialTimeStep Time step to use for first step of numerical integration + * \param stopPropagationFunction Function determining whether the propagation is to be stopped at the current time. + * \param solutionHistory History of dependent variables that are to be saved given as map + * (time as key; returned by reference) + * \param dependentVariableHistory History of dependent variables that are to be saved given as map + * (time as key; returned by reference) + * \param dependentVariableFunction Function returning dependent variables (obtained from environment and state + * derivative model). + * \param saveFrequency Frequency at which to save the numerical integrated states (in units of i.e. per n integration time + * steps, with n = saveFrequency). * \param printInterval Frequency with which to print progress to console (nan = never). - * \return History of numerical states (first of pair) and derivatives of states (second of pair) - * given as maps with time as key. */ template< typename StateType = Eigen::MatrixXd, typename TimeType = double > -std::map< TimeType, StateType > integrateEquations( - boost::function< StateType( const TimeType, const StateType&) > stateDerivativeFunction, - const StateType initialState, - boost::shared_ptr< numerical_integrators::IntegratorSettings< TimeType > > integratorSettings, +void integrateEquations( + const boost::shared_ptr< numerical_integrators::NumericalIntegrator< TimeType, StateType, StateType > > integrator, + const double initialTimeStep, + const boost::function< bool( const double ) > stopPropagationFunction, + std::map< TimeType, StateType >& solutionHistory, + std::map< TimeType, Eigen::VectorXd >& dependentVariableHistory, + const boost::function< Eigen::VectorXd( ) > dependentVariableFunction = + boost::function< Eigen::VectorXd( ) >( ), + const int saveFrequency = TUDAT_NAN, const TimeType printInterval = TUDAT_NAN ) { - using namespace tudat::numerical_integrators; - - - // Create numerical integrator. - boost::shared_ptr< NumericalIntegrator< TimeType, StateType, StateType > > integrator - = createIntegrator< TimeType, StateType >( stateDerivativeFunction, - initialState, integratorSettings ); // Get Initial state and time. - TimeType currentTime = integratorSettings->initialTime_; - StateType newState = initialState; + TimeType currentTime = integrator->getCurrentIndependentVariable( ); + TimeType initialTime = currentTime; + StateType newState = integrator->getCurrentState( ); - // Initialization of numerical solutions for variational equations. - std::map< TimeType, StateType > solutionHistory; + // Initialization of numerical solutions for variational equations + solutionHistory.clear( ); solutionHistory[ currentTime ] = newState; + dependentVariableHistory.clear( ); + - // Check if numerical integration is forward or backwrd. - TimeType timeStepSign = 1.0L; - if( integratorSettings->initialTimeStep_ < 0.0 ) + if( !dependentVariableFunction.empty( ) ) { - timeStepSign = -1.0L; + integrator->getStateDerivativeFunction( )( currentTime, newState ); + dependentVariableHistory[ currentTime ] = dependentVariableFunction( ); } // Set initial time step and total integration time. - TimeType timeStep = integratorSettings->initialTimeStep_; - TimeType endTime = integratorSettings->endTime_; + TimeType timeStep = initialTimeStep; TimeType previousTime = currentTime; - // Perform first integration step. - newState = integrator->performIntegrationStep( timeStep ); - - currentTime = integrator->getCurrentIndependentVariable( ); - - timeStep = timeStepSign * integrator->getNextStepSize( ); - solutionHistory[ currentTime ] = newState; - - int printIndex = 0; - int printFrequency = integratorSettings->printFrequency_; + int saveIndex = 0; // Perform numerical integration steps until end time reached. - while( timeStepSign * static_cast< TimeType >( currentTime ) - < timeStepSign * static_cast< TimeType >( endTime ) ) + do { previousTime = currentTime; // Perform integration step. newState = integrator->performIntegrationStep( timeStep ); currentTime = integrator->getCurrentIndependentVariable( ); - timeStep = timeStepSign * integrator->getNextStepSize( ); + timeStep = integrator->getNextStepSize( ); // Save integration result in map - printIndex++; - printIndex = printIndex % printFrequency; - if( printIndex == 0 ) + saveIndex++; + saveIndex = saveIndex % saveFrequency; + if( saveIndex == 0 ) { solutionHistory[ currentTime ] = newState; + + if( !dependentVariableFunction.empty( ) ) + { + integrator->getStateDerivativeFunction( )( currentTime, newState ); + dependentVariableHistory[ currentTime ] = dependentVariableFunction( ); + } } // Print solutions if( printInterval == printInterval ) { - if( ( static_cast( std::fabs( currentTime - integratorSettings->initialTime_ ) ) % + if( ( static_cast( std::fabs( currentTime - initialTime ) ) % static_cast< int >( printInterval ) ) < - ( static_cast< int >( std::fabs( previousTime - integratorSettings->initialTime_ ) ) % + ( static_cast< int >( std::fabs( previousTime - initialTime ) ) % static_cast( printInterval ) ) ) { - std::cout << "Current time and state in integration: " << std::setprecision( 10 ) << - timeStep << " " << currentTime << " " << newState.transpose( ) << std::endl; + std::cout<<"Current time and state in integration: "<( currentTime ) ) ); +} + +//! Function to numerically integrate a given first order differential equation +/*! + * Function to numerically integrate a given first order differential equation, with the state derivative a function of + * a single independent variable and the current state + * \param stateDerivativeFunction Function returning the state derivative from current time and state. + * \param solutionHistory History of numerical states given as map (time as key; returned by reference) + * \param initialState Initial state + * \param integratorSettings Settings for numerical integrator. + * \param stopPropagationFunction Function determining whether the propagation is to be stopped at the current time. + * \param dependentVariableHistory History of dependent variables that are to be saved given as map + * (time as key; returned by reference) + * \param dependentVariableFunction Function returning dependent variables (obtained from environment and state + * derivative model). + * \param printInterval Frequency with which to print progress to console (nan = never). + */ +template< typename StateType = Eigen::MatrixXd, typename TimeType = double > +void integrateEquations( + boost::function< StateType( const TimeType, const StateType& ) > stateDerivativeFunction, + std::map< TimeType, StateType >& solutionHistory, + const StateType initialState, + const boost::shared_ptr< numerical_integrators::IntegratorSettings< TimeType > > integratorSettings, + const boost::function< bool( const double ) > stopPropagationFunction, + std::map< TimeType, Eigen::VectorXd >& dependentVariableHistory, + const boost::function< Eigen::VectorXd( ) > dependentVariableFunction = + boost::function< Eigen::VectorXd( ) >( ), + const TimeType printInterval = TUDAT_NAN ) +{ + // Create numerical integrator. + boost::shared_ptr< numerical_integrators::NumericalIntegrator< TimeType, StateType, StateType > > integrator = + numerical_integrators::createIntegrator< TimeType, StateType >( + stateDerivativeFunction, initialState, integratorSettings ); + + integrateEquations< StateType, TimeType >( + integrator, integratorSettings->initialTimeStep_, stopPropagationFunction, solutionHistory, + dependentVariableHistory, + dependentVariableFunction, + integratorSettings->saveFrequency_, printInterval ); - return solutionHistory; } } // namespace propagators } // namespace tudat + #endif // TUDAT_INTEGRATEEQUATIONS_H diff --git a/Tudat/Astrodynamics/Propagators/nBodyCowellStateDerivative.h b/Tudat/Astrodynamics/Propagators/nBodyCowellStateDerivative.h index 0b843ceebb..b9d10fc31f 100644 --- a/Tudat/Astrodynamics/Propagators/nBodyCowellStateDerivative.h +++ b/Tudat/Astrodynamics/Propagators/nBodyCowellStateDerivative.h @@ -19,51 +19,86 @@ namespace tudat namespace propagators { +//! Class for computing the state derivative of translational motion of N bodies, using a Cowell propagator. template< typename StateScalarType = double, typename TimeType = double > class NBodyCowellStateDerivative: public NBodyStateDerivative< StateScalarType, TimeType > { public: + + //! Constructor + /*! + * Constructor + * \param accelerationModelsPerBody A map containing the list of accelerations acting on each + * body, identifying the body being acted on and the body acted on by an acceleration. The map + * has as key a string denoting the name of the body the list of accelerations, provided as the + * value corresponding to a key, is acting on. This map-value is again a map with string as + * key, denoting the body exerting the acceleration, and as value a pointer to an acceleration + * model. + * \param centralBodyData Object responsible for providing the current integration origins from + * the global origins. + * \param bodiesToIntegrate List of names of bodies that are to be integrated numerically. + */ NBodyCowellStateDerivative( const basic_astrodynamics::AccelerationMap& accelerationModelsPerBody, const boost::shared_ptr< CentralBodyData< StateScalarType, TimeType > > centralBodyData, const std::vector< std::string >& bodiesToIntegrate ): NBodyStateDerivative< StateScalarType, TimeType >( accelerationModelsPerBody, centralBodyData, cowell, bodiesToIntegrate ){ } + //! Destructor ~NBodyCowellStateDerivative( ){ } //! Calculates the state derivative of the translational motion of the system. /*! - * Calculates the state derivative (velocity+acceleration of each body) of the translational - * motion of the system at the given time and position/velocity of bodies. - * \param time Time (TDB seconds since J2000) at which the system is to be updated. - * \param stateOfSystemToBeIntegrated List of 6 * bodiesToBeIntegratedNumerically_.size( ), - * containing Caartesian position/velocity of the bodies being integrated. The order of the - * values is defined by the order of bodies in bodiesToBeIntegratedNumerically_ - * \return Current state derivative (velocity+acceleration) of system of bodies integrated numerically. + * Calculates the state derivative (velocity+acceleration of each body) of the translational motion of the system + * at the given time and position/velocity of bodies. + * \param time Time (TDB seconds since J2000) at which the system is to be updated. + * \param stateOfSystemToBeIntegrated List of 6 * bodiesToBeIntegratedNumerically_.size( ), containing Caartesian + * position/velocity of the bodies being integrated. The order of the values is defined by the order of bodies in + * bodiesToBeIntegratedNumerically_ + * \param stateDerivative Current state derivative (velocity+acceleration) of system of bodies integrated numerically + * (returned by reference). */ - Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > calculateSystemStateDerivative( - const TimeType time, - const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& stateOfSystemToBeIntegrated ) + void calculateSystemStateDerivative( + const TimeType time, const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& stateOfSystemToBeIntegrated, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > > stateDerivative ) { - return ( this->sumStateDerivativeContributions( - stateOfSystemToBeIntegrated.template cast< double >( ), - time ) ).template cast< StateScalarType >( ); + stateDerivative.setZero( ); + this->sumStateDerivativeContributions( stateOfSystemToBeIntegrated, stateDerivative ); } + //! Function to convert the state in the conventional form to the propagator-specific form. + /*! + * Function to convert the state in the conventional form to the propagator-specific form. For the Cowell propagator, + * the two are equivalent, and this function returns the input state. + * \param cartesianSolution State in 'conventional form' + * \param time Current time at which the state is valid (not used in this class). + * \return State (outputSolution), converted to the 'propagator-specific form' (which is equal to outputSolution). + */ Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > convertFromOutputSolution( - const Eigen::Matrix< StateScalarType, Eigen::Dynamic, - Eigen::Dynamic >& cartesianSolution, const TimeType& time ) + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& cartesianSolution, const TimeType& time ) { return cartesianSolution; } - Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > - convertToOutputSolution( - const Eigen::Matrix< StateScalarType, Eigen::Dynamic, - Eigen::Dynamic >& internalSolution, const TimeType& time ) + //! Function to convert the propagator-specific form of the state to the conventional form. + /*! + * Function to convert the propagator-specific form of the state to the conventional form. For the Cowell propagator, + * the two are equivalent, and this function returns the input state. + * In contrast to the convertCurrentStateToGlobalRepresentation function, this + * function does not provide the state in the inertial frame, but instead provides it in the + * frame in which it is propagated. + * \param internalSolution State in propagator-specific form (i.e. form that is used in + * numerical integration, equal to conventional form for this class). + * \param time Current time at which the state is valid (not used in this class). + * \param currentCartesianLocalSoluton State (internalSolution), converted to the 'conventional form', + * which is equal to outputSolution for this class (returned by reference). + */ + void convertToOutputSolution( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& internalSolution, const TimeType& time, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > currentCartesianLocalSoluton ) { - return internalSolution; + currentCartesianLocalSoluton = internalSolution; } }; diff --git a/Tudat/Astrodynamics/Propagators/nBodyEnckeStateDerivative.cpp b/Tudat/Astrodynamics/Propagators/nBodyEnckeStateDerivative.cpp new file mode 100644 index 0000000000..5ef934d639 --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/nBodyEnckeStateDerivative.cpp @@ -0,0 +1,136 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/Astrodynamics/Propagators/nBodyEnckeStateDerivative.h" + +namespace tudat +{ + +namespace propagators +{ + +//! Function to remove the central gravity acceleration from an AccelerationMap +std::vector< boost::function< double( ) > > removeCentralGravityAccelerations( + const std::vector< std::string >& centralBodies, const std::vector< std::string >& bodiesToIntegrate, + basic_astrodynamics::AccelerationMap& accelerationModelsPerBody ) +{ + using namespace basic_astrodynamics; + using namespace gravitation; + + std::vector< boost::function< double( ) > > centralBodyGravitationalParameters; + centralBodyGravitationalParameters.resize( bodiesToIntegrate.size( ) ); + + std::vector< boost::shared_ptr< AccelerationModel< Eigen::Vector3d > > > listOfAccelerations; + + // Iterate over all central bodies. + for( unsigned int i = 0; i < centralBodies.size( ); i++ ) + { + // Check if current central body is exerting any accelerations on current body. + if( accelerationModelsPerBody[ bodiesToIntegrate.at( i ) ].count( centralBodies.at( i ) ) == 0 ) + { + std::string errorMessage = + "Error, cannot remove central point gravity of body " + bodiesToIntegrate.at( i ) + + " with central body " + centralBodies.at( i ) + " no accelerations due to requested central body found."; + throw std::runtime_error( errorMessage ); + } + else + { + // Find central acceleration to central body, i.e. iterate over list of accelerations and find + // central acceleration candiates (should be 1) + int lastCandidate = -1; + int numberOfCandidates = 0; + bool isLastCandidateSphericalHarmonic = 0; + listOfAccelerations = + accelerationModelsPerBody[ bodiesToIntegrate.at( i ) ][ centralBodies.at( i ) ]; + + for( unsigned int j = 0; j < listOfAccelerations.size( ); j++ ) + { + // Get type of current acceleration. + AvailableAcceleration currentAccelerationType = getAccelerationModelType( listOfAccelerations[ j ] ); + + // If central gravity, set as central acceleration candidate. + if( currentAccelerationType == central_gravity ) + { + numberOfCandidates++; + lastCandidate = j; + } + else if( currentAccelerationType == spherical_harmonic_gravity ) + { + isLastCandidateSphericalHarmonic = 1; + numberOfCandidates++; + lastCandidate = j; + } + else if( ( currentAccelerationType == third_body_central_gravity ) || + ( currentAccelerationType == third_body_spherical_harmonic_gravity ) ) + { + std::string errorMessage = + "Error when removing central body point gravity term, removal of 3rd body accelerations (of " + + centralBodies.at( i ) + + " on " + bodiesToIntegrate.at( i ) + ",) not yet supported"; + throw std::runtime_error( errorMessage ); + } + } + + // If no or multiple central acceleration candidates were found, give error. + if( numberOfCandidates == 0 ) + { + std::string errorMessage = + "Error when removing central body point gravity term on body " + + bodiesToIntegrate.at( i ) + + " with central body " + centralBodies.at( i ) + ", no central gravity found."; + throw std::runtime_error( errorMessage ); + + } + else if( numberOfCandidates != 1 ) + { + std::string errorMessage = + "Error when removing central body point gravity term on body " + + bodiesToIntegrate.at( i ) + + " with central body " + centralBodies.at( i ) + ", multiple central gravities found."; + throw std::runtime_error( errorMessage ); + } + else + { + if( !isLastCandidateSphericalHarmonic ) + { + // Set central body gravitational parameter (used for Kepler orbit propagation) + centralBodyGravitationalParameters.at( i ) = + boost::dynamic_pointer_cast< CentralGravitationalAccelerationModel3d >( + listOfAccelerations.at( lastCandidate ) )->getGravitationalParameterFunction( ); + + // Remove central acceleration from list of accelerations that are evaluated at each time step. + listOfAccelerations.erase( listOfAccelerations.begin( ) + lastCandidate, + listOfAccelerations.begin( ) + lastCandidate + 1 ); + accelerationModelsPerBody[ bodiesToIntegrate.at( i ) ][ centralBodies.at( i ) ] = + listOfAccelerations; + } + else + { + boost::shared_ptr< SphericalHarmonicsGravitationalAccelerationModel > originalAcceleration = + boost::dynamic_pointer_cast< SphericalHarmonicsGravitationalAccelerationModel >( + listOfAccelerations.at( lastCandidate ) ); + centralBodyGravitationalParameters.at( i ) = originalAcceleration->getGravitationalParameterFunction( ); + + // Create 'additional' acceleration model which subtracts central gravity term. + accelerationModelsPerBody[ bodiesToIntegrate.at( i ) ][ centralBodies.at( i ) ].push_back( + boost::make_shared< CentralGravitationalAccelerationModel3d > + ( originalAcceleration->getStateFunctionOfBodyExertingAcceleration( ), + originalAcceleration->getGravitationalParameterFunction( ), + originalAcceleration->getStateFunctionOfBodyUndergoingAcceleration( ) ) ); + } + } + } + } + return centralBodyGravitationalParameters; +} + +} + +} diff --git a/Tudat/Astrodynamics/Propagators/nBodyEnckeStateDerivative.h b/Tudat/Astrodynamics/Propagators/nBodyEnckeStateDerivative.h new file mode 100644 index 0000000000..8bca2bbd4f --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/nBodyEnckeStateDerivative.h @@ -0,0 +1,328 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_NBODYENCKESTATEDERIVATIVE_H +#define TUDAT_NBODYENCKESTATEDERIVATIVE_H + +#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/keplerPropagator.h" + +#include "Tudat/Mathematics/RootFinders/rootFinder.h" +#include "Tudat/Astrodynamics/Gravitation/centralGravityModel.h" + +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h" +#include "Tudat/Astrodynamics/Propagators/nBodyStateDerivative.h" + +namespace tudat +{ + +namespace propagators +{ + +//! Function to calculate Encke's function, to be used during propagation using Encke's method +/*! + * Function to calculate Encke's function, to be used during propagation using Encke's method + * \param qValue Value of free parameter in Encke's function (typically denoted as q) + * \return value of Encke's function for given free parameter. + */ +template< typename StateScalarType = double > +StateScalarType calculateEnckeQFunction( const StateScalarType qValue ) +{ + StateScalarType powerTerm = mathematical_constants::getFloatingInteger< StateScalarType >( 1 ) + + mathematical_constants::getFloatingInteger< StateScalarType >( 2 ) * qValue; + return mathematical_constants::getFloatingInteger< StateScalarType >( 1 ) - 1.0 / ( powerTerm * std::sqrt( powerTerm ) ); +} + +//! Function to remove the central gravity acceleration from an AccelerationMap +/*! + * Function to remove the central gravity acceleration from an AccelerationMap. This is crucial for propagation methods in + * which the deviation from a reference Kepler orbit is propagated. If the central gravity is a spherical harmonic + * acceleration, the point mass term is removed by setting the C(0,0) coefficnet to 0 + * \param bodiesToIntegrate List of names of bodies that are to be integrated numerically. + * \param centralBodies List of names of bodies of which the central terms are to be removed + * (per entry of bodiesToIntegrate) + * \param accelerationModelsPerBody A map containing the list of accelerations acting on each + * body, identifying the body being acted on and the body acted on by an acceleration. The map + * has as key a string denoting the name of the body the list of accelerations, provided as the + * value corresponding to a key, is acting on. This map-value is again a map with string as + * key, denoting the body exerting the acceleration, and as value a pointer to an acceleration + * model. + * \return Functions returning the gravitational parameters of the central terms that were removed. + */ +std::vector< boost::function< double( ) > > removeCentralGravityAccelerations( + const std::vector< std::string >& centralBodies, const std::vector< std::string >& bodiesToIntegrate, + basic_astrodynamics::AccelerationMap& accelerationModelsPerBody ); + +//! Class for computing the state derivative of translational motion of N bodies, using an Encke propagator. +/*! + * Class for computing the state derivative of translational motion of N bodies, using an Encke propagator. + * The Encke propagator propagates the Cartesian deviation from an ideal (pre-defined) Keplerian orbit. + * See e.g. Wakker, Astrodynamics II for mathematical details. + */ +template< typename StateScalarType = double, typename TimeType = double > +class NBodyEnckeStateDerivative: public NBodyStateDerivative< StateScalarType, TimeType > +{ +public: + + //! Constructor, computes required reference quantities, and removes central gravity from acceleration list. + /*! + * Constructor, computes required reference quantities, and removes central gravity from acceleration list. For + * a spherical harmonic central gravity, the C(0,0) coefficient is set to zero. + * \param accelerationModelsPerBody A map containing the list of accelerations acting on each + * body, identifying the body being acted on and the body acted on by an acceleration. The map + * has as key a string denoting the name of the body the list of accelerations, provided as the + * value corresponding to a key, is acting on. This map-value is again a map with string as + * key, denoting the body exerting the acceleration, and as value a pointer to an acceleration + * model. + * \param centralBodyData Object responsible for providing the current integration origins from + * the global origins. + * \param bodiesToIntegrate List of names of bodies that are to be integrated numerically. + * \param initialKeplerElements Kepler elements of bodiesToIntegrate, valid at initialTime. + * \param initialTime Time at which the initialKeplerElements provide the orbital state. + */ + NBodyEnckeStateDerivative( const basic_astrodynamics::AccelerationMap& accelerationModelsPerBody, + const boost::shared_ptr< CentralBodyData< StateScalarType, TimeType > > centralBodyData, + const std::vector< std::string >& bodiesToIntegrate, + const std::vector< Eigen::Matrix< StateScalarType, 6, 1 > >& initialKeplerElements, + const TimeType& initialTime ): + NBodyStateDerivative< StateScalarType, TimeType >( + accelerationModelsPerBody, centralBodyData, encke, bodiesToIntegrate ), + initialKeplerElements_( initialKeplerElements ), + initialTime_( initialTime ), + currentKeplerOrbitTime_( TUDAT_NAN ) + { + currentKeplerianOrbitCartesianState_.resize( bodiesToIntegrate.size( ) ); + + + originalAccelerationModelsPerBody_ = this->accelerationModelsPerBody_ ; + + // Remove central gravitational acceleration from list of accelerations that is to be evaluated + centralBodyGravitationalParameters_ = + removeCentralGravityAccelerations( + centralBodyData->getCentralBodies( ), this->bodiesToBeIntegratedNumerically_, + this->accelerationModelsPerBody_ ); + + // Create root-finder for Kepler orbit propagation + rootFinder_ = boost::make_shared< root_finders::NewtonRaphsonCore< StateScalarType > >( + boost::bind( &root_finders::termination_conditions:: + RootAbsoluteToleranceTerminationCondition< StateScalarType >:: + checkTerminationCondition, + boost::make_shared< root_finders::termination_conditions:: + RootAbsoluteToleranceTerminationCondition< StateScalarType > >( + 20.0 * std::numeric_limits< StateScalarType >::epsilon( ), 1000 ), + _1, _2, _3, _4, _5 ) ); + } + + //! Function to clear reference values of Encke state derivative model + /*! + * Function to clear reference values of Encke state derivative model, in addition to those performed in the + * clearTranslationalStateDerivativeModel function. It resets the currentKeplerOrbitTime_ to ensure that + * the reference orbit is recomputed. + */ + void clearDerivedStateDerivativeModel( ) + { + currentKeplerOrbitTime_ = TUDAT_NAN; + } + + //! Calculates the state derivative of the translational motion of the system, using the Encke algorithm + /*! + * Calculates the state derivative the translational motion of the system + * at the given time and state of bodies. The velocity and acceleration of each body w.r.t. their reference Kepler + * orbits are computed by this function, i.e. the derivative of the Encke state. + * \param time Time (TDB seconds since J2000) at which the system is to be updated. + * \param stateOfSystemToBeIntegrated List of 6 * bodiesToBeIntegratedNumerically_.size( ), containing Cartesian + * position/velocity deviations from the reference Kepler orbits of the bodies being integrated. + * The order of the values is defined by the order of bodies in bodiesToBeIntegratedNumerically_ + * \param stateDerivative Current derivative of Encke state (velocity + acceleration w.r.t. reference Kepler orbit) of + * system of bodies integrated numerically (returned by reference). + */ + void calculateSystemStateDerivative( + const TimeType time, + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& stateOfSystemToBeIntegrated, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > > stateDerivative ) + { + stateDerivative.setZero( ); + + // Retrieve Keplerian orbit state for each body. + calculateKeplerTrajectoryCartesianStates( time ); + + // Get Cartesian state derivative for all bodies of Encke state (excluding central gravitational accelerations). + this->sumStateDerivativeContributions( + stateOfSystemToBeIntegrated, stateDerivative ); + + // Initialize Encke algorithm variables. + StateScalarType qValue = 0.0; + StateScalarType qFunction = 0.0; + StateScalarType keplerianRadius = 0.0; + Eigen::Matrix< StateScalarType, 3, 1 > positionPerturbation = Eigen::Matrix< StateScalarType, 3, 1 >::Zero( ); + + // Update state derivative for each body. + for( unsigned int i = 0; i < this->bodiesToBeIntegratedNumerically_.size( ); i++ ) + { + // Get position perturbation. + positionPerturbation = stateOfSystemToBeIntegrated.segment( i * 6, 3 ); + + // Get distance from central body, assuming purely Keplerian orbit. + keplerianRadius = currentKeplerianOrbitCartesianState_[ i ].segment( 0, 3 ).norm( ); + + // Calculate Encke algorithm variables. + qValue = positionPerturbation.dot( currentKeplerianOrbitCartesianState_[ i ].segment( 0, 3 ) + + 0.5 * positionPerturbation ) / ( keplerianRadius * keplerianRadius ); + qFunction = calculateEnckeQFunction( qValue ); + + // Update state derivative with Encke term. + stateDerivative.block( i * 6 + 3, 0, 3, 1 ) += static_cast< StateScalarType >( + centralBodyGravitationalParameters_[ i ]( ) ) / + ( keplerianRadius * keplerianRadius * keplerianRadius ) * ( + ( positionPerturbation + currentKeplerianOrbitCartesianState_[ i ].segment( 0, 3 ) ) * qFunction - + positionPerturbation ); + } + + } + + //! Function to convert the state in the conventional form to the Encke-propagator-specific form. + /*! + * Function to convert the state in the conventional form to the propagator-specific form. For the Encke propagator, + * this transforms the Cartesian state w.r.t. the central body (conventional form) to the Cartesian deviation + * from the Kepler orbit w.r.t. this central body (Encke form). + * \param cartesianSolution State in 'conventional form' + * \param time Current time at which the state is valid, used to computed Kepler orbits + * \return State (outputSolution), converted to the Encke-propagator-specific form + */ + Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > convertFromOutputSolution( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& cartesianSolution, + const TimeType& time ) + { + Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > currentState = cartesianSolution; + + // Calculate Keplerian orbit states in local frames. + calculateKeplerTrajectoryCartesianStates( time ); + + // Subtract frame origin and Keplerian states from inertial state. + for( unsigned int i = 0; i < this->bodiesToBeIntegratedNumerically_.size( ); i++ ) + { + currentState.segment( i * 6, 6 ) -= ( currentKeplerianOrbitCartesianState_[ i ] ); + } + + return currentState; + + } + + //! Function to convert the Encke-propagator-specific form of the state to the conventional form. + /*! + * Function to convert the Encle-propagator-specific form of the state to the conventional form. For the Encke + * propagator, this transforms the Cartesian state w.r.t. the central body (conventional form) to the Cartesian deviation + * from the Kepler orbit w.r.t. this central body (Encke form). + * In contrast to the convertCurrentStateToGlobalRepresentation function, this + * function does not provide the state in the inertial frame, but instead provides it in the + * frame in which it is propagated. + * \param internalSolution State in Encke-propagator-specific form (i.e. form that is used in + * numerical integration)/ + * \param time Current time at which the state is valid + * \param currentCartesianLocalSoluton State (internalSolution, which is Encke-formulation), + * converted to the 'conventional form'. + */ + void convertToOutputSolution( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& internalSolution, const TimeType& time, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > currentCartesianLocalSoluton ) + { + // Calculate Keplerian orbit state around centeal bodies. + calculateKeplerTrajectoryCartesianStates( time ); + + // Add Keplerian state to perturbation from Encke algorithm to get Cartesian state in local frames. + for( unsigned int i = 0; i < this->bodiesToBeIntegratedNumerically_.size( ); i++ ) + { + currentCartesianLocalSoluton.segment( i * 6, 6 ) = currentKeplerianOrbitCartesianState_[ i ] + + internalSolution.block( i * 6, 0, 6, 1 ); + } + } + + basic_astrodynamics::AccelerationMap getFullAccelerationsMap( ) + { + return originalAccelerationModelsPerBody_; + } + +private: + + //! Function to calculate and set the reference Kepler orbit in Cartesian coordinates for given body. + /*! + * Function to calculate and set the reference Kepler orbit in Cartesian coordinates for given body. + * \param time Time at which Kepler orbit is to be computed. + * \param bodyIndex Index in list of bodies for which Kepler orbit is to be computed. + */ + void calculateKeplerTrajectoryCartesianState( + const TimeType time, + const int bodyIndex ) + { + // Propagate Kepler orbit to current time and set. + currentKeplerianOrbitCartesianState_[ bodyIndex ] = + orbital_element_conversions::convertKeplerianToCartesianElements< StateScalarType >( + orbital_element_conversions::propagateKeplerOrbit< StateScalarType >( + initialKeplerElements_.at( bodyIndex ), static_cast< StateScalarType >( time - initialTime_ ), + static_cast< StateScalarType >( centralBodyGravitationalParameters_.at( bodyIndex )( ) ), + rootFinder_ ), + static_cast< StateScalarType >( centralBodyGravitationalParameters_.at( bodyIndex )( ) ) ); + } + + //! Function to calculate and set the reference Kepler orbit in Cartesian coordinates for all bodies. + /*! + * Function to calculate and set the reference Kepler orbit in Cartesian coordinates for all bodies. + * \param time Time at which Kepler orbits are to be computed. + */ + void calculateKeplerTrajectoryCartesianStates( + const TimeType time ) + { + // Check if update is neede. + if( !( currentKeplerOrbitTime_ == time ) ) + { + // Iterate over bodies and calculate Cartesian state of associated Kepler orbit at current time. + for( unsigned int i = 0; i < this->bodiesToBeIntegratedNumerically_.size( ); i++ ) + { + calculateKeplerTrajectoryCartesianState( time, i ); + } + currentKeplerOrbitTime_ = time; + } + } + + //! Gravitational parameters of central bodies used to convert Cartesian to Keplerian orbits, and vice versa + std::vector< boost::function< double( ) > > centralBodyGravitationalParameters_; + + //! Kepler elements of bodiesToIntegrate, valid at initialTime_. + std::vector< Eigen::Matrix< StateScalarType, 6, 1 > > initialKeplerElements_; + + //! Time at which the initialKeplerElements provide the reference Keper orbit. + TimeType initialTime_ ; + + //! Central body accelerations for each propagated body, which has been removed from accelerationModelsPerBody_/ + std::vector< boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > > + centralAccelerations_; + + //! Root finder used to propagate Kepler orbit. + boost::shared_ptr< root_finders::RootFinderCore< StateScalarType > > rootFinder_; + + //! Current Cartesian states of reference Kepler orbits, valid at currentKeplerOrbitTime_, computed by + //! calculateKeplerTrajectoryCartesianStates + std::vector< Eigen::Matrix< StateScalarType, 6, 1 > > currentKeplerianOrbitCartesianState_; + + //! Time at which the currentKeplerianOrbitCartesianState_ provide the Cartesian representation of the + //! referfence Kepler state. + TimeType currentKeplerOrbitTime_; + + basic_astrodynamics::AccelerationMap originalAccelerationModelsPerBody_; + + +}; + + +} // namespace propagators + +} // namespace tudat + +#endif // TUDAT_NBODYENCKESTATEDERIVATIVE_H diff --git a/Tudat/Astrodynamics/Propagators/setNumericallyIntegratedStates.cpp b/Tudat/Astrodynamics/Propagators/nBodyStateDerivative.cpp similarity index 73% rename from Tudat/Astrodynamics/Propagators/setNumericallyIntegratedStates.cpp rename to Tudat/Astrodynamics/Propagators/nBodyStateDerivative.cpp index 095f2fcc2a..8b73d5a90d 100644 --- a/Tudat/Astrodynamics/Propagators/setNumericallyIntegratedStates.cpp +++ b/Tudat/Astrodynamics/Propagators/nBodyStateDerivative.cpp @@ -1,15 +1,4 @@ -/* Copyright (c) 2010-2016, Delft University of Technology - * All rigths reserved - * - * This file is part of the Tudat. Redistribution and use in source and - * binary forms, with or without modification, are permitted exclusively - * under the terms of the Modified BSD license. You should have received - * a copy of the license with this file. If not, please or visit: - * http://tudat.tudelft.nl/LICENSE. - */ - -#include "Tudat/Astrodynamics/Propagators/setNumericallyIntegratedStates.h" -#include "Tudat/Mathematics/Interpolators/lagrangeInterpolator.h" +#include "Tudat/Astrodynamics/Propagators/nBodyStateDerivative.h" namespace tudat { @@ -17,26 +6,6 @@ namespace tudat namespace propagators { - -//! Function to create an interpolator for the new translational state of a body. -template< > -boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::Matrix< double, 6, 1 > > > -createStateInterpolator( const std::map< double, Eigen::Matrix< double, 6, 1 > >& stateMap ) -{ - return boost::make_shared< - interpolators::LagrangeInterpolator< double, Eigen::Matrix< double, 6, 1 > > >( stateMap, 6 ); -} - -//! Function to create an interpolator for the new translational state of a body. -template< > -boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::Matrix< long double, 6, 1 > > > -createStateInterpolator( const std::map< double, Eigen::Matrix< long double, 6, 1 > >& stateMap ) -{ - return boost::make_shared< - interpolators::LagrangeInterpolator< double, - Eigen::Matrix< long double, 6, 1 > > >( stateMap, 6 ); -} - //! Function to determine in which order the ephemerides are to be updated std::vector< std::string > determineEphemerisUpdateorder( std::vector< std::string > integratedBodies, std::vector< std::string > centralBodies, @@ -133,6 +102,6 @@ std::vector< std::string > determineEphemerisUpdateorder( std::vector< std::stri return updateOrder; } -} // namespace propagators +} -} // namespace tudat +} diff --git a/Tudat/Astrodynamics/Propagators/nBodyStateDerivative.h b/Tudat/Astrodynamics/Propagators/nBodyStateDerivative.h index 11db5dc3c3..9c95f4f51c 100644 --- a/Tudat/Astrodynamics/Propagators/nBodyStateDerivative.h +++ b/Tudat/Astrodynamics/Propagators/nBodyStateDerivative.h @@ -23,7 +23,6 @@ #include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h" #include "Tudat/Astrodynamics/Propagators/centralBodyData.h" #include "Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h" -#include "Tudat/Astrodynamics/Propagators/propagationSettings.h" namespace tudat @@ -32,6 +31,27 @@ namespace tudat namespace propagators { +//! Enum listing propagator types for translational dynamics that can be used. +enum TranslationalPropagatorType +{ + cowell = 0, + encke = 1 +}; + + +//! Function to determine in which order the ephemerides are to be updated +/*! + * Function to determine in which order the ephemerides are to be updated. The order depends on the + * dependencies between the ephemeris/integration origins. + * \param integratedBodies List of bodies that are numerically integrated. + * \param centralBodies List of origins w.r.t. the integratedBodies' translational dynamics is propagated. + * \param ephemerisOrigins Origin of the Ephemeris objects of the integratedBodies. + * \return + */ +std::vector< std::string > determineEphemerisUpdateorder( std::vector< std::string > integratedBodies, + std::vector< std::string > centralBodies, + std::vector< std::string > ephemerisOrigins ); + //! State derivative for the translational dynamics of N bodies /*! * This class calculates the trabnslational state derivative of any @@ -75,53 +95,97 @@ class NBodyStateDerivative: public propagators::SingleStateTypeDerivative< State centralBodyData_( centralBodyData ), propagatorType_( propagatorType ), bodiesToBeIntegratedNumerically_( bodiesToIntegrate ) - { } + { + // Add empty acceleration map if body is to be propagated with no accelerations. + for( unsigned int i = 0; i < bodiesToBeIntegratedNumerically_.size( ); i++ ) + { + if( accelerationModelsPerBody_.count( bodiesToBeIntegratedNumerically_.at( i ) ) == 0 ) + { + accelerationModelsPerBody_[ bodiesToBeIntegratedNumerically_.at( i ) ] = + basic_astrodynamics::SingleBodyAccelerationMap( ); + } + } + + // Correct order of propagated bodies. + for( outerAccelerationIterator = accelerationModelsPerBody_.begin( ); + outerAccelerationIterator != accelerationModelsPerBody_.end( ); + outerAccelerationIterator++ ) + { + std::vector< std::string >::iterator findIterator = + std::find( bodiesToBeIntegratedNumerically_.begin( ), bodiesToBeIntegratedNumerically_.end( ), outerAccelerationIterator->first ); + bodyOrder_.push_back( std::distance( bodiesToBeIntegratedNumerically_.begin( ), findIterator ) ); + } + + } //! Destructor virtual ~NBodyStateDerivative( ){ } - //! Function to update the state derivative model to the current time. + //! Function to clear any reference/cached values of state derivative model /*! - * Function to update the state derivative model (i.e. acceleration, torque, etc. models) to the - * current time. Note that this function only updates the state derivative model itself, the - * environment models must be updated before calling this function. - * \param currentTime Time at which state derivative is to be calculated + * Function to clear any reference/cached values of state derivative model, in addition to those performed in the + * clearTranslationalStateDerivativeModel function. Default implementation is empty. */ - void updateStateDerivativeModel( const TimeType currentTime ) + virtual void clearDerivedTranslationalStateDerivativeModel( ){ } + + //! Function to clear reference/cached values of acceleration models + /*! + * Function to clear reference/cached values of acceleration models, to ensure that they are all recalculated. + */ + void clearTranslationalStateDerivativeModel( ) { - // Reser all acceleration times (to allow multiple evaluations at same time, e.g. stage 2 - // and 3 in RK4 integrator) - for( accelerationMapIterator = accelerationModelsPerBody_.begin( ); - accelerationMapIterator != accelerationModelsPerBody_.end( ); - accelerationMapIterator++ ) + // Reset all acceleration times (to allow multiple evaluations at same time, e.g. stage 2 and 3 in RK4 integrator) + for( outerAccelerationIterator = accelerationModelsPerBody_.begin( ); + outerAccelerationIterator != accelerationModelsPerBody_.end( ); outerAccelerationIterator++ ) { // Iterate over all accelerations acting on body - for( innerAccelerationIterator = accelerationMapIterator->second.begin( ); - innerAccelerationIterator != accelerationMapIterator->second.end( ); - innerAccelerationIterator++ ) + for( innerAccelerationIterator = outerAccelerationIterator->second.begin( ); + innerAccelerationIterator != outerAccelerationIterator->second.end( ); innerAccelerationIterator++ ) { - // Update accelerations + // Update accelerationsj for( unsigned int j = 0; j < innerAccelerationIterator->second.size( ); j++ ) { - innerAccelerationIterator->second[ j ]->resetTime( TUDAT_NAN ); } } } + } + + //! Function to clear reference/cached values of translational state derivative model + /*! + * Function to clear reference/cached values of translational state derivative model. For each derived class, this + * entails resetting the current time in the acceleration models to NaN (see clearTranslationalStateDerivativeModel). + * Every derived class requiring additional values to be cleared should implement the + * clearDerivedTranslationalStateDerivativeModel function. + */ + void clearStateDerivativeModel( ) + { + clearTranslationalStateDerivativeModel( ); + clearDerivedTranslationalStateDerivativeModel( ); + } + + //! Function to update the state derivative model to the current time. + /*! + * Function to update the state derivative model (i.e. acceleration, torque, etc. models) to the + * current time. Note that this function only updates the state derivative model itself, the + * environment models must be updated before calling this function. + * \param currentTime Time at which state derivative is to be calculated + */ + void updateStateDerivativeModel( const TimeType currentTime ) + { // Iterate over all accelerations and update their internal state. - for( accelerationMapIterator = accelerationModelsPerBody_.begin( ); - accelerationMapIterator != accelerationModelsPerBody_.end( ); accelerationMapIterator++ ) + for( outerAccelerationIterator = accelerationModelsPerBody_.begin( ); + outerAccelerationIterator != accelerationModelsPerBody_.end( ); outerAccelerationIterator++ ) { // Iterate over all accelerations acting on body - for( innerAccelerationIterator = accelerationMapIterator->second.begin( ); - innerAccelerationIterator != accelerationMapIterator->second.end( ); + for( innerAccelerationIterator = outerAccelerationIterator->second.begin( ); + innerAccelerationIterator != outerAccelerationIterator->second.end( ); innerAccelerationIterator++ ) { // Update accelerations for( unsigned int j = 0; j < innerAccelerationIterator->second.size( ); j++ ) { - innerAccelerationIterator->second[ j ]->updateMembers( currentTime ); } } @@ -137,23 +201,22 @@ class NBodyStateDerivative: public propagators::SingleStateTypeDerivative< State * \param internalSolution State in propagator-specific form (i.e. form that is used in * numerical integration). * \param time Current time at which the state is valid. - * \return State (internalSolution), converted to the Cartesian state in inertial coordinates. + * \param currentCartesianLocalSoluton State (internalSolution), converted to the Cartesian state in inertial coordinates + * (returned by reference). */ - Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > convertCurrentStateToGlobalRepresentation( - const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& internalSolution, - const TimeType& time ) + void convertCurrentStateToGlobalRepresentation( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& internalSolution, const TimeType& time, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > currentCartesianLocalSoluton ) { - Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > cartesianLocalSolution - = this->convertToOutputSolution( internalSolution, time ); + this->convertToOutputSolution( internalSolution, time, currentCartesianLocalSoluton ); + + centralBodyData_->getReferenceFrameOriginInertialStates( + currentCartesianLocalSoluton, time, centralBodyInertialStates_, true ); - std::vector< Eigen::Matrix< StateScalarType, 6, 1 > > centralBodyInertialStates - = centralBodyData_->getReferenceFrameOriginInertialStates( cartesianLocalSolution, - time, true ); - for( unsigned int i = 0; i < centralBodyInertialStates.size( ); i++ ) + for( unsigned int i = 0; i < centralBodyInertialStates_.size( ); i++ ) { - cartesianLocalSolution.segment( i * 6, 6 ) += centralBodyInertialStates[ i ]; + currentCartesianLocalSoluton.segment( i * 6, 6 ) += centralBodyInertialStates_[ i ]; } - return cartesianLocalSolution; } //! Function to get list of names of bodies that are to be integrated numerically. @@ -171,12 +234,12 @@ class NBodyStateDerivative: public propagators::SingleStateTypeDerivative< State * Function to get map containing the list of accelerations acting on each body, * \return A map containing the list of accelerations acting on each body, */ - basic_astrodynamics::AccelerationMap getAccelerationsMap( ) + virtual basic_astrodynamics::AccelerationMap getFullAccelerationsMap( ) { return accelerationModelsPerBody_; } - //! Function to get object providing the current integration origins + //! Function to get object providing the current integration origins /*! * Function to get object responsible for providing the current integration origins from the * global origins. @@ -207,6 +270,57 @@ class NBodyStateDerivative: public propagators::SingleStateTypeDerivative< State return 6 * bodiesToBeIntegratedNumerically_.size( ); } + //! Function to retrieve the total acceleration acting on a given body. + /*! + * Function to retrieve the total acceleration acting on a given body. The environment + * and acceleration models must have been updated to the current state before calling this + * function. NOTE: This function is typically used to retrieve the acceleration for output purposes, not to compute the + * translational state derivative. + * \param bodyName Name of body for which accelerations are to be retrieved. + * \return + */ + Eigen::Vector3d getTotalAccelerationForBody( + const std::string& bodyName ) + { + // Check if body is propagated. + Eigen::Vector3d totalAcceleration = Eigen::Vector3d::Zero( ); + if( std::find( bodiesToBeIntegratedNumerically_.begin( ), + bodiesToBeIntegratedNumerically_.end( ), + bodyName ) == bodiesToBeIntegratedNumerically_.end( ) ) + { + std::string errorMessage = "Error when getting total acceleration for body " + bodyName + + ", no such acceleration is found"; + throw std::runtime_error( errorMessage ); + } + else + { + if( accelerationModelsPerBody_.count( bodyName ) != 0 ) + { + basic_astrodynamics::SingleBodyAccelerationMap accelerationsOnBody = + accelerationModelsPerBody_.at( bodyName ); + + // Iterate over all accelerations acting on body + for( innerAccelerationIterator = accelerationsOnBody.begin( ); + innerAccelerationIterator != accelerationsOnBody.end( ); + innerAccelerationIterator++ ) + { + for( unsigned int j = 0; j < innerAccelerationIterator->second.size( ); j++ ) + { + // Calculate acceleration and add to state derivative. + totalAcceleration += innerAccelerationIterator->second[ j ]->getAcceleration( ); + } + } + } + } + return totalAcceleration; + } + + basic_astrodynamics::AccelerationMap getAccelerationsMap( ) + { + return accelerationModelsPerBody_; + + } + protected: //! Function to get the state derivative of the system in Cartesian coordinates. @@ -215,46 +329,45 @@ class NBodyStateDerivative: public propagators::SingleStateTypeDerivative< State * and acceleration models must have been updated to the current state before calling this * function. * \param stateOfSystemToBeIntegrated Current Cartesian state of the system. - * \param time Time at which the state derivative is to be computed - * \return State derivative of the system in Cartesian coordinates. + * \param stateDerivative State derivative of the system in Cartesian coordinates (returned by reference). */ - Eigen::VectorXd sumStateDerivativeContributions( - const Eigen::VectorXd& stateOfSystemToBeIntegrated, const TimeType time ) + void sumStateDerivativeContributions( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& stateOfSystemToBeIntegrated, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > > stateDerivative ) { using namespace basic_astrodynamics; - // Declare and initialize to zero vector to be returned. - Eigen::VectorXd stateDerivative = Eigen::VectorXd::Zero( stateOfSystemToBeIntegrated.size( ) ); + stateDerivative.setZero( ); + + int currentBodyIndex = 0; + int currentAccelerationIndex = 0; // Iterate over all bodies with accelerations. - for( unsigned int i = 0; i < bodiesToBeIntegratedNumerically_.size( ); i++ ) + for( outerAccelerationIterator = accelerationModelsPerBody_.begin( ); + outerAccelerationIterator != accelerationModelsPerBody_.end( ); + outerAccelerationIterator++ ) { - // If body undergoes acceleration, calculate and add accelerations. - if( accelerationModelsPerBody_.count( bodiesToBeIntegratedNumerically_[ i ] ) != 0 ) - { + currentBodyIndex = bodyOrder_[ currentAccelerationIndex ]; + // Iterate over all accelerations acting on body + for( innerAccelerationIterator = outerAccelerationIterator->second.begin( ); + innerAccelerationIterator != outerAccelerationIterator->second.end( ); + innerAccelerationIterator++ ) + { + for( unsigned int j = 0; j < innerAccelerationIterator->second.size( ); j++ ) { - // Iterate over all accelerations acting on body - for( innerAccelerationIterator = - accelerationModelsPerBody_[ bodiesToBeIntegratedNumerically_[ i ] ].begin( ); - innerAccelerationIterator != - accelerationModelsPerBody_[ bodiesToBeIntegratedNumerically_[ i ] ].end( ); - innerAccelerationIterator++ ) - { - for( unsigned int j = 0; j < innerAccelerationIterator->second.size( ); j++ ) - { - // Calculate acceleration and add to state derivative. - stateDerivative.segment( i * 6 + 3, 3 ) += ( - innerAccelerationIterator->second[ j ]->getAcceleration( ) ); - } - } + // Calculate acceleration and add to state derivative. + stateDerivative.block( currentBodyIndex * 6 + 3, 0, 3, 1 ) += ( + innerAccelerationIterator->second[ j ]->getAcceleration( ) ). + template cast< StateScalarType >( ); } } + // Add body velocity as derivative of its position. - stateDerivative.segment( i * 6, 3 ) = stateOfSystemToBeIntegrated.segment( i * 6 + 3, 3 ); + stateDerivative.block( currentBodyIndex * 6, 0, 3, 1 ) = + ( stateOfSystemToBeIntegrated.segment( currentBodyIndex * 6 + 3, 3 ) ); + currentAccelerationIndex++; } - - return stateDerivative; } @@ -277,17 +390,23 @@ class NBodyStateDerivative: public propagators::SingleStateTypeDerivative< State //! List of names of bodies that are to be integrated numerically. std::vector< std::string > bodiesToBeIntegratedNumerically_; + std::vector< int > bodyOrder_; + //! Predefined iterator to save (de-)allocation time. - basic_astrodynamics::AccelerationMap::iterator accelerationMapIterator; + std::unordered_map< std::string, std::vector< + boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > > >::iterator innerAccelerationIterator; //! Predefined iterator to save (de-)allocation time. - std::map< std::string, std::vector< boost::shared_ptr< - basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > > >::iterator - innerAccelerationIterator; + std::unordered_map< std::string, std::unordered_map< std::string, std::vector< + boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > > > >::iterator outerAccelerationIterator; + //! List of states of teh central bodies of the propagated bodies. + std::vector< Eigen::Matrix< StateScalarType, 6, 1 > > centralBodyInertialStates_; }; } // namespace propagators + } // namespace tudat + #endif // TUDAT_NBODYSTATEDERIVATIVE_H diff --git a/Tudat/Astrodynamics/Propagators/propagationSettings.h b/Tudat/Astrodynamics/Propagators/propagationSettings.h deleted file mode 100644 index 5d5ef9a558..0000000000 --- a/Tudat/Astrodynamics/Propagators/propagationSettings.h +++ /dev/null @@ -1,213 +0,0 @@ -/* Copyright (c) 2010-2016, Delft University of Technology - * All rigths reserved - * - * This file is part of the Tudat. Redistribution and use in source and - * binary forms, with or without modification, are permitted exclusively - * under the terms of the Modified BSD license. You should have received - * a copy of the license with this file. If not, please or visit: - * http://tudat.tudelft.nl/LICENSE. - */ - -#ifndef TUDAT_PROPAGATIONSETTINGS_H -#define TUDAT_PROPAGATIONSETTINGS_H - -#include -#include -#include -#include - -#include - -#include - -#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" -#include "Tudat/Astrodynamics/BasicAstrodynamics/timeConversions.h" - -namespace tudat -{ - -namespace propagators -{ - - -//! Enum listing types of dynamics that can be numerically integrated -enum IntegratedStateType -{ - transational_state -}; - -//! Enum listing propagator types for translational dynamics that can be used. -enum TranslationalPropagatorType -{ - cowell = 0 -}; - -//! Base class for defining setting of a propagator -/*! - * Base class for defining setting of a propagator. This class is non-functional, and each state - * type requires its own derived class (which may have multiple derived classes of its own). - */ -template< typename StateScalarType > -class PropagatorSettings -{ -public: - - //! Constructor - /*! - * Constructor - * \param stateType Type of state being propagated - * \param initialBodyStates Initial state used as input for numerical integration - */ - PropagatorSettings( const IntegratedStateType stateType, - const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > initialBodyStates ): - stateType_( stateType ), initialStates_( initialBodyStates ){ } - - //! Virtual destructor. - virtual ~PropagatorSettings( ){ } - - //!T ype of state being propagated - IntegratedStateType stateType_; - - //! Function to retrieve the initial state used as input for numerical integration - /*! - * Function to retrieve the initial state used as input for numerical integration - * \return Initial state used as input for numerical integration - */ - Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > getInitialStates( ) - { - return initialStates_; - } - - //! Function to reset the initial state used as input for numerical integration - /*! - * Function to reset the initial state used as input for numerical integration - * \param initialBodyStates New initial state used as input for numerical integration - */ - virtual void resetInitialStates( const Eigen::Matrix< StateScalarType, - Eigen::Dynamic, 1 >& initialBodyStates ) - { - initialStates_ = initialBodyStates; - } - -protected: - - //! Initial state used as input for numerical integration - Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > initialStates_; - -}; - -//! Class for defining settings for propagating translational dynamics. -/*! - * Class for defining settings for propagating translational dynamics. The propagator defines the - * form of the equations of motion (i.e. Cowell, Encke, Gauss etc.). This base class can be used - * for Cowell propagator. Other propagators have dedicated derived class. - */ -template< typename StateScalarType = double > -class TranslationalStatePropagatorSettings: public PropagatorSettings< StateScalarType > -{ -public: - - //! Constructor of translational state propagator settings - /*! - * Constructor creating translational state propagator settings object - * \param centralBodies List of bodies w.r.t. which the bodies in bodiesToIntegrate_ are - * propagated. - * \param accelerationsMap A map containing the list of accelerations acting on each body, - * identifying the body being acted on and the body acted on by an acceleration. The map has as - * key a string denoting the name of the body the list of accelerations, provided as the value - * corresponding to a key, is acting on. This map-value is again a map with string as key, - * denoting the body exerting the acceleration, and as value a pointer to an acceleration model. - * \param bodiesToIntegrate List of bodies for which the translational state is to be propagated. - * \param initialBodyStates Initial state used as input for numerical integration - * \param propagator Type of translational state propagator to be used - */ - TranslationalStatePropagatorSettings( const std::vector< std::string >& centralBodies, - const basic_astrodynamics::AccelerationMap& accelerationsMap, - const std::vector< std::string >& bodiesToIntegrate, - const Eigen::Matrix< StateScalarType, - Eigen::Dynamic, 1 >& initialBodyStates, - const TranslationalPropagatorType propagator = cowell): - PropagatorSettings< StateScalarType >( transational_state, initialBodyStates ), - centralBodies_( centralBodies ), accelerationsMap_( accelerationsMap ), - bodiesToIntegrate_( bodiesToIntegrate ), propagator_( propagator ){ } - - //! Virtual destructor - /*! - * Virtual destructor - */ - ~TranslationalStatePropagatorSettings( ){ } - - //! List of bodies w.r.t. which the bodies in bodiesToIntegrate_ are propagated. - std::vector< std::string > centralBodies_; - - //! A map containing the list of accelerations acting on each body - /*! - * A map containing the list of accelerations acting on each body, identifying the body being - * acted on and the body acted on by an acceleration. The map has as key a string denoting the - * name of the body the list of accelerations, provided as the value corresponding to a key, is - * acting on. This map-value is again a map with string as key, denoting the body exerting the - * acceleration, and as value a pointer to an acceleration model. - */ - basic_astrodynamics::AccelerationMap accelerationsMap_; - - //! List of bodies for which the translational state is to be propagated. - std::vector< std::string > bodiesToIntegrate_; - - //! Type of translational state propagator to be used - TranslationalPropagatorType propagator_; - -}; - -template< typename StateScalarType > -//! Function to retrieve the list of integrated state types and reference ids -/*! - * Function to retrieve the list of integrated state types and reference ids. For translational and - * rotational dynamics, the id refers only to the body being propagated (and the second entry of the - * pair is empty: ""). For proper time propagation, a body and a reference point may be provided, - * resulting in non-empty first and second pair entries. - * \param propagatorSettings Settings that are to be used for the propagation. - * \return List of integrated state types and reference ids - */ -std::map< IntegratedStateType, std::vector< std::pair< std::string, std::string > > > getIntegratedTypeAndBodyList( - const boost::shared_ptr< PropagatorSettings< StateScalarType > > propagatorSettings ) -{ - std::map< IntegratedStateType, std::vector< std::pair< std::string, std::string > > > integratedStateList; - - // Identify propagator type - switch( propagatorSettings->stateType_ ) - { - case transational_state: - { - - boost::shared_ptr< TranslationalStatePropagatorSettings< StateScalarType > > - translationalPropagatorSettings = boost::dynamic_pointer_cast< - TranslationalStatePropagatorSettings< StateScalarType > >( propagatorSettings ); - if( translationalPropagatorSettings == NULL ) - { - throw std::runtime_error( "Error getting integrated state type list, translational state input inconsistent" ); - } - - // Retrieve list of integrated bodies in correct formatting. - std::vector< std::pair< std::string, std::string > > integratedBodies; - for( unsigned int i = 0; i < translationalPropagatorSettings->bodiesToIntegrate_.size( ); i++ ) - { - integratedBodies.push_back( std::make_pair( - translationalPropagatorSettings->bodiesToIntegrate_.at( i ), "" ) ); - } - integratedStateList[ transational_state ] = integratedBodies; - - break; - } - default: - throw std::runtime_error( "Error, could not process integrated state type " + - boost::lexical_cast< std::string >( propagatorSettings->stateType_ ) ); - } - - return integratedStateList; -} - -} // namespace propagators - -} // namespace tudat - -#endif // TUDAT_PROPAGATIONSETTINGS_H diff --git a/Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.cpp b/Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.cpp new file mode 100644 index 0000000000..d2e9fef12b --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.cpp @@ -0,0 +1,53 @@ +#include + +#include "Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h" + +namespace tudat +{ + +namespace propagators +{ + +//! Get size of state for single propagated state of given type. +int getSingleIntegrationSize( const IntegratedStateType stateType ) +{ + int singleStateSize = 0; + switch( stateType ) + { + case transational_state: + singleStateSize = 6; + break; + case body_mass_state: + singleStateSize = 1; + break; + default: + std::string errorMessage = + "Did not recognize state type " + boost::lexical_cast< std::string >( stateType ) + "when getting size"; + throw std::runtime_error( errorMessage ); + } + return singleStateSize; +} + +//! Get order of differential equation for governing equations of dynamics of given type. +int getSingleIntegrationDifferentialEquationOrder( const IntegratedStateType stateType ) +{ + int singleStateSize = 0; + switch( stateType ) + { + case transational_state: + singleStateSize = 2; + break; + case body_mass_state: + singleStateSize = 1; + break; + default: + std::string errorMessage = + "Did not recognize state type " + boost::lexical_cast< std::string >( stateType ) + "when getting order"; + throw std::runtime_error( errorMessage ); + } + return singleStateSize; +} + +} + +} diff --git a/Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h b/Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h index 125809348a..ec1a7c3398 100644 --- a/Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h +++ b/Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h @@ -13,14 +13,40 @@ #include -#include "Tudat/Astrodynamics/Propagators/propagationSettings.h" - namespace tudat { namespace propagators { + + +//! Enum listing types of dynamics that can be numerically integrated +enum IntegratedStateType +{ + hybrid = 0, + transational_state = 1, + body_mass_state = 2 +}; + + +//! Get size of state for single propagated state of given type. +/*! + * Get size of state for single propagated state of given type (i.e. 6 for translational state). + * \param stateType Type of state + * \return Size of single state. + */ +int getSingleIntegrationSize( const IntegratedStateType stateType ); + +//! Get order of differential equation for governing equations of dynamics of given type. +/*! + * Get order of differential equation for governing equations of dynamics of given type (i.e. 2 for translational state). + * \param stateType Type of state + * \return Order of differential equations. + */ +int getSingleIntegrationDifferentialEquationOrder( const IntegratedStateType stateType ); + + //! Base class for calculating the state derivative model for a single type of dynamics. /*! * Base class for calculating the state derivative model for a single @@ -53,14 +79,22 @@ class SingleStateTypeDerivative * updated before calling this function. It returns the state derivative in teh form required * for the specific type of propagator used (defined by derived class). * \param time Time at which the state derivative is to be calculated. - * \param stateOfSystemToBeIntegrated Current state of the system, in the form that the - * equations are propagated (i.e. directly from numerical integrator) - * \return Derivative of the state of the system, in the form that the equations are propagated - * (i.e. to be piped directly to numerical integrator) + * \param stateOfSystemToBeIntegrated Current state of the system, in the form that the equations are propagated (i.e. + * directly from numerical integrator) + * \param stateDerivative Derivative of the state of the system, in the form that the equations are propagated + * (i.e. to be piped directly to numerical integrator), returned by reference. */ - virtual Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > calculateSystemStateDerivative( + virtual void calculateSystemStateDerivative( const TimeType time, - const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& stateOfSystemToBeIntegrated ) = 0; + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& stateOfSystemToBeIntegrated, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > > stateDerivative ) = 0; + + //! Function to clear reference/cached values of state derivative model + /*! + * Function to clear reference/cached values of state derivative model, such as the current time and/or state. + * This function is to be implemented in each derived class + */ + virtual void clearStateDerivativeModel( ) = 0; //! Function to update the state derivative model to the current time. /*! @@ -82,11 +116,12 @@ class SingleStateTypeDerivative * \param internalSolution State in propagator-specific form (i.e. form that is used in * numerical integration). * \param time Current time at which the state is valid. - * \return State (internalSolution), converted to the 'conventional form' in inertial - * coordinates, that can for instance be set directly in the body object. + * \param currentCartesianLocalSoluton State (internalSolution), converted to the 'conventional form' in inertial + * coordinates, that can for instance be set directly in the body object (returned by reference). */ - virtual Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > convertCurrentStateToGlobalRepresentation( - const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& internalSolution, const TimeType& time ) = 0; + virtual void convertCurrentStateToGlobalRepresentation( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& internalSolution, const TimeType& time, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > currentCartesianLocalSoluton ) = 0; //! Function to convert the state in the conventional form to the propagator-specific form. @@ -113,10 +148,12 @@ class SingleStateTypeDerivative * \param internalSolution State in propagator-specific form (i.e. form that is used in * numerical integration). * \param time Current time at which the state is valid. - * \return State (internalSolution), converted to the 'conventional form' + * \param currentCartesianLocalSoluton State (internalSolution), converted to the 'conventional form' (returned by + * reference). */ - virtual Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > convertToOutputSolution( - const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& internalSolution, const TimeType& time ) = 0; + virtual void convertToOutputSolution( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& internalSolution, const TimeType& time, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > currentCartesianLocalSoluton ) = 0; //! Function to return the size of the state handled by the object /*! diff --git a/Tudat/Astrodynamics/Propagators/stateTransitionMatrixInterface.cpp b/Tudat/Astrodynamics/Propagators/stateTransitionMatrixInterface.cpp new file mode 100644 index 0000000000..641dad5471 --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/stateTransitionMatrixInterface.cpp @@ -0,0 +1,42 @@ +#include + +#include "Tudat/Astrodynamics/Propagators/stateTransitionMatrixInterface.h" + +namespace tudat +{ + +namespace propagators +{ + +//! Function to reset the state transition and sensitivity matrix interpolators +void SingleArcCombinedStateTransitionAndSensitivityMatrixInterface::updateMatrixInterpolators( + const boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > stateTransitionMatrixInterpolator, + const boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > sensitivityMatrixInterpolator ) +{ + stateTransitionMatrixInterpolator_ = stateTransitionMatrixInterpolator; + sensitivityMatrixInterpolator_ = sensitivityMatrixInterpolator; +} + +//! Function to get the concatenated state transition and sensitivity matrix at a given time. +Eigen::MatrixXd SingleArcCombinedStateTransitionAndSensitivityMatrixInterface::getCombinedStateTransitionAndSensitivityMatrix( + const double evaluationTime ) +{ + combinedStateTransitionMatrix_.setZero( ); + + // Set Phi and S matrices. + combinedStateTransitionMatrix_.block( 0, 0, stateTransitionMatrixSize_, stateTransitionMatrixSize_ ) = + stateTransitionMatrixInterpolator_->interpolate( evaluationTime ); + + if( sensitivityMatrixSize_ > 0 ) + { + combinedStateTransitionMatrix_.block( 0, stateTransitionMatrixSize_, stateTransitionMatrixSize_, sensitivityMatrixSize_ ) = + sensitivityMatrixInterpolator_->interpolate( evaluationTime ); + } + + return combinedStateTransitionMatrix_; +} + +} + +} + diff --git a/Tudat/Astrodynamics/Propagators/stateTransitionMatrixInterface.h b/Tudat/Astrodynamics/Propagators/stateTransitionMatrixInterface.h new file mode 100644 index 0000000000..c717d60a40 --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/stateTransitionMatrixInterface.h @@ -0,0 +1,217 @@ +#ifndef TUDAT_STATETRANSITIONMATRIXINTERFACE_H +#define TUDAT_STATETRANSITIONMATRIXINTERFACE_H + +#include + +#include + +#include + +#include "Tudat/Mathematics/Interpolators/oneDimensionalInterpolator.h" + +namespace tudat +{ + +namespace propagators +{ + +//! Base class for interface object of interpolation of numerically propagated state transition and sensitivity matrices. +/*! + * Base class for interface object of interpolation of numerically propagated state transition and sensitivity matrices. + * Derived classes implement the case of single-arc/multi-arc/combined dynamics. + */ +class CombinedStateTransitionAndSensitivityMatrixInterface +{ +public: + + //! Constructor + /*! + * Constructor + * \param numberOfInitialDynamicalParameters Size of the estimated initial state vector (and size of square + * state transition matrix. + * \param numberOfParameters Total number of estimated parameters (initial states and other parameters). + */ + CombinedStateTransitionAndSensitivityMatrixInterface( + const int numberOfInitialDynamicalParameters, + const int numberOfParameters ) + { + stateTransitionMatrixSize_ = numberOfInitialDynamicalParameters; + sensitivityMatrixSize_ = numberOfParameters - stateTransitionMatrixSize_; + } + + //! Destructor. + virtual ~CombinedStateTransitionAndSensitivityMatrixInterface( ){ } + + //! Function to get the concatenated state transition and sensitivity matrix at a given time. + /*! + * Function to get the concatenated state transition and sensitivity matrix at a given time. + * \param evaluationTime Time at which to evaluate matrix interpolators + * \return Concatenated state transition and sensitivity matrices. + */ + virtual Eigen::MatrixXd getCombinedStateTransitionAndSensitivityMatrix( const double evaluationTime ) = 0; + + //! Function to get the concatenated state transition and sensitivity matrix at a given time, which includes + //! zero values for parameters not active in current arc. + /*! + * Function to get the concatenated state transition and sensitivity matrix at a given time, which includes + * zero values for parameters not active in current arc. + * \param evaluationTime Time at which to evaluate matrix interpolators + * \return Concatenated state transition and sensitivity matrices, including inactive parameters at + * evaluationTime. + */ + virtual Eigen::MatrixXd getFullCombinedStateTransitionAndSensitivityMatrix( const double evaluationTime ) = 0; + + //! Function to get the size of state transition matrix + /*! + * Function to get the size of state transition matrix + * \return Size of state transition matrix + */ + int getStateTransitionMatrixSize( ) + { + return stateTransitionMatrixSize_; + } + + //! Function to get the number of columns of sensitivity matrix. + /*! + * Function to get the number of columns of sensitivity matrix. + * \return Number of columns of sensitivity matrix. + */ + int getSensitivityMatrixSize( ) + { + return sensitivityMatrixSize_; + } + + //! Function to get the size of the total parameter vector. + /*! + * Function to get the size of the total parameter vector (both global and all local parameters). + * \return Size of the total parameter vector. + */ + virtual int getFullParameterVectorSize( ) = 0; + +protected: + + //! Size of state transition matrix + int stateTransitionMatrixSize_; + + //! Number of columns of sensitivity matrix. + int sensitivityMatrixSize_; + +}; + +//! Interface object of interpolation of numerically propagated state transition and sensitivity matrices for single-arc +//! estimation. +class SingleArcCombinedStateTransitionAndSensitivityMatrixInterface: + public CombinedStateTransitionAndSensitivityMatrixInterface +{ +public: + + //! Constructor + /*! + * Constructor + * \param stateTransitionMatrixInterpolator Interpolator returning the state transition matrix as a function of time. + * \param sensitivityMatrixInterpolator Interpolator returning the sensitivity matrix as a function of time. + * \param numberOfInitialDynamicalParameters Size of the estimated initial state vector (and size of square + * state transition matrix. + * \param numberOfParameters Total number of estimated parameters (initial states and other parameters). + */ + SingleArcCombinedStateTransitionAndSensitivityMatrixInterface( + const boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > + stateTransitionMatrixInterpolator, + const boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > + sensitivityMatrixInterpolator, + const int numberOfInitialDynamicalParameters, + const int numberOfParameters ): + CombinedStateTransitionAndSensitivityMatrixInterface( numberOfInitialDynamicalParameters, numberOfParameters ), + stateTransitionMatrixInterpolator_( stateTransitionMatrixInterpolator ), + sensitivityMatrixInterpolator_( sensitivityMatrixInterpolator ) + { + combinedStateTransitionMatrix_ = Eigen::MatrixXd::Zero( + stateTransitionMatrixSize_, stateTransitionMatrixSize_ + sensitivityMatrixSize_ ); + } + + //! Destructor. + ~SingleArcCombinedStateTransitionAndSensitivityMatrixInterface( ){ } + + //! Function to reset the state transition and sensitivity matrix interpolators + /*! + * Function to reset the state transition and sensitivity matrix interpolators + * \param stateTransitionMatrixInterpolator New interpolator returning the state transition matrix as a function of time. + * \param sensitivityMatrixInterpolator New interpolator returning the sensitivity matrix as a function of time. + */ + void updateMatrixInterpolators( + const boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > + stateTransitionMatrixInterpolator, + const boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > + sensitivityMatrixInterpolator ); + + //! Function to get the interpolator returning the state transition matrix as a function of time. + /*! + * \brief Function to get the interpolator returning the state transition matrix as a function of time. + * \return Interpolator returning the state transition matrix as a function of time. + */ + boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > + getStateTransitionMatrixInterpolator( ) + { + return stateTransitionMatrixInterpolator_; + } + + //! Function to get the interpolator returning the sensitivity matrix as a function of time. + /*! + * \brief Function to get the interpolator returning the sensitivity matrix as a function of time. + * \return Interpolator returning the sensitivity matrix as a function of time. + */ + boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > + getSensitivityMatrixInterpolator( ) + { + return sensitivityMatrixInterpolator_; + } + //! Function to get the concatenated state transition and sensitivity matrix at a given time. + /*! + * Function to get the concatenated state transition and sensitivity matrix at a given time. + * \param evaluationTime Time at which to evaluate matrix interpolators + * \return Concatenated state transition and sensitivity matrices. + */ + Eigen::MatrixXd getCombinedStateTransitionAndSensitivityMatrix( const double evaluationTime ); + + //! Function to get the concatenated state transition and sensitivity matrix at a given time. + /*! + * Function to get the concatenated state transition and sensitivity matrix at a given time + * (functionality equal to getCombinedStateTransitionAndSensitivityMatrix for single-arc case). + * \param evaluationTime Time at which to evaluate matrix interpolators + * \return Concatenated state transition and sensitivity matrices. + */ + Eigen::MatrixXd getFullCombinedStateTransitionAndSensitivityMatrix( const double evaluationTime ) + { + return getCombinedStateTransitionAndSensitivityMatrix( evaluationTime ); + } + + //! Function to get the size of the total parameter vector. + /*! + * Function to get the size of the total parameter vector. For single-arc, this is simply the combination of + * the size of the state transition and sensitivity matrices. + * \return Size of the total parameter vector. + */ + int getFullParameterVectorSize( ) + { + return sensitivityMatrixSize_ + stateTransitionMatrixSize_; + } + + +private: + + //! Predefined matrix to use as return value when calling getCombinedStateTransitionAndSensitivityMatrix. + Eigen::MatrixXd combinedStateTransitionMatrix_; + + //! Interpolator returning the state transition matrix as a function of time. + boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > + stateTransitionMatrixInterpolator_; + + //! Interpolator returning the sensitivity matrix as a function of time. + boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > + sensitivityMatrixInterpolator_; +}; + +} // namespace propagators + +} // namespace tudat +#endif // TUDAT_STATETRANSITIONMATRIXINTERFACE_H diff --git a/Tudat/Astrodynamics/Propagators/variationalEquations.cpp b/Tudat/Astrodynamics/Propagators/variationalEquations.cpp new file mode 100644 index 0000000000..af2cc39c73 --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/variationalEquations.cpp @@ -0,0 +1,171 @@ +#include + +#include + +#include + +#include "Tudat/Astrodynamics/Propagators/variationalEquations.h" +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/accelerationPartial.h" + + +namespace tudat +{ + +namespace propagators +{ + +//! Calculates matrix containing partial derivatives of state derivatives w.r.t. body state. +void VariationalEquations::setBodyStatePartialMatrix( ) +{ + // Initialize partial matrix + variationalMatrix_.setZero( ); + + if( dynamicalStatesToEstimate_.count( propagators::transational_state ) > 0 ) + { + int startIndex = stateTypeStartIndices_.at( propagators::transational_state ); + for( unsigned int i = 0; i < dynamicalStatesToEstimate_.at( propagators::transational_state ).size( ); i++ ) + { + variationalMatrix_.block( startIndex + i * 6, startIndex + i * 6 + 3, 3, 3 ).setIdentity( ); + } + } + + // Iterate over all bodies undergoing accelerations for which initial condition is to be estimated. + for( std::map< IntegratedStateType, std::vector< std::multimap< std::pair< int, int >, + boost::function< void( Eigen::Block< Eigen::MatrixXd > ) > > > >::iterator + typeIterator = statePartialList_.begin( ); typeIterator != statePartialList_.end( ); typeIterator++ ) + { + int startIndex = stateTypeStartIndices_.at( typeIterator->first ); + int currentStateSize = getSingleIntegrationSize( typeIterator->first ); + int entriesToSkipPerEntry = currentStateSize - currentStateSize / + getSingleIntegrationDifferentialEquationOrder( typeIterator->first ); + for( unsigned int i = 0; i < typeIterator->second.size( ); i++ ) + { + // Iterate over all bodies exerting an acceleration on this body. + for( statePartialIterator_ = typeIterator->second.at( i ).begin( ); + statePartialIterator_ != typeIterator->second.at( i ).end( ); + statePartialIterator_++ ) + { + statePartialIterator_->second( + variationalMatrix_.block( + startIndex + entriesToSkipPerEntry + i* currentStateSize, statePartialIterator_->first.first, + currentStateSize - entriesToSkipPerEntry, statePartialIterator_->first.second ) ); + + } + } + } + + // Correct partials for hierarchical dynamics + for( unsigned int i = 0; i < statePartialAdditionIndices_.size( ); i++ ) + { + variationalMatrix_.block( 0, statePartialAdditionIndices_.at( i ).second, totalDynamicalStateSize_, 3 ) += + variationalMatrix_.block( 0, statePartialAdditionIndices_.at( i ).first, totalDynamicalStateSize_, 3 ); + } +} + +//! Function to clear reference/cached values of state derivative partials. +void VariationalEquations::clearPartials( ) +{ + for( stateDerivativeTypeIterator_ = stateDerivativePartialList_.begin( ); + stateDerivativeTypeIterator_ != stateDerivativePartialList_.end( ); + stateDerivativeTypeIterator_++ ) + { + for( unsigned int i = 0; i < stateDerivativeTypeIterator_->second.size( ); i++ ) + { + for( unsigned int j = 0; j < stateDerivativeTypeIterator_->second.at( i ).size( ); j++ ) + { + stateDerivativeTypeIterator_->second.at( i ).at( j )->resetTime( TUDAT_NAN ); + } + + } + } +} + +//! This function updates all state derivative models to the current time and state. +void VariationalEquations::updatePartials( const double currentTime ) +{ + // Update all acceleration partials to current state and time. Information is passed indirectly from here, through + // (function) pointers set in acceleration partial classes + for( stateDerivativeTypeIterator_ = stateDerivativePartialList_.begin( ); + stateDerivativeTypeIterator_ != stateDerivativePartialList_.end( ); + stateDerivativeTypeIterator_++ ) + { + for( unsigned int i = 0; i < stateDerivativeTypeIterator_->second.size( ); i++ ) + { + for( unsigned int j = 0; j < stateDerivativeTypeIterator_->second.at( i ).size( ); j++ ) + { + stateDerivativeTypeIterator_->second.at( i ).at( j )->update( currentTime ); + } + + } + } + + for( stateDerivativeTypeIterator_ = stateDerivativePartialList_.begin( ); + stateDerivativeTypeIterator_ != stateDerivativePartialList_.end( ); + stateDerivativeTypeIterator_++ ) + { + for( unsigned int i = 0; i < stateDerivativeTypeIterator_->second.size( ); i++ ) + { + for( unsigned int j = 0; j < stateDerivativeTypeIterator_->second.at( i ).size( ); j++ ) + { + stateDerivativeTypeIterator_->second.at( i ).at( j )->updateParameterPartials( ); + } + + } + } +} +\ +//! Function (called by constructor) to set up the statePartialList_ member from the state derivative partials +void VariationalEquations::setStatePartialFunctionList( ) +{ + std::pair< boost::function< void( Eigen::Block< Eigen::MatrixXd > ) >, int > currentDerivativeFunction; + + // Iterate over all state types + for( std::map< propagators::IntegratedStateType, + orbit_determination::StateDerivativePartialsMap >::iterator + stateDerivativeTypeIterator_ = stateDerivativePartialList_.begin( ); + stateDerivativeTypeIterator_ != stateDerivativePartialList_.end( ); + stateDerivativeTypeIterator_++ ) + { + // Iterate over all bodies undergoing 'accelerations' for which initial state is to be estimated. + for( unsigned int i = 0; i < stateDerivativeTypeIterator_->second.size( ); i++ ) + { + std::multimap< std::pair< int, int >, boost::function< void( Eigen::Block< Eigen::MatrixXd > ) > > + currentBodyPartialList; + + // Iterate over all 'accelerations' from single body on other single body + for( unsigned int j = 0; j < stateDerivativeTypeIterator_->second.at( i ).size( ); j++ ) + { + for( std::map< propagators::IntegratedStateType, + std::vector< std::pair< std::string, std::string > > >::iterator + estimatedStateIterator = dynamicalStatesToEstimate_.begin( ); + estimatedStateIterator != dynamicalStatesToEstimate_.end( ); + estimatedStateIterator++ ) + { + // Iterate over all bodies to see if body exerting acceleration is also to be estimated (cross-terms) + for( unsigned int k = 0; k < estimatedStateIterator->second.size( ); k++ ) + { + currentDerivativeFunction = stateDerivativeTypeIterator_->second.at( i ).at( j )-> + getDerivativeFunctionWrtStateOfIntegratedBody( + estimatedStateIterator->second.at( k ), estimatedStateIterator->first ); + + // If function is not-empty: add to list. + if( currentDerivativeFunction.second != 0 ) + { + currentBodyPartialList.insert( + std::make_pair( + std::make_pair( k * getSingleIntegrationSize( estimatedStateIterator->first ) + + stateTypeStartIndices_.at( estimatedStateIterator->first ), + getSingleIntegrationSize( estimatedStateIterator->first ) ), + currentDerivativeFunction.first ) ); + } + } + } + } + statePartialList_[ stateDerivativeTypeIterator_->first ].push_back( currentBodyPartialList ); + } + } +} + +} + +} diff --git a/Tudat/Astrodynamics/Propagators/variationalEquations.h b/Tudat/Astrodynamics/Propagators/variationalEquations.h new file mode 100644 index 0000000000..52ff23457d --- /dev/null +++ b/Tudat/Astrodynamics/Propagators/variationalEquations.h @@ -0,0 +1,485 @@ +#ifndef TUDAT_VARIATIONALEQUATIONS_H +#define TUDAT_VARIATIONALEQUATIONS_H + +#include +#include +#include + +#include + +#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h" + +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" + +#include "Tudat/Astrodynamics/Propagators/nBodyStateDerivative.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/initialTranslationalState.h" +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/accelerationPartial.h" + +namespace tudat +{ + +namespace propagators +{ + +//! Class from which the variational equations can be evaluated. +/*! + * Class from which the variational equations can be evaluated. The time derivative of the state transition and + * sensitivity matrices are computed from a set of state derivative partials objects, at the current time and state. + * This class performs all required bookkeeping to update, evaluate and combine these state derivative partials into + * the variational equations. The VariationalEquationsSolver object is used to manage and execute the full numerical + * integration of these variational equations and equations of motion. + */ +class VariationalEquations +{ + +public: + + //! Constructor of variation equations class. + /*! + * Constructor of variation equations class. Since the vehicle state must be integrated along with + * the variational equations, an object calculating the state derivative is required. + * \param stateDerivativePartialList List partials of state derivative models from which the variational equations + * are set up. The key is the type of dynamics for which partials are taken, the values are StateDerivativePartialsMap + * (see StateDerivativePartialsMap definition for details) + * \param parametersToEstimate Object containing all parameters that are to be estimated and their current settings and + * values. + * \param stateTypeStartIndices Start index (value) in vector of propagated state for each type of state (key) + */ + template< typename ParameterType > + VariationalEquations( + const std::map< IntegratedStateType, orbit_determination::StateDerivativePartialsMap > + stateDerivativePartialList, + const boost::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > parametersToEstimate, + const std::map< IntegratedStateType, int >& stateTypeStartIndices ): + stateDerivativePartialList_( stateDerivativePartialList ), stateTypeStartIndices_( stateTypeStartIndices ) + { + dynamicalStatesToEstimate_ = + estimatable_parameters::getListOfInitialDynamicalStateParametersEstimate< ParameterType >( + parametersToEstimate ); + + // Get size of dynamical state to estimate + numberOfParameterValues_ = estimatable_parameters::getSingleArcParameterSetSize( parametersToEstimate ); + totalDynamicalStateSize_ = 0; + for( std::map< IntegratedStateType, orbit_determination::StateDerivativePartialsMap >::iterator + partialTypeIterator = stateDerivativePartialList_.begin( ); + partialTypeIterator != stateDerivativePartialList_.end( ); partialTypeIterator++ ) + { + + if( dynamicalStatesToEstimate_.count( partialTypeIterator->first ) == 0 ) + { + std::string errorMessage = "Error when making variational equations object, found no state to estimate of type " + + boost::lexical_cast< std::string >( partialTypeIterator->first ); + throw std::runtime_error( errorMessage ); + } + else if( dynamicalStatesToEstimate_.at( partialTypeIterator->first ).size( ) != + partialTypeIterator->second.size( ) ) + { + throw std::runtime_error( "Error when making variational equations object, input partial list size is inconsistent" ); + } + + totalDynamicalStateSize_ += + getSingleIntegrationSize( partialTypeIterator->first ) * partialTypeIterator->second.size( ); + } + + // Initialize matrices. + variationalMatrix_ = Eigen::MatrixXd::Zero( totalDynamicalStateSize_, totalDynamicalStateSize_ ); + variationalParameterMatrix_ = + Eigen::MatrixXd::Zero( totalDynamicalStateSize_, numberOfParameterValues_ - totalDynamicalStateSize_ ); + + // Set parameter partial functions. + setStatePartialFunctionList( ); + setTranslationalStatePartialFrameScalingFunctions( parametersToEstimate ); + setParameterPartialFunctionList( parametersToEstimate ); + } + + //! Calculates matrix containing partial derivatives of state derivatives w.r.t. body state. + /*! + * Calculates matrix containing partial derivatives of state derivatives w.r.t. body state, i.e. + * first matrix in right hand side of Eq. (7.45) in (Montenbruck & Gill, 2000). + * \return Matrix containing partial derivatives of state derivative w.r.t. body state + */ + void setBodyStatePartialMatrix( ); + + //! Function to compute the contribution of the derivatives w.r.t. current states in the variational equations + /*! + * Function to compute the contribution of the derivatives w.r.t. current states in the variational equations, + * e.g. first term in Eq. (7.45) in (Montenbruck & Gill, 2000). + * \param stateTransitionAndSensitivityMatrices Current combined state transition and sensitivity matric + * \param currentMatrixDerivative Matrix block which is to return (by reference) the given contribution to the + * variational equations. + */ + template< typename StateScalarType > + void getBodyInitialStatePartialMatrix( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& stateTransitionAndSensitivityMatrices, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > > currentMatrixDerivative ) + { + setBodyStatePartialMatrix( ); + + // Add partials of body positions and velocities. + currentMatrixDerivative.block( 0, 0, totalDynamicalStateSize_, numberOfParameterValues_ ) = + ( variationalMatrix_.template cast< StateScalarType >( ) * stateTransitionAndSensitivityMatrices ); + } + + //! Calculates matrix containing partial derivatives of state derivatives w.r.t. parameters. + /*! + * Calculates matrix containing partial derivatives of state derivatives w.r.t. parameters, i.e. + * second matrix in rhs of Eq. (7.45) in (Montenbruck & Gill, 2000). + * \param currentMatrixDerivative Matrix block containing partial derivatives of accelerarion w.r.t. parameters + * (returned by reference). + */ + template< typename StateScalarType > + void getParameterPartialMatrix( + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > > currentMatrixDerivative ) + { + // Initialize matrix to zeros + variationalParameterMatrix_.setZero( ); + + + // Iterate over all bodies undergoing accelerations for which initial condition is to be estimated. + for( std::map< IntegratedStateType, std::vector< std::multimap< std::pair< int, int >, + boost::function< void( Eigen::Block< Eigen::MatrixXd > ) > > > >::iterator typeIterator = + parameterPartialList_.begin( ); typeIterator != parameterPartialList_.end( ); typeIterator++ ) + { + int startIndex = stateTypeStartIndices_.at( typeIterator->first ); + int currentStateSize = getSingleIntegrationSize( typeIterator->first ); + int entriesToSkipPerEntry = currentStateSize - + currentStateSize / getSingleIntegrationDifferentialEquationOrder( typeIterator->first ); + + // Iterate over all bodies being estimated. + for( unsigned int i = 0; i < typeIterator->second.size( ); i++ ) + { + // Iterate over all parameter partial functions determined by setParameterPartialFunctionList( ) + for( functionIterator = typeIterator->second[ i ].begin( ); + functionIterator != typeIterator->second[ i ].end( ); + functionIterator++ ) + { + functionIterator->second( + variationalParameterMatrix_.block( + startIndex + entriesToSkipPerEntry + currentStateSize * i, + functionIterator->first.first - totalDynamicalStateSize_, + currentStateSize - entriesToSkipPerEntry, + functionIterator->first.second ) ); + } + } + + } + + currentMatrixDerivative.block( 0, totalDynamicalStateSize_, totalDynamicalStateSize_, + numberOfParameterValues_ - totalDynamicalStateSize_ ) += + variationalParameterMatrix_.template cast< StateScalarType >( ); + + } + + //! Evaluates the complete variational equations. + /*! + * Evaluates the complete variational equations at a given time and state transition matrix, sensitivity matrix and + * state (accessed indirectly). This function evaluates the complete Eq. (7.45) from (Montenbruck & Gill, 2000). + * \param time Current time + * \param stateTransitionAndSensitivityMatrices Combined state transition and sensitivity matrix. + * \param currentMatrixDerivative Variation equations result (returned by reference). + */ + template< typename StateScalarType > + void evaluateVariationalEquations( + const double time, const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& + stateTransitionAndSensitivityMatrices, + Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > > currentMatrixDerivative ) + { + // Compute and add state partials. + getBodyInitialStatePartialMatrix< StateScalarType >( stateTransitionAndSensitivityMatrices,currentMatrixDerivative ); + + if( numberOfParameterValues_ > totalDynamicalStateSize_ ) + { + // Add partials of parameters. + getParameterPartialMatrix< StateScalarType >( currentMatrixDerivative ); + } + } + + //! Function to clear reference/cached values of state derivative partials. + /*! + * Function to clear reference/cached values of state derivative partials, to ensure that they are all recalculated. + */ + void clearPartials( ); + + //! This function updates all state derivative models to the current time and state. + /*! + * This function updates all state derivative models to the current time and state. + * \param currentTime Time to which the system is to be updated. + */ + void updatePartials( const double currentTime ); + + //! Returns the number of parameter values. + /*! + * Returns the number of parameter values (i.e. number of columns in state transition matrix). + * \return Number of parameter values. + */ + double getNumberOfParameterValues( ) + { + return numberOfParameterValues_; + } + +protected: + +private: + + //! Function (called by constructor) to set up the statePartialList_ member from the state derivative partials + /*! + * Function (called by constructor) to set up the functions to evaluate the partial derivatives of the state derivatives + * w.r.t. a current state (stored in the statePartialList_ member) from the state derivative partials. + */ + void setStatePartialFunctionList( ); + + //! Function to add parameter partial functions for single state derivative model, and set of parameter objects. + /*! + * Function to add parameter partial functions for single state derivative model, and set of parameter objects. + * Partial derivative functions that are not-empty are added to the functionListOfBody input (returned by reference). + * A list of parameters of a single type (double or vector) are handled a single function call. + * \param parameterList Map of parameters for which partial functions are to checked and created. Map keys are + * start entry of parameter in total parameter vector. + * \param partialObject State derivative partial object from which partial functions are to be retrieved. + * \param functionListOfBody Multimap of partial derivative functions to which entries are to be added by this function. + * Map key is start index and size of given parameter in sensitivity matrix. Map value is partial function. + * \param totalParameterVectorIndicesToSubtract Number of entries by which to shift start index in sensitivity + * matrix from entry in parameter vector (used for multi-arc estimation). + */ + template< typename CurrentParameterType > + void addParameterPartialToList( + const std::map< int, boost::shared_ptr< estimatable_parameters::EstimatableParameter< CurrentParameterType > > >& + parameterList, + const boost::shared_ptr< orbit_determination::StateDerivativePartial > partialObject, + std::multimap< std::pair< int, int >, boost::function< void( Eigen::Block< Eigen::MatrixXd > ) > >& + functionListOfBody, + const int totalParameterVectorIndicesToSubtract = 0 ) + { + using namespace acceleration_partials; + + // Iterate over all parameters. + for( typename std::map< int, + boost::shared_ptr< estimatable_parameters::EstimatableParameter< CurrentParameterType > > >::const_iterator + parameterIterator = parameterList.begin( ); parameterIterator != parameterList.end( ); parameterIterator++ ) + { + // Add current parameter to list of partials to be computed for current acceleration (if dependency exists) + int functionToEvaluate = + partialObject->setParameterPartialUpdateFunction( parameterIterator->second ); + + // If function is non-NULL, add to list + if( functionToEvaluate != 0 ) + { + // Make pair of indices for generating parameter partial matrix: + //first is start column in matrix, second is number of entries (1 for double parameter) + std::pair< int, int > indexPair = std::make_pair( + parameterIterator->first - totalParameterVectorIndicesToSubtract, functionToEvaluate ); + + // Add to list. + functionListOfBody.insert( + std::pair< std::pair< int, int >, boost::function< void( Eigen::Block< Eigen::MatrixXd > ) > > + ( indexPair, boost::bind( + static_cast< void ( orbit_determination::StateDerivativePartial::* ) + ( const boost::shared_ptr< + estimatable_parameters::EstimatableParameter< CurrentParameterType > >, + Eigen::Block< Eigen::MatrixXd > )> + ( &orbit_determination::StateDerivativePartial::getCurrentParameterPartial ), + partialObject, parameterIterator->second, _1 ) ) ); + } + } + } + + //! This function creates the list of partial derivatives of the state w.r.t. parameter values. + /*! + * This function creates the list of partial derivatives of the state w.r.t. parameter values. + * The function is called once by the constructor and the resulting functions are set as member variables. + * This prevents having to check whether an acceleration model depends on every parameter during every time step. + * \param parametersToEstimate Object containing all parameters that are to be estimated and their current settings and + * values. + */ + template< typename ParameterType > + void setParameterPartialFunctionList( + const boost::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > + parametersToEstimate ) + { + // Get double parameters. + std::map< int, boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > > + doubleParametersToEstimate = + parametersToEstimate->getDoubleParameters( ); + + // Get vector parameters. + std::map< int, boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > > + vectorParametersToEstimate = + parametersToEstimate->getVectorParameters( ); + + int totalParameterVectorIndicesToSubtract = parametersToEstimate->getInitialDynamicalStateParameterSize( ) - + estimatable_parameters::getSingleArcInitialDynamicalStateParameterSetSize( parametersToEstimate ); + + for( std::map< propagators::IntegratedStateType, + orbit_determination::StateDerivativePartialsMap >::iterator + stateDerivativeTypeIterator = stateDerivativePartialList_.begin( ); + stateDerivativeTypeIterator != stateDerivativePartialList_.end( ); + stateDerivativeTypeIterator++ ) + { + + // Initialize vector of lists to correct size. + parameterPartialList_[ stateDerivativeTypeIterator->first ].resize( + stateDerivativeTypeIterator->second.size( ) ); + + // Iterate over all bodies of which initial position is being estimated. + for( unsigned int i = 0; i < stateDerivativeTypeIterator->second.size( ); i++ ) + { + // Initialize list of parameter partial functions for single body. + std::multimap< std::pair< int, int >, boost::function< void( Eigen::Block< Eigen::MatrixXd > ) > > + functionListOfBody; + + // Iterate over all accelerations due to this body on current body. + for( unsigned int j = 0; j < stateDerivativeTypeIterator->second.at( i ).size( ); j++ ) + { + addParameterPartialToList< double >( + doubleParametersToEstimate, stateDerivativeTypeIterator->second.at( i ).at( j ), + functionListOfBody, totalParameterVectorIndicesToSubtract ); + addParameterPartialToList< Eigen::VectorXd >( + vectorParametersToEstimate, stateDerivativeTypeIterator->second.at( i ).at( j ), + functionListOfBody, totalParameterVectorIndicesToSubtract ); + } + + // Add generated parameter partial list of current body. + parameterPartialList_[ stateDerivativeTypeIterator->first ][ i ] = functionListOfBody; + } + } + } + + //! Function called by constructor to handle estimation of hierarchical translational dynamics + /*! + * Function called by constructor to handle estimation of hierarchical translational dynamics, i.e. where + * the state of body A is estimated w.r.t. body B, and body B is itself estimated w.r.t. to some third body (or inertial + * point) C. + * \param parametersToEstimate Total list of parameters to estimate. + */ + template< typename ParameterType > + void setTranslationalStatePartialFrameScalingFunctions( + const boost::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > + parametersToEstimate ) + { + std::vector< boost::shared_ptr< estimatable_parameters::EstimatableParameter< + Eigen::Matrix< ParameterType, Eigen::Dynamic, 1 > > > > initialDynamicalParameters = + parametersToEstimate->getEstimatedInitialStateParameters( ); + + std::vector< std::string > propagatedBodies; + std::vector< std::string > centralBodies; + + // Retrieve propagated bodies and central bodies of estimation. + for( unsigned int i = 0; i < initialDynamicalParameters.size( ); i++ ) + { + if( initialDynamicalParameters.at( i )->getParameterName( ).first == estimatable_parameters::initial_body_state ) + { + propagatedBodies.push_back( + initialDynamicalParameters.at( i )->getParameterName( ).second.first ); + centralBodies.push_back( boost::dynamic_pointer_cast + < estimatable_parameters::InitialTranslationalStateParameter< ParameterType > >( + initialDynamicalParameters.at( i ) )->getCentralBody( ) ); + } + } + + // Get order in which ephemerides were to be updated. + std::vector< std::string > updateOrder = determineEphemerisUpdateorder( + propagatedBodies, centralBodies, centralBodies ); + + // Iterate over central bodies and propagated bodies and check for dependencies + for( int i = updateOrder.size( ) - 1; i >= 0 ; i-- ) + { + int currentBodyIndex = std::distance( + propagatedBodies.begin( ), + std::find( propagatedBodies.begin( ), propagatedBodies.end( ), updateOrder.at( i ) ) ); + for( unsigned int j = 0; j < propagatedBodies.size( ); j++ ) + { + if( centralBodies.at( currentBodyIndex ) == propagatedBodies.at( j ) ) + { + + statePartialAdditionIndices_.push_back( + std::make_pair( stateTypeStartIndices_[ propagators::transational_state ] + + currentBodyIndex * propagators::getSingleIntegrationSize( propagators::transational_state ), + stateTypeStartIndices_[ propagators::transational_state ] + + j * propagators::getSingleIntegrationSize( propagators::transational_state ) ) ); + } + } + } + } + + + //! Map with list of StateDerivativePartialsMaps, with state type as key. + /*! + * List partials of state derivative models from which the variational equations + * are set up. The key is the type of dynamics for which partials are taken, the values are StateDerivativePartialsMap + * (see StateDerivativePartialsMap definition for details) + */ + std::map< propagators::IntegratedStateType, orbit_determination::StateDerivativePartialsMap > + stateDerivativePartialList_; + + //! Map of start entry in sensitivity matrix of each type of estimated dynamics. + std::map< IntegratedStateType, int > stateTypeStartIndices_; + + //! List of all functions returning current partial derivative w.r.t. a current dynamical state + /*! + * List of all functions returning current partial derivative w.r.t. a current dynamical state (map key). The + * vector entries correspond to the entries in the outer vector of StateDerivativePartialsMaps in + * stateDerivativePartialList_. The multimaps inside the vector provide the functions (as values) adding the + * partials to a given matrix block and the start column and number of columns in matrix partial (as keys). + */ + std::map< IntegratedStateType, + std::vector< std::multimap< std::pair< int, int >, boost::function< void( Eigen::Block< Eigen::MatrixXd > ) > > > > + statePartialList_; + + //! Pre-defined iterator for efficiency. + std::multimap< std::pair< int, int >, boost::function< void( Eigen::Block< Eigen::MatrixXd > ) > >::iterator + statePartialIterator_; + + //! Vector of pair providing indices of column blocks of variational equations to add to other column blocks + /*! + * Vector of pair providing indices of column blocks of variational equations to add to other column blocks, + * which is needed by the hierarchical estimation fo dynamics. The second pair entry is the start of the column + * block (of size 3) to where it should be copied. The first entry denotes from where it should be copied. + * \sa setTranslationalStatePartialFrameScalingFunctions + */ + std::vector< std::pair< int, int > > statePartialAdditionIndices_; + + + //! List of all functions returning current partial derivative w.r.t. a parameter + /*! + * List of all functions returning current partial derivative w.r.t. a parameter. + * Map key denotes associated dynamics type w.r.t which partial is taken. The + * vector entries correspond to the entries in the outer vector of StateDerivativePartialsMaps in + * stateDerivativePartialList_. The multimaps inside the vector provide the functions (as values) adding the + * partials to a given matrix block and the start column and number of columns in matrix partial (as keys). + */ + std::map< IntegratedStateType, std::vector< std::multimap< std::pair< int, int >, + boost::function< void( Eigen::Block< Eigen::MatrixXd > ) > > > > parameterPartialList_; + + //! Pre-declared iterator over all parameter partial functions. + std::multimap< std::pair< int, int >, boost::function< void( Eigen::Block< Eigen::MatrixXd > ) > > + ::iterator functionIterator; + + //! Pre-declared iterator over all state types + std::map< propagators::IntegratedStateType, orbit_determination::StateDerivativePartialsMap > + ::iterator stateDerivativeTypeIterator_; + + //! List of identifiers for points/bodies for which initial dynamical state is to be estimated. + std::map< propagators::IntegratedStateType, std::vector< std::pair< std::string, std::string > > > + dynamicalStatesToEstimate_; + + + //! Number of parameter values in estimation (i.e. number of columns in sensitivity matrix) + int numberOfParameterValues_; + + //! Total size of (single-arc) state vector of dynamics that is to be estimated. + int totalDynamicalStateSize_; + + //! Total matrix of partial derivatives of state derivatives w.r.t. current states. + Eigen::MatrixXd variationalMatrix_; + + //! Total matrix of partial derivatives of state derivatives w.r.t. parameter vectors. + Eigen::MatrixXd variationalParameterMatrix_; +}; + + +} // namespace propagators + +} // namespace tudat + +#endif // TUDAT_VARIATIONALEQUATIONS_H diff --git a/Tudat/Astrodynamics/Propulsion/CMakeLists.txt b/Tudat/Astrodynamics/Propulsion/CMakeLists.txt new file mode 100755 index 0000000000..0a894fc300 --- /dev/null +++ b/Tudat/Astrodynamics/Propulsion/CMakeLists.txt @@ -0,0 +1,43 @@ + # Copyright (c) 2010-2016, Delft University of Technology + # All rigths reserved + # + # This file is part of the Tudat. Redistribution and use in source and + # binary forms, with or without modification, are permitted exclusively + # under the terms of the Modified BSD license. You should have received + # a copy of the license with this file. If not, please or visit: + # http://tudat.tudelft.nl/LICENSE. + # + + +# Set the source files. +set(PROPULSION_SOURCES + "${SRCROOT}${PROPULSIONDIR}/thrustGuidance.cpp" + "${SRCROOT}${PROPULSIONDIR}/thrustFunctions.cpp" +) + +# Set the header files. +set(PROPULSION_HEADERS + "${SRCROOT}${PROPULSIONDIR}/thrustAccelerationModel.h" + "${SRCROOT}${PROPULSIONDIR}/thrustGuidance.h" + "${SRCROOT}${PROPULSIONDIR}/thrustFunctions.h" + "${SRCROOT}${PROPULSIONDIR}/thrustMagnitudeWrapper.h" + "${SRCROOT}${PROPULSIONDIR}/massRateFromThrust.h" +) + +# Add static libraries. +add_library(tudat_propulsion STATIC ${PROPULSION_SOURCES} ${PROPULSION_HEADERS}) +setup_tudat_library_target(tudat_propulsion "${SRCROOT}${PROPULSIONDIR}") + +# Add unit tests. +set(PROPULSION_UNITTESTS + "${SRCROOT}${PROPULSIONDIR}/UnitTests/unitTestBasicAstrodynamics.cpp" + "${SRCROOT}${PROPULSIONDIR}/UnitTests/unitTestAstrodynamicsFunctions.cpp" + "${SRCROOT}${PROPULSIONDIR}/UnitTests/unitTestOrbitalElementConversions.cpp" + "${SRCROOT}${PROPULSIONDIR}/UnitTests/unitTestPhysicalConstants.cpp" + "${SRCROOT}${PROPULSIONDIR}/UnitTests/unitTestUnitConversions.cpp" + "${SRCROOT}${PROPULSIONDIR}/UnitTests/unitTestUnifiedStateModelElementConversions.cpp" +) + +add_executable(test_ThrustAcceleration "${SRCROOT}${PROPULSIONDIR}/UnitTests/unitTestThrustAcceleration.cpp") +setup_custom_test_program(test_ThrustAcceleration "${SRCROOT}${PROPULSIONDIR}") +target_link_libraries(test_ThrustAcceleration ${TUDAT_PROPAGATION_LIBRARIES} ${Boost_LIBRARIES}) diff --git a/Tudat/Astrodynamics/Propulsion/UnitTests/unitTestThrustAcceleration.cpp b/Tudat/Astrodynamics/Propulsion/UnitTests/unitTestThrustAcceleration.cpp new file mode 100644 index 0000000000..0b7ae10f2d --- /dev/null +++ b/Tudat/Astrodynamics/Propulsion/UnitTests/unitTestThrustAcceleration.cpp @@ -0,0 +1,678 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#define BOOST_TEST_MAIN + +#include +#include +#include +#include + +#include +#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h" +#include +#include +#include +#include +#include "Tudat/SimulationSetup/PropagationSetup/createNumericalSimulator.h" +#include +#include + +#include +#include + +#include + +namespace tudat +{ + +namespace unit_tests +{ + +BOOST_AUTO_TEST_SUITE( test_thrust_acceleration ) + +BOOST_AUTO_TEST_CASE( testConstantThrustAcceleration ) +{ + using namespace tudat; + using namespace numerical_integrators; + using namespace simulation_setup; + using namespace basic_astrodynamics; + using namespace propagators; + using namespace basic_mathematics; + using namespace basic_astrodynamics; + + // Create Earth object + simulation_setup::NamedBodyMap bodyMap; + + // Create vehicle objects. + double vehicleMass = 5.0E3; + bodyMap[ "Vehicle" ] = boost::make_shared< simulation_setup::Body >( ); + bodyMap[ "Vehicle" ]->setConstantBodyMass( vehicleMass ); + bodyMap[ "Vehicle" ]->setEphemeris( + boost::make_shared< ephemerides::TabulatedCartesianEphemeris< > >( + boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, basic_mathematics::Vector6d > >( ), + "SSB" ) ); + + // Finalize body creation. + setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" ); + + // Define propagator settings variables. + SelectedAccelerationMap accelerationMap; + std::vector< std::string > bodiesToPropagate; + std::vector< std::string > centralBodies; + + Eigen::Vector3d thrustDirection; + thrustDirection << -1.4, 2.4, 5,6; + + double thrustMagnitude = 1.0E3; + double specificImpulse = 250.0; + double massRate = thrustMagnitude / ( specificImpulse * physical_constants::SEA_LEVEL_GRAVITATIONAL_ACCELERATION ); + + // Define acceleration model settings. + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle; + accelerationsOfVehicle[ "Vehicle" ].push_back( boost::make_shared< ThrustAccelerationSettings >( + boost::make_shared< CustomThrustDirectionSettings >( + boost::lambda::constant( thrustDirection ) ), + boost::make_shared< ConstantThrustEngineSettings >( + thrustMagnitude, specificImpulse ) ) ); + + accelerationMap[ "Vehicle" ] = accelerationsOfVehicle; + + bodiesToPropagate.push_back( "Vehicle" ); + centralBodies.push_back( "SSB" ); + + // Set initial state + basic_mathematics::Vector6d systemInitialState = basic_mathematics::Vector6d::Zero( ); + + // Create acceleration models and propagation settings. + basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodyMap, accelerationMap, bodiesToPropagate, centralBodies ); + + boost::shared_ptr< PropagationTimeTerminationSettings > terminationSettings = + boost::make_shared< propagators::PropagationTimeTerminationSettings >( 1000.0 ); + boost::shared_ptr< TranslationalStatePropagatorSettings< double > > translationalPropagatorSettings = + boost::make_shared< TranslationalStatePropagatorSettings< double > > + ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, terminationSettings ); + boost::shared_ptr< IntegratorSettings< > > integratorSettings = + boost::make_shared< IntegratorSettings< > > + ( rungeKutta4, 0.0, 0.1 ); + { + // Create simulation object and propagate dynamics. + SingleArcDynamicsSimulator< > dynamicsSimulator( + bodyMap, integratorSettings, translationalPropagatorSettings, true, false, false ); + + // Retrieve numerical solutions for state and dependent variables + std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > > numericalSolution = + dynamicsSimulator.getEquationsOfMotionNumericalSolution( ); + + Eigen::Vector3d constantAcceleration = thrustDirection.normalized( ) * thrustMagnitude / vehicleMass; + for( std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > >::const_iterator outputIterator = + numericalSolution.begin( ); outputIterator != numericalSolution.end( ); outputIterator++ ) + { + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ( outputIterator->second.segment( 0, 3 ) ), + ( 0.5 * constantAcceleration * std::pow( outputIterator->first, 2.0 ) ), 1.0E-12 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ( outputIterator->second.segment( 3, 3 ) ), + ( constantAcceleration * outputIterator->first ), 1.0E-12 ); + } + } + { + std::map< std::string, boost::shared_ptr< basic_astrodynamics::MassRateModel > > massRateModels; + massRateModels[ "Vehicle" ] = ( + createMassRateModel( "Vehicle", boost::make_shared< FromThrustMassModelSettings >( 1 ), + bodyMap, accelerationModelMap ) ); + + boost::shared_ptr< PropagatorSettings< double > > massPropagatorSettings = + boost::make_shared< MassPropagatorSettings< double > >( + boost::assign::list_of( "Vehicle" ), massRateModels, + ( Eigen::Matrix< double, 1, 1 >( ) << vehicleMass ).finished( ), terminationSettings ); + + std::vector< boost::shared_ptr< PropagatorSettings< double > > > propagatorSettingsVector; + propagatorSettingsVector.push_back( translationalPropagatorSettings ); + propagatorSettingsVector.push_back( massPropagatorSettings ); + + boost::shared_ptr< PropagatorSettings< double > > propagatorSettings = + boost::make_shared< MultiTypePropagatorSettings< double > >( propagatorSettingsVector, terminationSettings ); + + // Create simulation object and propagate dynamics. + SingleArcDynamicsSimulator< > dynamicsSimulator( + bodyMap, integratorSettings, propagatorSettings, true, false, false ); + + std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > > numericalSolution = + dynamicsSimulator.getEquationsOfMotionNumericalSolution( ); + + for( std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > >::const_iterator outputIterator = + numericalSolution.begin( ); outputIterator != numericalSolution.end( ); outputIterator++ ) + { + double currentMass = vehicleMass - outputIterator->first * massRate; + + Eigen::Vector3d currentVelocity = thrustDirection.normalized( ) * + specificImpulse * physical_constants::SEA_LEVEL_GRAVITATIONAL_ACCELERATION * + std::log( vehicleMass / currentMass ); + BOOST_CHECK_CLOSE_FRACTION( outputIterator->second( 6 ), currentMass, 1.0E-12 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ( outputIterator->second.segment( 3, 3 ) ), currentVelocity, 1.0E-11 ); + + } + } +} + +BOOST_AUTO_TEST_CASE( testFromEngineThrustAcceleration ) +{ + using namespace tudat; + using namespace numerical_integrators; + using namespace simulation_setup; + using namespace basic_astrodynamics; + using namespace propagators; + using namespace basic_mathematics; + using namespace basic_astrodynamics; + + + for( unsigned int i = 0; i < 4; i++ ) + { + // Create Earth object + simulation_setup::NamedBodyMap bodyMap; + + // Create vehicle objects. + double vehicleMass = 5.0E3; + double dryVehicleMass = 2.0E3; + + bodyMap[ "Vehicle" ] = boost::make_shared< simulation_setup::Body >( ); + bodyMap[ "Vehicle" ]->setConstantBodyMass( vehicleMass ); + bodyMap[ "Vehicle" ]->setEphemeris( + boost::make_shared< ephemerides::TabulatedCartesianEphemeris< > >( + boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, basic_mathematics::Vector6d > >( ), + "SSB" ) ); + + + double thrustMagnitude1 = 1.0E3; + double specificImpulse1 = 250.0; + double massFlow1 = propulsion::computePropellantMassRateFromSpecificImpulse( + thrustMagnitude1, specificImpulse1 ); + + + double thrustMagnitude2 = 2.0E3; + double specificImpulse2 = 300.0; + double massFlow2 = propulsion::computePropellantMassRateFromSpecificImpulse( + thrustMagnitude2, specificImpulse2 ); + + boost::shared_ptr< system_models::VehicleSystems > vehicleSystems = boost::make_shared< + system_models::VehicleSystems >( dryVehicleMass ); + boost::shared_ptr< system_models::EngineModel > vehicleEngineModel1 = + boost::make_shared< system_models::DirectEngineModel >( + boost::lambda::constant( specificImpulse1 ), boost::lambda::constant( massFlow1 ) ); + boost::shared_ptr< system_models::EngineModel > vehicleEngineModel2 = + boost::make_shared< system_models::DirectEngineModel >( + boost::lambda::constant( specificImpulse2 ), boost::lambda::constant( massFlow2 ) ); + vehicleSystems->setEngineModel( vehicleEngineModel1, "Engine1" ); + vehicleSystems->setEngineModel( vehicleEngineModel2, "Engine2" ); + bodyMap.at( "Vehicle" )->setVehicleSystems( vehicleSystems ); + + // Finalize body creation. + setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" ); + + // Define propagator settings variables. + SelectedAccelerationMap accelerationMap; + std::vector< std::string > bodiesToPropagate; + std::vector< std::string > centralBodies; + + Eigen::Vector3d thrustDirection; + thrustDirection << -1.4, 2.4, 5,6; + + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle; + // Define acceleration model settings. + switch( i ) + { + case 0: + { + accelerationsOfVehicle[ "Vehicle" ].push_back( boost::make_shared< ThrustAccelerationSettings >( + boost::make_shared< CustomThrustDirectionSettings >( + boost::lambda::constant( thrustDirection ) ), + boost::make_shared< FromBodyThrustEngineSettings >( + 1, "" ) ) ); + accelerationMap[ "Vehicle" ] = accelerationsOfVehicle; + break; + } + case 1: + { + accelerationsOfVehicle[ "Vehicle" ].push_back( boost::make_shared< ThrustAccelerationSettings >( + boost::make_shared< CustomThrustDirectionSettings >( + boost::lambda::constant( thrustDirection ) ), + boost::make_shared< FromBodyThrustEngineSettings >( + 0, "Engine1" ) ) ); + accelerationMap[ "Vehicle" ] = accelerationsOfVehicle; + break; + } + case 2: + { + accelerationsOfVehicle[ "Vehicle" ].push_back( boost::make_shared< ThrustAccelerationSettings >( + boost::make_shared< CustomThrustDirectionSettings >( + boost::lambda::constant( thrustDirection ) ), + boost::make_shared< FromBodyThrustEngineSettings >( + 0, "Engine2" ) ) ); + accelerationMap[ "Vehicle" ] = accelerationsOfVehicle; + break; + } + case 3: + { + accelerationsOfVehicle[ "Vehicle" ].push_back( boost::make_shared< ThrustAccelerationSettings >( + boost::make_shared< CustomThrustDirectionSettings >( + boost::lambda::constant( thrustDirection ) ), + boost::make_shared< FromBodyThrustEngineSettings >( + 0, "Engine1" ) ) ); + accelerationsOfVehicle[ "Vehicle" ].push_back( boost::make_shared< ThrustAccelerationSettings >( + boost::make_shared< CustomThrustDirectionSettings >( + boost::lambda::constant( thrustDirection ) ), + boost::make_shared< FromBodyThrustEngineSettings >( + 0, "Engine2" ) ) ); + accelerationMap[ "Vehicle" ] = accelerationsOfVehicle; + break; + } + } + + bodiesToPropagate.push_back( "Vehicle" ); + centralBodies.push_back( "SSB" ); + + // Set initial state + basic_mathematics::Vector6d systemInitialState = basic_mathematics::Vector6d::Zero( ); + + // Create acceleration models and propagation settings. + basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodyMap, accelerationMap, bodiesToPropagate, centralBodies ); + + boost::shared_ptr< PropagationTimeTerminationSettings > terminationSettings = + boost::make_shared< propagators::PropagationTimeTerminationSettings >( 1000.0 ); + boost::shared_ptr< TranslationalStatePropagatorSettings< double > > translationalPropagatorSettings = + boost::make_shared< TranslationalStatePropagatorSettings< double > > + ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, terminationSettings ); + boost::shared_ptr< IntegratorSettings< > > integratorSettings = + boost::make_shared< IntegratorSettings< > > + ( rungeKutta4, 0.0, 0.1 ); + + std::map< std::string, boost::shared_ptr< basic_astrodynamics::MassRateModel > > massRateModels; + + double totalMassRate, totalThrust; + switch( i ) + { + case 0: + { + massRateModels[ "Vehicle" ] = ( + createMassRateModel( "Vehicle", boost::make_shared< FromThrustMassModelSettings >( 1 ), + bodyMap, accelerationModelMap ) ); + totalMassRate = massFlow1 + massFlow2; + totalThrust = thrustMagnitude1 + thrustMagnitude2; + break; + } + case 1: + { + massRateModels[ "Vehicle" ] = ( + createMassRateModel( "Vehicle", boost::make_shared< FromThrustMassModelSettings >( 0, "Engine1" ), + bodyMap, accelerationModelMap ) ); + totalMassRate = massFlow1; + totalThrust = thrustMagnitude1; + break; + } + case 2: + { + massRateModels[ "Vehicle" ] = ( + createMassRateModel( "Vehicle", boost::make_shared< FromThrustMassModelSettings >( 0, "Engine2" ), + bodyMap, accelerationModelMap ) ); + totalMassRate = massFlow2; + totalThrust = thrustMagnitude2; + break; + } + case 3: + { + massRateModels[ "Vehicle" ] = ( + createMassRateModel( "Vehicle", boost::make_shared< FromThrustMassModelSettings >( 0, "Engine1" ), + bodyMap, accelerationModelMap ) ); + totalMassRate = massFlow1; + totalThrust = thrustMagnitude1 + thrustMagnitude2; + break; + } + } + + double totalSpecificImpulse = totalThrust / ( physical_constants::SEA_LEVEL_GRAVITATIONAL_ACCELERATION * totalMassRate ); + + boost::shared_ptr< PropagatorSettings< double > > massPropagatorSettings = + boost::make_shared< MassPropagatorSettings< double > >( + boost::assign::list_of( "Vehicle" ), massRateModels, + ( Eigen::Matrix< double, 1, 1 >( ) << vehicleMass ).finished( ), terminationSettings ); + + std::vector< boost::shared_ptr< PropagatorSettings< double > > > propagatorSettingsVector; + propagatorSettingsVector.push_back( translationalPropagatorSettings ); + propagatorSettingsVector.push_back( massPropagatorSettings ); + + boost::shared_ptr< PropagatorSettings< double > > propagatorSettings = + boost::make_shared< MultiTypePropagatorSettings< double > >( propagatorSettingsVector, terminationSettings ); + + // Create simulation object and propagate dynamics. + SingleArcDynamicsSimulator< > dynamicsSimulator( + bodyMap, integratorSettings, propagatorSettings, true, false, false ); + + std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > > numericalSolution = + dynamicsSimulator.getEquationsOfMotionNumericalSolution( ); + + for( std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > >::const_iterator outputIterator = + numericalSolution.begin( ); outputIterator != numericalSolution.end( ); outputIterator++ ) + { + double currentMass = vehicleMass - outputIterator->first * totalMassRate; + + Eigen::Vector3d currentVelocity = thrustDirection.normalized( ) * + totalSpecificImpulse * physical_constants::SEA_LEVEL_GRAVITATIONAL_ACCELERATION * + std::log( vehicleMass / currentMass ); + BOOST_CHECK_CLOSE_FRACTION( outputIterator->second( 6 ), currentMass, 1.0E-12 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ( outputIterator->second.segment( 3, 3 ) ), currentVelocity, 1.0E-11 ); + + } + } + +} + +BOOST_AUTO_TEST_CASE( testRadialAndVelocityThrustAcceleration ) +{ + using namespace tudat; + using namespace numerical_integrators; + using namespace simulation_setup; + using namespace basic_astrodynamics; + using namespace propagators; + using namespace basic_mathematics; + using namespace basic_astrodynamics; + + //Load spice kernels. + std::string kernelsPath = input_output::getSpiceKernelPath( ); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de-403-masses.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de421.bsp"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "naif0009.tls"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "pck00009.tpc"); + + double thrustMagnitude = 1.0E3; + double specificImpulse = 250.0; + + for( unsigned int i = 0; i < 2; i++ ) + { + // Create Earth object + simulation_setup::NamedBodyMap bodyMap; + + // Create vehicle objects. + double vehicleMass = 5.0E3; + bodyMap[ "Vehicle" ] = boost::make_shared< simulation_setup::Body >( ); + bodyMap[ "Vehicle" ]->setConstantBodyMass( vehicleMass ); + bodyMap[ "Vehicle" ]->setEphemeris( + boost::make_shared< ephemerides::TabulatedCartesianEphemeris< > >( + boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, basic_mathematics::Vector6d > >( ), + "Earth" ) ); + bodyMap[ "Earth" ] = boost::make_shared< Body >( ); + + bodyMap[ "Earth" ]->setEphemeris( + boost::make_shared< ephemerides::SpiceEphemeris >( "Sun", "SSB", false, false ) ); + bodyMap[ "Earth" ]->setGravityFieldModel( boost::make_shared< gravitation::GravityFieldModel >( + spice_interface::getBodyGravitationalParameter( "Earth" ) ) ); + + // Finalize body creation. + setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" ); + + // Define propagator settings variables. + SelectedAccelerationMap accelerationMap; + std::vector< std::string > bodiesToPropagate; + std::vector< std::string > centralBodies; + + bool isThurstInVelocityDirection; + + if( i == 0 ) + { + isThurstInVelocityDirection = 0; + } + else + { + isThurstInVelocityDirection = 1; + } + + // Define acceleration model settings. + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle; + accelerationsOfVehicle[ "Vehicle" ].push_back( boost::make_shared< ThrustAccelerationSettings >( + boost::make_shared< ThrustDirectionFromStateGuidanceSettings >( + "Earth", isThurstInVelocityDirection, 1 ), + boost::make_shared< ConstantThrustEngineSettings >( + thrustMagnitude, specificImpulse ) ) ); + if( i == 1 ) + { + accelerationsOfVehicle[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + } + + accelerationMap[ "Vehicle" ] = accelerationsOfVehicle; + + bodiesToPropagate.push_back( "Vehicle" ); + centralBodies.push_back( "Earth" ); + + // Set initial state + double radius = 1.0E3; + double circularVelocity = std::sqrt( radius * thrustMagnitude / vehicleMass ); + basic_mathematics::Vector6d systemInitialState = basic_mathematics::Vector6d::Zero( ); + + if( i == 0 ) + { + systemInitialState( 0 ) = radius; + systemInitialState( 4 ) = circularVelocity; + } + else + { + systemInitialState( 0 ) = 8.0E6; + systemInitialState( 4 ) = 7.5E3; + + } + + // Create acceleration models and propagation settings. + basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodyMap, accelerationMap, bodiesToPropagate, centralBodies ); + + boost::shared_ptr< DependentVariableSaveSettings > dependentVariableSaveSettings; + if( i == 1 ) + { + std::vector< boost::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables; + dependentVariables.push_back( + boost::make_shared< SingleAccelerationDependentVariableSaveSettings >( + thrust_acceleration, "Vehicle", "Vehicle", 0 ) ); + dependentVariableSaveSettings = boost::make_shared< DependentVariableSaveSettings >( dependentVariables ); + + } + boost::shared_ptr< PropagationTimeTerminationSettings > terminationSettings = + boost::make_shared< propagators::PropagationTimeTerminationSettings >( 1000.0 ); + boost::shared_ptr< TranslationalStatePropagatorSettings< double > > translationalPropagatorSettings = + boost::make_shared< TranslationalStatePropagatorSettings< double > > + ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, terminationSettings, + cowell, dependentVariableSaveSettings ); + boost::shared_ptr< IntegratorSettings< > > integratorSettings = + boost::make_shared< IntegratorSettings< > > + ( rungeKutta4, 0.0, 0.1 ); + + // Create simulation object and propagate dynamics. + SingleArcDynamicsSimulator< > dynamicsSimulator( + bodyMap, integratorSettings, translationalPropagatorSettings, true, false, false ); + + // Retrieve numerical solutions for state and dependent variables + std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > > numericalSolution = + dynamicsSimulator.getEquationsOfMotionNumericalSolution( ); + + std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > > dependentVariableSolution = + dynamicsSimulator.getDependentVariableHistory( ); + + if( i == 0 ) + { + double angularVelocity = circularVelocity / radius; + + for( std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > >::const_iterator outputIterator = + numericalSolution.begin( ); outputIterator != numericalSolution.end( ); outputIterator++ ) + { + double currentAngle = angularVelocity * outputIterator->first; + + BOOST_CHECK_CLOSE_FRACTION( + ( outputIterator->second.segment( 0, 3 ).norm( ) ), radius, 1.0E-10 * radius ); + BOOST_CHECK_CLOSE_FRACTION( + ( outputIterator->second.segment( 3, 3 ).norm( ) ), circularVelocity, 1.0E-10 * circularVelocity ); + BOOST_CHECK_SMALL( + std::fabs( outputIterator->second( 0 ) - radius * std::cos( currentAngle ) ), 1.0E-10 * radius ); + BOOST_CHECK_SMALL( + std::fabs( outputIterator->second( 1 ) - radius * std::sin( currentAngle ) ), 1.0E-10 * radius ); + BOOST_CHECK_SMALL( + std::fabs( outputIterator->second( 2 ) ), 1.0E-15 ); + BOOST_CHECK_SMALL( + std::fabs( outputIterator->second( 3 ) + circularVelocity * std::sin( currentAngle ) ), 1.0E-10 * circularVelocity ); + BOOST_CHECK_SMALL( + std::fabs( outputIterator->second( 4 ) - circularVelocity * std::cos( currentAngle ) ), 1.0E-10 * circularVelocity ); + BOOST_CHECK_SMALL( + std::fabs( outputIterator->second( 5 ) ), 1.0E-15 ); + } + } + else if( i == 1 ) + { + for( std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > >::const_iterator outputIterator = + numericalSolution.begin( ); outputIterator != numericalSolution.end( ); outputIterator++ ) + { + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ( -1.0 * thrustMagnitude / vehicleMass * outputIterator->second.segment( 3, 3 ).normalized( ) ), + ( dependentVariableSolution.at( outputIterator->first ) ), 1.0E-14 ); + + } + } + } + +} + +BOOST_AUTO_TEST_CASE( testThrustAccelerationFromExistingRotation ) +{ + using namespace tudat; + using namespace numerical_integrators; + using namespace simulation_setup; + using namespace basic_astrodynamics; + using namespace propagators; + using namespace basic_mathematics; + using namespace basic_astrodynamics; + + //Load spice kernels. + std::string kernelsPath = input_output::getSpiceKernelPath( ); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de-403-masses.tpc"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "de421.bsp"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "naif0009.tls"); + spice_interface::loadSpiceKernelInTudat( kernelsPath + "pck00009.tpc"); + + double thrustMagnitude = 1.0E3; + double specificImpulse = 250.0; + + // Create Earth object + simulation_setup::NamedBodyMap bodyMap; + + // Create vehicle objects. + double vehicleMass = 5.0E3; + bodyMap[ "Vehicle" ] = boost::make_shared< simulation_setup::Body >( ); + bodyMap[ "Vehicle" ]->setConstantBodyMass( vehicleMass ); + bodyMap[ "Vehicle" ]->setEphemeris( + boost::make_shared< ephemerides::TabulatedCartesianEphemeris< > >( + boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, basic_mathematics::Vector6d > >( ), + "Earth" ) ); + bodyMap[ "Vehicle" ]->setRotationalEphemeris( + boost::make_shared< ephemerides::SpiceRotationalEphemeris >( "ECLIPJ2000", "IAU_MOON" ) ); + bodyMap[ "Earth" ] = boost::make_shared< Body >( ); + + bodyMap[ "Earth" ]->setEphemeris( + boost::make_shared< ephemerides::SpiceEphemeris >( "Sun", "SSB", false, false ) ); + bodyMap[ "Earth" ]->setGravityFieldModel( boost::make_shared< gravitation::GravityFieldModel >( + spice_interface::getBodyGravitationalParameter( "Earth" ) ) ); + + // Finalize body creation. + setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" ); + + // Define propagator settings variables. + SelectedAccelerationMap accelerationMap; + std::vector< std::string > bodiesToPropagate; + std::vector< std::string > centralBodies; + + // Define acceleration model settings. + Eigen::Vector3d bodyFixedThrustDirection = ( Eigen::Vector3d( ) << 1.4, 3.1, -0.5 ).finished( ).normalized( ); + + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle; + accelerationsOfVehicle[ "Vehicle" ].push_back( boost::make_shared< ThrustAccelerationSettings >( + boost::make_shared< ThrustDirectionGuidanceSettings >( + thrust_direction_from_existing_body_orientation, "Earth" ), + boost::make_shared< ConstantThrustEngineSettings >( + thrustMagnitude, specificImpulse, bodyFixedThrustDirection ) ) ); + accelerationsOfVehicle[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) ); + + accelerationMap[ "Vehicle" ] = accelerationsOfVehicle; + + bodiesToPropagate.push_back( "Vehicle" ); + centralBodies.push_back( "Earth" ); + + // Set initial state + basic_mathematics::Vector6d systemInitialState = basic_mathematics::Vector6d::Zero( ); + + + systemInitialState( 0 ) = 8.0E6; + systemInitialState( 4 ) = 7.5E3; + + // Create acceleration models and propagation settings. + basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodyMap, accelerationMap, bodiesToPropagate, centralBodies ); + + boost::shared_ptr< DependentVariableSaveSettings > dependentVariableSaveSettings; + + + std::vector< boost::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables; + dependentVariables.push_back( + boost::make_shared< SingleAccelerationDependentVariableSaveSettings >( + thrust_acceleration, "Vehicle", "Vehicle", 0 ) ); + dependentVariableSaveSettings = boost::make_shared< DependentVariableSaveSettings >( dependentVariables ); + + + boost::shared_ptr< PropagationTimeTerminationSettings > terminationSettings = + boost::make_shared< propagators::PropagationTimeTerminationSettings >( 1000.0 ); + boost::shared_ptr< TranslationalStatePropagatorSettings< double > > translationalPropagatorSettings = + boost::make_shared< TranslationalStatePropagatorSettings< double > > + ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, terminationSettings, + cowell, dependentVariableSaveSettings ); + boost::shared_ptr< IntegratorSettings< > > integratorSettings = + boost::make_shared< IntegratorSettings< > > + ( rungeKutta4, 0.0, 2.5 ); + + // Create simulation object and propagate dynamics. + SingleArcDynamicsSimulator< > dynamicsSimulator( + bodyMap, integratorSettings, translationalPropagatorSettings, true, false, false ); + + // Retrieve numerical solutions for state and dependent variables + std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > > dependentVariableOutput = + dynamicsSimulator.getDependentVariableHistory( ); + + double thrustAcceleration = thrustMagnitude / vehicleMass; + Eigen::Quaterniond rotationToInertialFrame; + for( std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > >::const_iterator outputIterator = + dependentVariableOutput.begin( ); outputIterator != dependentVariableOutput.end( ); outputIterator++ ) + { + rotationToInertialFrame = bodyMap.at( "Vehicle" )->getRotationalEphemeris( )->getRotationToBaseFrame( + outputIterator->first ); + for( unsigned int i = 0; i < 3; i++ ) + { + BOOST_CHECK_CLOSE_FRACTION( + ( thrustAcceleration * ( rotationToInertialFrame * bodyFixedThrustDirection )( i ) ), + outputIterator->second( i ), 2.0E-15 ); + } + + } + +} + + + +BOOST_AUTO_TEST_SUITE_END( ) + +} // namespace unit_tests + +} // namespace tudat diff --git a/Tudat/Astrodynamics/Propulsion/massRateFromThrust.h b/Tudat/Astrodynamics/Propulsion/massRateFromThrust.h new file mode 100644 index 0000000000..c2c70dd77a --- /dev/null +++ b/Tudat/Astrodynamics/Propulsion/massRateFromThrust.h @@ -0,0 +1,105 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_FROMTHRUSTMASSRATEMODEL_H +#define TUDAT_FROMTHRUSTMASSRATEMODEL_H + +#include +#include +#include +#include + +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/massRateModel.h" +#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h" +#include "Tudat/Astrodynamics/Propulsion/thrustAccelerationModel.h" + +namespace tudat +{ +namespace propulsion +{ + +//! This class provides the mass rate from a (set of) thrust model(s), used in numerically propagating a body's total mass. +/*! + * This class provides the mass rate from a (set of) thrust model(s, used in numerically propagating a body's total mass. + * The mass rate is directly obtained from the thrust accelerations. This class can be used to sum the mass rates due to + * any number of thrust acceleration models. + */ +class FromThrustMassRateModel: public basic_astrodynamics::MassRateModel +{ +public: + + //! Constructor taking single acceleration model. + /*! + * Constructor taking single acceleration model. + * \param thrustAcceleration Thrust acceleration model from which the mass rate is to be retrieved. + */ + FromThrustMassRateModel( + const boost::shared_ptr< ThrustAcceleration > thrustAcceleration ) + { + thrustAccelerations_.push_back( thrustAcceleration ); + } + + //! Constructor taking list of acceleration models. + /*! + * Constructor taking list of acceleration models. + * \param thrustAccelerations List of thrust acceleration models from which the mass rates are to be retrieved and + * summed. + */ + /*! + * Constructor + */ + FromThrustMassRateModel( + const std::vector< boost::shared_ptr< ThrustAcceleration > > thrustAccelerations ): + thrustAccelerations_( thrustAccelerations ){ } + + //! Destructor. + ~FromThrustMassRateModel( ){ } + + //! Update member variables used by the mass rate model and compute the mass rate + /*! + * Update member variables used by the mass rate model and compute the mass rate. + * Updates the thrusta acceleration models and sums the resulting mass rates, updating the currentMassRate_ variable. + * \param currentTime Time at which mass rate model is to be updated. + */ + void updateMembers( const double currentTime = TUDAT_NAN ) + { + // Check if update is needed. + if( currentTime != currentTime_ ) + { + // Update thrust accelerations + for( unsigned int i = 0; i < thrustAccelerations_.size( ); i++ ) + { + thrustAccelerations_.at( i )->updateMembers( currentTime ); + } + + // Compute total mass rate + currentMassRate_ = 0.0; + for( unsigned int i = 0; i < thrustAccelerations_.size( ); i++ ) + { + currentMassRate_ += thrustAccelerations_.at( i )->getCurrentMassRate( ); + } + + currentTime_ = currentTime; + } + } + +private: + + //! List of thrust accelerations from which the total mass rate is to be computed. + std::vector< boost::shared_ptr< ThrustAcceleration > > thrustAccelerations_; +}; + +} // namespace propulsion + +} // namespace tudat + +#endif // TUDAT_FROMTHRUSTMASSRATEMODEL_H diff --git a/Tudat/Astrodynamics/Propulsion/thrustAccelerationModel.h b/Tudat/Astrodynamics/Propulsion/thrustAccelerationModel.h new file mode 100644 index 0000000000..3b0f9d4a92 --- /dev/null +++ b/Tudat/Astrodynamics/Propulsion/thrustAccelerationModel.h @@ -0,0 +1,229 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_THRUSTACCELERATIONMODEL_H +#define TUDAT_THRUSTACCELERATIONMODEL_H + +#include + +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/massRateModel.h" +#include "Tudat/Astrodynamics/Propagators/environmentUpdateTypes.h" +#include "Tudat/Astrodynamics/Propulsion/thrustGuidance.h" +#include "Tudat/Astrodynamics/Propulsion/thrustMagnitudeWrapper.h" + +namespace tudat +{ + +namespace propulsion +{ + +//! Class used for computing an acceleration due to a continuous thrust. +/*! + * Class used for computing an acceleration due to a continuous thrust. The thrust magnitude and direction (in the + * propagation frame) are retrieved from separate functions provided by tye user. + */ +class ThrustAcceleration : public basic_astrodynamics::AccelerationModel< Eigen::Vector3d > +{ +public: + + /*! + * Constructor. + * \param thrustMagnitudeFunction Function returning the current magnitude of the thrust. Any dependencies of the + * thrust on (in)dependent variables is to be handled by the thrustUpdateFunction. + * \param thrustDirectionFunction Function returning the direction of the thrust (as a unit vector). + * Any dependencies of the thrust on (in)dependent variables is to be handled by the thrustUpdateFunction. + * \param bodyMassFunction Function returning the current mass of the body being propagated. + * \param massRateFunction Function returning total propellant mass rate from the thrust system. + * \param associatedThroustSource ID associated with the source of the thrust (i.e. engine name). + * \param thrustUpdateFunction Function used to update the thrust magnitude and direction to current time (default empty) + * \param timeResetFunction Function to reset the time in the classes to which the thrustUpdateFunction function directs, + * default empty. + * \param requiredModelUpdates List of environment models that are to be updated before computing the acceleration, + * list is included here to account for versatility of dependencies of thrust model (guidance) algorithms. Default empty. + */ + ThrustAcceleration( + const boost::function< double( ) > thrustMagnitudeFunction, + const boost::function< Eigen::Vector3d( ) > thrustDirectionFunction, + const boost::function< double( ) > bodyMassFunction, + const boost::function< double( ) > massRateFunction, + const std::string associatedThroustSource = "", + const boost::function< void( const double ) > thrustUpdateFunction = boost::function< void( const double ) >( ), + const boost::function< void( const double ) > timeResetFunction = boost::function< void( const double ) >( ), + const std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > >& requiredModelUpdates = + std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > >( ) ): + AccelerationModel< Eigen::Vector3d >( ), + thrustMagnitudeFunction_( thrustMagnitudeFunction ), + thrustDirectionFunction_( thrustDirectionFunction ), + bodyMassFunction_( bodyMassFunction ), + massRateFunction_( massRateFunction ), + associatedThroustSource_( associatedThroustSource ), + thrustUpdateFunction_( thrustUpdateFunction ), + timeResetFunction_( timeResetFunction ), + requiredModelUpdates_( requiredModelUpdates ){ } + + //! Destructor + ~ThrustAcceleration( ){ } + + //! Function to retrieve the current acceleration, as set by last call to updateMembers function. + /*! + * Function to retrieve the current acceleration, as set by last call to updateMembers function. + * \return Current thrust acceleration. + */ + Eigen::Vector3d getAcceleration( ) + { + return currentAcceleration_; + } + + //! Function to reset the current time + /*! + * Function to reset the current time of the acceleration model. + * \param currentTime Current time (default NaN). + */ + virtual void resetTime( const double currentTime = TUDAT_NAN ) + { + currentTime_ = currentTime; + + if( !timeResetFunction_.empty( ) ) + { + timeResetFunction_( currentTime_ ); + } + } + + //! Update member variables used by the thrust acceleration model. + /*! + * Updates member variables used by the thrust acceleration model. + * Function pointers to retrieve the current values of quantities from which the + * acceleration is to be calculated are set by constructor. This function calls and combines their output to + * compute the acceleration. + * \param currentTime Time at which acceleration model is to be updated. + */ + void updateMembers( const double currentTime = TUDAT_NAN ) + { + // Check if update is needed + if( !( currentTime_ == currentTime ) ) + { + // Update thrust dependencies if needed + if( !thrustUpdateFunction_.empty( ) ) + { + thrustUpdateFunction_( currentTime ); + } + + // Retrieve thrust direction. + currentAccelerationDirection_ = thrustDirectionFunction_( ); + + if( ( std::fabs( currentAccelerationDirection_.norm( ) ) - 1.0 ) > + 10.0 * std::numeric_limits< double >::epsilon( ) ) + { + throw std::runtime_error( "Error in thrust acceleration, direction is not a unit vector" ); + } + + // Retrieve magnitude of thrust and mass rate. + currentThrustMagnitude_ = thrustMagnitudeFunction_( ); + currentMassRate_ = -massRateFunction_( ); + + // Compute acceleration due to thrust + currentAcceleration_ = currentAccelerationDirection_ * currentThrustMagnitude_ / bodyMassFunction_( ); + + // Reset current time. + currentTime_ = currentTime; + } + + } + + //! Function to retreieve the current propellant mass rate, as computed by last call to updateMembers function. + /*! + * Function to retreieve the current propellant mass rate, as computed by last call to updateMembers function. + * \return ICurrent propellant mass rate, as computed by last call to updateMembers function. + */ + double getCurrentMassRate( ) + { + return currentMassRate_; + } + + //! Function to retreieve the ID associated with the source of the thrust (i.e. engine name). + /*! + * Function to retreieve the ID associated with the source of the thrust (i.e. engine name). + * \return ID associated with the source of the thrust (i.e. engine name). + */ + std::string getAssociatedThroustSource( ) + { + return associatedThroustSource_; + } + + //! Function to retreieve the list of environment models that are to be updated before computing the acceleration. + /*! + * Function to retreieve the list of environment models that are to be updated before computing the acceleration. + * \return List of environment models that are to be updated before computing the acceleration. + */ + std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > getRequiredModelUpdates( ) + { + return requiredModelUpdates_; + } + + +protected: + + //! Function returning the current magnitude of the thrust. + /*! + * Function returning the current magnitude of the thrust. Any dependencies of the + * thrust on (in)dependent variables is to be handled by the thrustUpdateFunction. + */ + boost::function< double( ) > thrustMagnitudeFunction_; + + //! Function returning the direction of the thrust (as a unit vector). + /*! + * Function returning the direction of the thrust (as a unit vector). Any dependencies of the + * thrust on (in)dependent variables is to be handled by the thrustUpdateFunction. + */ + boost::function< Eigen::Vector3d( ) > thrustDirectionFunction_; + + //! Function returning the current mass of the body being propagated. + boost::function< double( ) > bodyMassFunction_; + + //! Function returning total propellant mass rate from the thrust system. + boost::function< double( ) > massRateFunction_; + + //! ID associated with the source of the thrust (i.e. engine name). + std::string associatedThroustSource_; + + //! Function used to update the thrust magnitude and direction to current time. + boost::function< void( const double ) > thrustUpdateFunction_; + + //! Current acceleration, as computed by last call to updateMembers function. + Eigen::Vector3d currentAcceleration_; + + //! Current acceleration direction, as computed by last call to updateMembers function. + Eigen::Vector3d currentAccelerationDirection_; + + //! Current thrust magnitude, as computed by last call to updateMembers function. + double currentThrustMagnitude_; + + //! Current propellant mass rate, as computed by last call to updateMembers function. + double currentMassRate_; + + //! Function to reset the time in the classes to which the thrustUpdateFunction function directs + const boost::function< void( const double ) > timeResetFunction_; + + //! List of environment models that are to be updated before computing the acceleration, + /*! + * List of environment models that are to be updated before computing the acceleration, + * list is included here to account for versatility of dependencies of thrust model (guidance) algorithms. + */ + std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > requiredModelUpdates_; +}; + +} // namespace propulsion + +} // namespace tudat + +#endif // TUDAT_THRUSTACCELERATIONMODEL_H diff --git a/Tudat/Astrodynamics/Propulsion/thrustFunctions.cpp b/Tudat/Astrodynamics/Propulsion/thrustFunctions.cpp new file mode 100644 index 0000000000..b3c30fe40b --- /dev/null +++ b/Tudat/Astrodynamics/Propulsion/thrustFunctions.cpp @@ -0,0 +1,39 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + + +#include "Tudat/Astrodynamics/Propulsion/thrustFunctions.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h" + +namespace tudat +{ + +namespace propulsion +{ + +//! Function to compute engine thrust from propellant mass rate and specific impulse +double computeThrustFromSpecificImpulse( + const double propellantMassRate, const double specificImpulse ) +{ + return physical_constants::SEA_LEVEL_GRAVITATIONAL_ACCELERATION * specificImpulse * propellantMassRate; +} + +//! Function to compute propellant mass rate from engine thrust and specific impulse +double computePropellantMassRateFromSpecificImpulse( + const double thrustMagnitude, const double specificImpulse ) +{ + return thrustMagnitude / ( physical_constants::SEA_LEVEL_GRAVITATIONAL_ACCELERATION * specificImpulse ); +} + + +} // namespace propulsion + +} // namespace tudat + diff --git a/Tudat/Astrodynamics/Propulsion/thrustFunctions.h b/Tudat/Astrodynamics/Propulsion/thrustFunctions.h new file mode 100644 index 0000000000..6c61d27c7d --- /dev/null +++ b/Tudat/Astrodynamics/Propulsion/thrustFunctions.h @@ -0,0 +1,46 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_THRUSTFUNCTIONS_H +#define TUDAT_THRUSTFUNCTIONS_H + +namespace tudat +{ + +namespace propulsion +{ + +//! Function to compute engine thrust from propellant mass rate and specific impulse +/*! + * Function to compute engine thrust from propellant mass rate and specific impulse + * \param propellantMassRate Propellant mass rate + * \param specificImpulse Specific impulse (normalized with g0 from physical_constants namespace) + * \return Total engine thrust + */ +double computeThrustFromSpecificImpulse( + const double propellantMassRate, const double specificImpulse ); + +//! Function to compute propellant mass rate from engine thrust and specific impulse +/*! + * Function to compute propellant mass rate from engine thrust and specific impulse + * \param thrustMagnitude Total engine thrust + * \param specificImpulse Specific impulse (normalized with g0 from physical_constants namespace) + * \return Propellant mass rate + */ +double computePropellantMassRateFromSpecificImpulse( + const double thrustMagnitude, const double specificImpulse ); + + +} // namespace propulsion + +} // namespace tudat + + +#endif // TUDAT_THRUSTFUNCTIONS_H diff --git a/Tudat/Astrodynamics/Propulsion/thrustGuidance.cpp b/Tudat/Astrodynamics/Propulsion/thrustGuidance.cpp new file mode 100644 index 0000000000..a1b57b9c54 --- /dev/null +++ b/Tudat/Astrodynamics/Propulsion/thrustGuidance.cpp @@ -0,0 +1,50 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/Astrodynamics/Propulsion/thrustGuidance.h" + +namespace tudat +{ + +namespace propulsion +{ + +//! Function to get the unit vector colinear with velocity segment of a translational state. +Eigen::Vector3d getForceDirectionColinearWithVelocity( + const boost::function< void( basic_mathematics::Vector6d& ) > currentStateFunction, + const double currentTime, const bool putForceInOppositeDirection ) +{ + static basic_mathematics::Vector6d currentState; + currentStateFunction( currentState ); + return ( ( putForceInOppositeDirection == 1 ) ? -1.0 : 1.0 ) * ( currentState.segment( 3, 3 ) ).normalized( ); +} + +//! Function to get the unit vector colinear with position segment of a translational state. +Eigen::Vector3d getForceDirectionColinearWithPosition( + const boost::function< void( basic_mathematics::Vector6d& ) > currentStateFunction, + const double currentTime, const bool putForceInOppositeDirection ) +{ + static basic_mathematics::Vector6d currentState; + currentStateFunction( currentState ); + return ( ( putForceInOppositeDirection == 1 ) ? -1.0 : 1.0 ) * ( currentState.segment( 0, 3 ) ).normalized( ); +} + +//! Function to get the force direction from a time-only function. +Eigen::Vector3d getForceDirectionFromTimeOnlyFunction( + const double currentTime, + const boost::function< Eigen::Vector3d( const double ) > timeOnlyFunction ) +{ + return timeOnlyFunction( currentTime ).normalized( ); +} + +} // namespace propulsion + +} // namespace tudat + diff --git a/Tudat/Astrodynamics/Propulsion/thrustGuidance.h b/Tudat/Astrodynamics/Propulsion/thrustGuidance.h new file mode 100644 index 0000000000..fcc4093d93 --- /dev/null +++ b/Tudat/Astrodynamics/Propulsion/thrustGuidance.h @@ -0,0 +1,284 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + + +#ifndef TUDAT_THRUSTGUIDANCE_H +#define TUDAT_THRUSTGUIDANCE_H + +#include +#include + +#include +#include "Tudat/Astrodynamics/ReferenceFrames/dependentOrientationCalculator.h" +#include + +namespace tudat +{ + +namespace propulsion +{ + +//! Base class for computing the direction of a given force/acceleration +/*! + * Base class for computing the direction of a given force/acceleration. The computation is done directly from some + * a priori imposed rule, possibly as a function of dependent, independent or state variables. + * A derived class for using the current orientation of the vehicle (as computed by some other method and + * retrieved from the body class) may be set using the OrientationBasedForceGuidance class. + */ +class BodyFixedForceDirectionGuidance : public reference_frames::DependentOrientationCalculator +{ +public: + + //! Constructor + /*! + * Constructor, sets the direction of the force in the body-fixed frame. + * \param bodyFixedForceDirection Function returning the direction of the force in the body-fixed frame. + */ + BodyFixedForceDirectionGuidance ( + const boost::function< Eigen::Vector3d( ) > bodyFixedForceDirection ): + DependentOrientationCalculator( ), bodyFixedForceDirection_( bodyFixedForceDirection ){ } + + //! Destructor + virtual ~BodyFixedForceDirectionGuidance ( ){ } + + //! Function to get the force/acceleration direction in the propagation frame + /*! + * Function to get the force/acceleration direction in the propagation frame. Function is pure virtual and to be + * implemented in derived class. + * \return Direction of force (as unit vector) expressed in propagation frame. + */ + virtual Eigen::Vector3d getCurrentForceDirectionInPropagationFrame( ) = 0; + + //! Function to compute the rotation from the propagation frame to the body-fixed frame + /*! + * Function to compute the rotation from the propagation frame to the body-fixed frame using the algorithm implemented + * in the derived class. The derived class must implement the function for the inverse rotation + * (getRotationToGlobalFrame). + * \return Quaternion that provides the rotation from the propagation frame to the body-fixed frame. + */ + Eigen::Quaterniond getRotationToLocalFrame( ) + { + return getRotationToGlobalFrame( ).inverse( ); + } + + //! Function to update the object to the current time. + /*! + * Function to update the object to the current time. This function updates only this base class, derived class + * must implement the updateForceDirection function that obdates the full object. + * \param time Time to which object is to be updated. + */ + void updateCalculator( const double time ) + { + currentBodyFixedForceDirection_ = ( bodyFixedForceDirection_( ) ).normalized( ); + updateForceDirection( time ); + } + +protected: + + //! Function to update the force/acceleration direction to the current time. + /*! + * Function to update the force/acceleration direction to the current time. This function is to be implemented in the + * derived class. + * \param time Time to which object is to be updated. + */ + virtual void updateForceDirection( const double time ) = 0; + + //! Function returning the direction of the force in the body-fixed frame. + boost::function< Eigen::Vector3d( ) > bodyFixedForceDirection_; + + //! Current direction of the force in the body-fixed frame as set by last call to updateCalculator function. + Eigen::Vector3d currentBodyFixedForceDirection_; +}; + + +//! Function to get the unit vector colinear with velocity segment of a translational state. +/*! + * Function to get the unit vector colinear with velocity segment of a translational state. + * \param currentStateFunction Function returning (by reference) translational Cartesian state from which the unit velocity + * vector is to be retrieved. + * \param currentTime Time at which computation is to be done (not used here; included for interface compatibility). + * \param putForceInOppositeDirection Boolean denoting whether the output vector should be in opposite (if true) or same + * direction (if false) as velocity segment of currentState + * \return Unit vector colinear with velocity segment of currentState. + */ +Eigen::Vector3d getForceDirectionColinearWithVelocity( + const boost::function< void( basic_mathematics::Vector6d& ) > currentStateFunction, + const double currentTime, const bool putForceInOppositeDirection ); + +//! Function to get the unit vector colinear with position segment of a translational state. +/*! + * Function to get the unit vector colinear with position segment of a translational state. + * \param currentStateFunction Function returning (by reference) translational Cartesian state from which the unit position + * vector is to be retrieved. + * \param currentTime Time at which computation is to be done (not used here; included for interface compatibility). + * \param putForceInOppositeDirection Boolean denoting whether the output vector should be in opposite (if true) or same + * direction (if false) as position segment of current state + * \return Unit vector colinear with position segment of current state. + */ +Eigen::Vector3d getForceDirectionColinearWithPosition( + const boost::function< void( basic_mathematics::Vector6d& ) > currentStateFunction, + const double currentTime, const bool putForceInOppositeDirection ); + +//! Function to get the force direction from a time-only function. +/*! + * Function to get the force direction from a time-only function. + * \param currentTime Current time. + * \param timeOnlyFunction Function returning unit vector (thrust direction) as a funtion of time. + * \return Thrust direction. + */ +Eigen::Vector3d getForceDirectionFromTimeOnlyFunction( + const double currentTime, + const boost::function< Eigen::Vector3d( const double ) > timeOnlyFunction ); + +//! Class for computing the force direction directly from a function returning the associated unit vector of direction. +class DirectionBasedForceGuidance: public BodyFixedForceDirectionGuidance +{ +public: + + //! Constructor + /*! + * Constructor + * \param forceDirectionFunction Function returning thrust-direction (represented in the relevant propagation frame) + * as a function of time. + * \param centralBody Name of central body + * \param bodyFixedForceDirection Function returning the unit-vector of the force direction in a body-fixed frame (e.g. + * engine thrust pointing in body-fixed frame). + */ + DirectionBasedForceGuidance( + const boost::function< Eigen::Vector3d( const double ) > forceDirectionFunction, + const std::string& centralBody, + const boost::function< Eigen::Vector3d( ) > bodyFixedForceDirection = + boost::lambda::constant( Eigen::Vector3d::UnitX( ) ) ): + BodyFixedForceDirectionGuidance ( bodyFixedForceDirection ), + forceDirectionFunction_( forceDirectionFunction ), + centralBody_( centralBody ){ } + + //! Function returning the current force direction, as computed by last call to updateCalculator/updateForceDirection. + /*! + * Function returning the current force direction, as computed by last call to updateCalculator/updateForceDirection. + * \return Current force direction, expressed in propagation frame. + */ + Eigen::Vector3d getCurrentForceDirectionInPropagationFrame( ) + { + return currentForceDirection_; + } + + //! Function to get the rotation from body-fixed to inertial frame. + /*! + * Function to get the rotation from body-fixed to inertial frame. NOT YET IMPLEMENTED IN THIS DERIVED CLASS. + * \return NOT YET IMPLEMENTED IN THIS DERIVED CLASS. + */ + Eigen::Quaterniond getRotationToGlobalFrame( ) + { + throw std::runtime_error( "Error, body-fixed frame to propagation frame not yet implemented for DirectionBasedForceGuidance." ); + } + + //! Function to return the name of the central body. + /*! + * Function to return the name of the central body. + * \return Name of the central body. + */ + std::string getCentralBody( ) + { + return centralBody_; + } + +protected: + + //! Function to update the force direction to the current time. + /*! + * Function to update the force direction to the current time. + * \param time Time to which object is to be updated. + */ + void updateForceDirection( const double time ) + { + currentForceDirection_ = forceDirectionFunction_( time ).normalized( ); + } + + //! Function returning thrust-direction (represented in the relevant propagation frame) as a function of time. + boost::function< Eigen::Vector3d( const double ) > forceDirectionFunction_; + + //! Current force direction, as computed by last call to updateCalculator/updateForceDirection. + Eigen::Vector3d currentForceDirection_; + + //! Name of central body + std::string centralBody_; + +}; + +//! Class for computing the force direction using the rotation matrix from the propagation to the body-fixed frame. +class OrientationBasedForceGuidance: public BodyFixedForceDirectionGuidance +{ +public: + + //! Constructor + /*! + * Constructor + * \param bodyFixedFrameToBaseFrameFunction Function returning rotation from the body-fixed frame to the relevant + * propagation frame as a function of time. + * \param bodyFixedForceDirection Function returning the unit-vector of the force direction in a body-fixed frame (e.g. + * engine thrust pointing in body-fixed frame). + */ + OrientationBasedForceGuidance( + const boost::function< Eigen::Quaterniond( const double ) > bodyFixedFrameToBaseFrameFunction, + const boost::function< Eigen::Vector3d( ) > bodyFixedForceDirection = + boost::lambda::constant( Eigen::Vector3d::UnitX( ) ) ): + BodyFixedForceDirectionGuidance ( bodyFixedForceDirection ), + bodyFixedFrameToBaseFrameFunction_( bodyFixedFrameToBaseFrameFunction ){ } + + //! Function returning the current force direction, as computed by last call to updateCalculator/updateForceDirection. + /*! + * Function returning the current force direction, as computed by last call to updateCalculator/updateForceDirection. + * \return Current force direction, expressed in propagation frame. + */ + Eigen::Vector3d getCurrentForceDirectionInPropagationFrame( ) + { + return ( getRotationToGlobalFrame( ) * currentBodyFixedForceDirection_ ).normalized( ); + } + + //! Function to get the rotation from body-fixed to inertial frame. + /*! + * Function to get the rotation from body-fixed to inertial frame. For thsi derived class, this simply means returning + * the currentBodyFixedFrameToBaseFrame_ rotation. + * \return The rotation from body-fixed to inertial frame. + */ + Eigen::Quaterniond getRotationToGlobalFrame( ) + { + return currentBodyFixedFrameToBaseFrame_ ; + } + +protected: + + //! Function to update the force direction to the current time. + /*! + * Function to update the force direction to the current time. + * \param time Time to which object is to be updated. + */ + void updateForceDirection( const double time ) + { + currentBodyFixedFrameToBaseFrame_ = bodyFixedFrameToBaseFrameFunction_( time ); + } + + //! Function returning rotation from the body-fixed frame to the relevant propagation frame as a function of time. + boost::function< Eigen::Quaterniond( const double ) > bodyFixedFrameToBaseFrameFunction_; + + //! The rotation from body-fixed to inertial frame. + Eigen::Quaterniond currentBodyFixedFrameToBaseFrame_; + +}; + + + +} // namespace propulsion + +} // namespace tudat + + +#endif // TUDAT_THRUSTGUIDANCE_H diff --git a/Tudat/Astrodynamics/Propulsion/thrustMagnitudeWrapper.h b/Tudat/Astrodynamics/Propulsion/thrustMagnitudeWrapper.h new file mode 100644 index 0000000000..c128f9decb --- /dev/null +++ b/Tudat/Astrodynamics/Propulsion/thrustMagnitudeWrapper.h @@ -0,0 +1,273 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_THRUSTMAGNITUDEWRAPPER_H +#define TUDAT_THRUSTMAGNITUDEWRAPPER_H + +#include +#include +#include + +#include "Tudat/Astrodynamics/SystemModels/engineModel.h" + +namespace tudat +{ + +namespace propulsion +{ + +//! Base class for determining the current thrust magnitude from method defined in derived class. +class ThrustMagnitudeWrapper +{ +public: + + //! Constructor + ThrustMagnitudeWrapper( ): + currentTime_( TUDAT_NAN ){ } + + //! Destructor. + virtual ~ThrustMagnitudeWrapper( ){ } + + //! Pure virtual function to update the thrust magnitude to the current time. + /*! + * Pure virtual function to update the thrust magnitude to the current time. Method of computation is to be + * implemented in the derived class. + * \param time Time at which the magitude is to be computed. + */ + virtual void update( const double time ) = 0; + + //! Pure virtual function to get the current thrust magnitude. + /*! + * Pure virtual function to get the current thrust magnitude. + * \return Current thrust magnitude. + */ + virtual double getCurrentThrustMagnitude( ) = 0; + + //! Pure virtual function to get the current mass rate. + /*! + * Pure virtual function to get the current mass rate. + * \return Current mass rate. + */ + virtual double getCurrentMassRate( ) = 0; + + //! Function to reset the current time of the thrust model + /*! + * Function to reset the current time of the thrust model. Function is typically used to reset the time to NaN, + * signalling the need for a recomputation of all required quantities. + * \param currentTime New current time to be set in model. + */ + void resetCurrentTime( const double currentTime = TUDAT_NAN ) + { + currentTime_ = currentTime; + } + +protected: + + //! Current time for model. + double currentTime_; +}; + +//! Class for custom computations of thrust magnitude and mass rate. +/*! + * Class for custom computations of thrust magnitude and mass rate. Both the magnitude and specific impulse + * are parameterized as a function of time and provided as input, providing a flexible interface for user-defined + * thrust magnitude profiles. + */ +class CustomThrustMagnitudeWrapper: public ThrustMagnitudeWrapper +{ +public: + + //! Constructor + /*! + * Constructor + * \param thrustMagnitudeFunction Function returning thrust as a function of time. + * \param specificImpulseFunction Function returning specific impulse as a function of time. + * \param isEngineOnFunction Function returning whether the function is on (returns true if so) at a given time. + */ + CustomThrustMagnitudeWrapper( + const boost::function< double( const double ) > thrustMagnitudeFunction, + const boost::function< double( const double ) > specificImpulseFunction, + const boost::function< bool( const double ) > isEngineOnFunction = boost::lambda::constant( true ) ): + thrustMagnitudeFunction_( thrustMagnitudeFunction ), + specificImpulseFunction_( specificImpulseFunction ), + isEngineOnFunction_( isEngineOnFunction ), + currentThrustMagnitude_( TUDAT_NAN ), + currentSpecificImpulse_( TUDAT_NAN ){ } + + //! Destructor. + ~CustomThrustMagnitudeWrapper( ){ } + + //! Function to update the thrust magnitude to the current time. + /*! + * Function to update the thrust magnitude to the current time. + * \param time Time to which the model is to be updated. + */ + void update( const double time ) + { + if( !( currentTime_ = time ) ) + { + // If engine is one, update engine. + if( isEngineOnFunction_( time ) ) + { + currentThrustMagnitude_ = thrustMagnitudeFunction_( time ); + currentSpecificImpulse_ = specificImpulseFunction_( time ); + } + // If engine is off, set thrust to zero. + else + { + currentThrustMagnitude_ = 0.0; + currentSpecificImpulse_ = TUDAT_NAN; + } + } + } + + //! Function to return the current thrust magnitude + /*! + * Function to return the current thrust magnitude, as computed by last call to update member function. + * \return Current thrust magnitude + */ + double getCurrentThrustMagnitude( ) + { + return currentThrustMagnitude_; + } + + //! Function to return the current mass rate. + /*! + * Function to return the current mass rate, computed from quantities set by last call to update member function. + * \return Current mass rate. + */ + double getCurrentMassRate( ) + { + if( currentThrustMagnitude_ != 0.0 ) + { + return propulsion::computePropellantMassRateFromSpecificImpulse( + currentThrustMagnitude_, currentSpecificImpulse_ ); + } + else + { + return 0.0; + } + } + +private: + + //! Function returning thrust as a function of time.. + boost::function< double( const double ) > thrustMagnitudeFunction_; + + //! Function returning specific impulse as a function of time. + boost::function< double( const double ) > specificImpulseFunction_; + + //! Function returning whether the function is on (returns true if so) at a given time. + boost::function< bool( const double ) > isEngineOnFunction_; + + //! Current thrust magnitude, as computed by last call to update member function. + double currentThrustMagnitude_; + + //! Current specific impulse, as computed by last call to update member function. + double currentSpecificImpulse_; + +}; + +//! Class to compute the engine thrust and mass rate from EngineModel object(s). +class ThrustMagnitudeFromEngineWrapper: public ThrustMagnitudeWrapper +{ +public: + + //! Constructor for single engine used for thrust. + /*! + * Constructor for single engine used for thrust + * \param engineModel Engine model used to compute thrust/mass rate. + */ + ThrustMagnitudeFromEngineWrapper( + const boost::shared_ptr< system_models::EngineModel > engineModel ): + currentThrust_( TUDAT_NAN ), currentMassRate_( TUDAT_NAN ) + { + engineModels_.push_back( engineModel ); + } + + //! Constructor for multiple engines used for thrust. + /*! + * Constructor for multiple engines used for thrust + * \param engineModels List of engine models used to compute thrust/mass rate. + */ + ThrustMagnitudeFromEngineWrapper( + const std::vector< boost::shared_ptr< system_models::EngineModel > > engineModels ): + engineModels_( engineModels ), currentThrust_( TUDAT_NAN ), currentMassRate_( TUDAT_NAN ) + { } + + //! Destructor. + ~ThrustMagnitudeFromEngineWrapper( ){ } + + + //! Function to update the thrust magnitude to the current time from the engine models. + /*! + * Function to update the thrust magnitude to the current time from the engine models + * \param time Time to which the model is to be updated. + */ + void update( const double time ) + { + if( !( currentTime_ = time ) ) + { + currentThrust_ = 0.0; + currentMassRate_ = 0.0; + + // Update engine models + for( unsigned int i = 0; i < engineModels_.size( ); i++ ) + { + engineModels_.at( i )->updateEngineModel( time ); + } + + // Add thrust and mass rate. + for( unsigned int i = 0; i < engineModels_.size( ); i++ ) + { + currentThrust_ += engineModels_.at( i )->getCurrentThrust( ); + currentMassRate_ += engineModels_.at( i )->getCurrentMassRate( ); + + } + } + } + + //! Function to return the current thrust magnitude + /*! + * Function to return the current thrust magnitude, as computed by last call to update member function. + * \return Current thrust magnitude + */ + double getCurrentThrustMagnitude( ) + { + return currentThrust_; + } + + //! Function to return the current mass rate + /*! + * Function to return the current mass rate, as computed by last call to update member function. + * \return Current mass rate + */ + double getCurrentMassRate( ) + { + return currentMassRate_; + } + +protected: + + //! List of engine models used to compute thrust/mass rate. + std::vector< boost::shared_ptr< system_models::EngineModel > > engineModels_; + + //! Current thrust magnitude, as computed by last call to update member function. + double currentThrust_; + + //! Current mass rate, as computed by last call to update member function. + double currentMassRate_; +}; + +} // namespace propulsion + +} // namespace tudat + +#endif // TUDAT_THRUSTMAGNITUDEWRAPPER_H diff --git a/Tudat/Astrodynamics/ReferenceFrames/CMakeLists.txt b/Tudat/Astrodynamics/ReferenceFrames/CMakeLists.txt index bc32dc19db..e5787d6ff1 100755 --- a/Tudat/Astrodynamics/ReferenceFrames/CMakeLists.txt +++ b/Tudat/Astrodynamics/ReferenceFrames/CMakeLists.txt @@ -45,6 +45,7 @@ set(REFERENCEFRAMES_HEADERS "${SRCROOT}${REFERENCEFRAMESDIR}/referenceFrameTransformations.h" "${SRCROOT}${REFERENCEFRAMESDIR}/apparentAccelerationModel.h" "${SRCROOT}${REFERENCEFRAMESDIR}/aerodynamicAngleCalculator.h" + "${SRCROOT}${REFERENCEFRAMESDIR}/dependentOrientationCalculator.h" ) # Add static libraries. @@ -62,4 +63,5 @@ target_link_libraries(test_ApparentAccelerationModel tudat_reference_frames ${Bo add_executable(test_AerodynamicAngleCalculator "${SRCROOT}${REFERENCEFRAMESDIR}/UnitTests/unitTestAerodynamicAngleCalculator.cpp") setup_custom_test_program(test_AerodynamicAngleCalculator "${SRCROOT}${REFERENCEFRAMESDIR}") -target_link_libraries(test_AerodynamicAngleCalculator tudat_reference_frames tudat_basic_astrodynamics tudat_basic_mathematics ${Boost_LIBRARIES}) +target_link_libraries(test_AerodynamicAngleCalculator tudat_simulation_setup tudat_propagators tudat_aerodynamics tudat_geometric_shapes tudat_gravitation tudat_electro_magnetism tudat_propulsion tudat_ephemerides tudat_numerical_integrators tudat_reference_frames tudat_basic_astrodynamics tudat_input_output tudat_basic_mathematics tudat_spice_interface cspice ${Boost_LIBRARIES}) + diff --git a/Tudat/Astrodynamics/ReferenceFrames/UnitTests/unitTestAerodynamicAngleCalculator.cpp b/Tudat/Astrodynamics/ReferenceFrames/UnitTests/unitTestAerodynamicAngleCalculator.cpp index 3b9aa82d4b..7c6d698df5 100644 --- a/Tudat/Astrodynamics/ReferenceFrames/UnitTests/unitTestAerodynamicAngleCalculator.cpp +++ b/Tudat/Astrodynamics/ReferenceFrames/UnitTests/unitTestAerodynamicAngleCalculator.cpp @@ -10,7 +10,13 @@ #include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h" #include "Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.h" -#include "Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h" +#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h" +#include +#include +#include +#include +#include +#include namespace tudat { @@ -48,12 +54,13 @@ void testAerodynamicAngleCalculation( // Create angle calculator AerodynamicAngleCalculator aerodynamicAngleCalculator( boost::lambda::constant( testState ), + boost::lambda::constant( Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ) ), "", 1, boost::lambda::constant( angleOfAttack ), boost::lambda::constant( angleOfSideslip), - boost::lambda::constant( bankAngle ), 1 ); + boost::lambda::constant( bankAngle ) ); // Update angle calculator. - aerodynamicAngleCalculator.update( ); + aerodynamicAngleCalculator.update( 0.0, true ); // Compare expected against computed angles. BOOST_CHECK_SMALL( @@ -216,11 +223,9 @@ BOOST_AUTO_TEST_CASE( testAerodynamicAngleCalculator ) testLatitude, testLongitude, angleOfAttack, angleOfSideslip, bankAngle ); } - - - } + BOOST_AUTO_TEST_SUITE_END( ) } // namespace unit_tests diff --git a/Tudat/Astrodynamics/ReferenceFrames/UnitTests/unitTestReferenceFrameTransformations.cpp b/Tudat/Astrodynamics/ReferenceFrames/UnitTests/unitTestReferenceFrameTransformations.cpp index 3d73b34281..e98977747a 100644 --- a/Tudat/Astrodynamics/ReferenceFrames/UnitTests/unitTestReferenceFrameTransformations.cpp +++ b/Tudat/Astrodynamics/ReferenceFrames/UnitTests/unitTestReferenceFrameTransformations.cpp @@ -617,6 +617,26 @@ BOOST_AUTO_TEST_CASE( testAerodynamicToBodyFrameTransformations ) } } +BOOST_AUTO_TEST_CASE( testEulerAngleRetrieval ) +{ + const double angleX = 2.1; + const double angleZ = -0.3; + const double angleY = 1.45; + + Eigen::Matrix3d rotationMatrix = + ( Eigen::AngleAxisd( -angleX, Eigen::Vector3d::UnitX( ) ) * + Eigen::AngleAxisd( -angleZ, Eigen::Vector3d::UnitZ( ) ) * + Eigen::AngleAxisd( -angleY, Eigen::Vector3d::UnitY( ) ) ).toRotationMatrix( ); + + Eigen::Vector3d eulerAngles = reference_frames::get132EulerAnglesFromRotationMatrix( + rotationMatrix ); + + BOOST_CHECK_CLOSE_FRACTION( angleX, eulerAngles( 0 ), 1.0E-15 ); + BOOST_CHECK_CLOSE_FRACTION( angleZ, eulerAngles( 1 ), 1.0E-15 ); + BOOST_CHECK_CLOSE_FRACTION( angleY, eulerAngles( 2 ), 1.0E-15 ); + +} + BOOST_AUTO_TEST_SUITE_END( ) } // namespace unit_tests diff --git a/Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.cpp b/Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.cpp index 175c237a30..080a27a58e 100644 --- a/Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.cpp +++ b/Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.cpp @@ -8,9 +8,12 @@ * http://tudat.tudelft.nl/LICENSE. */ +#include + #include #include #include +#include #include "Tudat/Astrodynamics/BasicAstrodynamics/sphericalStateConversions.h" #include "Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.h" @@ -24,42 +27,147 @@ namespace tudat namespace reference_frames { +//! Function to get a string representing a 'named identification' of a reference frame. +std::string getAerodynamicFrameName( const AerodynamicsReferenceFrames frame ) +{ + std::string frameName; + switch( frame ) + { + case inertial_frame: + frameName = "inertial frame "; + break; + case corotating_frame: + frameName = "corotating frame "; + break; + case vertical_frame: + frameName = "vertical frame "; + break; + case trajectory_frame: + frameName = "trajectory frame "; + break; + case aerodynamic_frame: + frameName = "aerodynamic frame "; + break; + case body_frame: + frameName = "body frame "; + break; + default: + std::string errorMessage = "Error, aerodynamic frame type " + + boost::lexical_cast< std::string >( frame ) + + "not found when retrieving frame name "; + throw std::runtime_error( errorMessage ); + } + return frameName; +} + +//! Function to get a string representing a 'named identification' of an aerodynamic angle +std::string getAerodynamicAngleName( const AerodynamicsReferenceFrameAngles angle ) +{ + std::string angleName; + switch( angle ) + { + case latitude_angle: + angleName = "latitude angle "; + break; + case longitude_angle: + angleName = "longitude angle "; + break; + case heading_angle: + angleName = "heading angle "; + break; + case flight_path_angle: + angleName = "flight path angle "; + break; + case angle_of_attack: + angleName = "angle of attack "; + break; + case angle_of_sideslip: + angleName = "sideslip angle "; + break; + case bank_angle: + angleName = "bank angle "; + break; + default: + std::string errorMessage = "Error, aerodynamic angle type " + + boost::lexical_cast< std::string >( angle ) + + "not found when retrieving angle name "; + throw std::runtime_error( errorMessage ); + } + return angleName; +} + + //! Function to update the orientation angles to the current state. -void AerodynamicAngleCalculator::update( ) +void AerodynamicAngleCalculator::update( const double currentTime, const bool updateBodyOrientation ) { // Clear all current rotation matrices. currentRotationMatrices_.clear( ); // Get current body-fixed state. - currentBodyFixedState_ = bodyFixedStateFunction_( ); - Eigen::Vector3d sphericalCoordinates = coordinate_conversions::convertCartesianToSpherical( - currentBodyFixedState_.segment( 0, 3 ) ); + if( !( currentTime == currentTime_ ) ) + { + currentBodyFixedState_ = bodyFixedStateFunction_( ); + currentRotationFromCorotatingToInertialFrame_ = rotationFromCorotatingToInertialFrame_( ); + + Eigen::Vector3d sphericalCoordinates = coordinate_conversions::convertCartesianToSpherical< double >( + currentBodyFixedState_.segment( 0, 3 ) ); - // Calculate latitude and longitude. - currentAerodynamicAngles_[ latitude_angle ] = - mathematical_constants::PI / 2.0 - sphericalCoordinates( 1 ); - currentAerodynamicAngles_[ longitude_angle ] = sphericalCoordinates( 2 ); + // Calculate latitude and longitude. + currentAerodynamicAngles_[ latitude_angle ] = + mathematical_constants::PI / 2.0 - sphericalCoordinates( 1 ); + currentAerodynamicAngles_[ longitude_angle ] = sphericalCoordinates( 2 ); + + // Calculate vertical <-> aerodynamic <-> body-fixed angles if neede. + if( calculateVerticalToAerodynamicFrame_ ) + { + Eigen::Vector3d verticalFrameVelocity = + getRotatingPlanetocentricToLocalVerticalFrameTransformationQuaternion( + currentAerodynamicAngles_.at( longitude_angle ), + currentAerodynamicAngles_.at( latitude_angle ) ) * + currentBodyFixedState_.segment( 3, 3 ); + + currentAerodynamicAngles_[ heading_angle ] = calculateHeadingAngle( verticalFrameVelocity ); + currentAerodynamicAngles_[ flight_path_angle ] = + calculateFlightPathAngle( verticalFrameVelocity ); + } - // Calculate vertical <-> aerodynamic <-> body-fixed angles if neede. - if( calculateVerticalToAerodynamicFrame_ ) + currentTime_ = currentTime; + } + + if( updateBodyOrientation && !( currentBodyAngleTime_ == currentTime ) ) { - Eigen::Vector3d verticalFrameVelocity = - getRotatingPlanetocentricToLocalVerticalFrameTransformationQuaternion( - currentAerodynamicAngles_.at( longitude_angle ), - currentAerodynamicAngles_.at( latitude_angle ) ) * - currentBodyFixedState_.segment( 3, 3 ); - - currentAerodynamicAngles_[ heading_angle ] = - orbital_element_conversions::calculateHeadingAngle( verticalFrameVelocity ); - currentAerodynamicAngles_[ flight_path_angle ] = - orbital_element_conversions::calculateFlightPathAngle( verticalFrameVelocity ); - - currentAerodynamicAngles_[ angle_of_attack ] = angleOfAttackFunction_( ); - currentAerodynamicAngles_[ angle_of_sideslip ] = angleOfSideslipFunction_( ); - currentAerodynamicAngles_[ bank_angle ] = bankAngleFunction_( ); + if( !angleUpdateFunction_.empty( ) ) + { + angleUpdateFunction_( currentTime ); + } + + if( !angleOfAttackFunction_.empty( ) ) + { + currentAerodynamicAngles_[ angle_of_attack ] = angleOfAttackFunction_( ); + } + + if( !angleOfSideslipFunction_.empty( ) ) + { + currentAerodynamicAngles_[ angle_of_sideslip ] = angleOfSideslipFunction_( ); + } + + if( !bankAngleFunction_.empty( ) ) + { + currentAerodynamicAngles_[ bank_angle ] = bankAngleFunction_( ); + } + + currentBodyAngleTime_ = currentTime; + + } + else if( !( currentBodyAngleTime_ == currentTime ) ) + { + currentAerodynamicAngles_[ angle_of_attack ] = 0.0; + currentAerodynamicAngles_[ angle_of_sideslip ] = 0.0; + currentAerodynamicAngles_[ bank_angle ] = 0.0; } } + //! Function to get the rotation quaternion between two frames Eigen::Quaterniond AerodynamicAngleCalculator::getRotationQuaternionBetweenFrames( const AerodynamicsReferenceFrames originalFrame, @@ -68,11 +176,6 @@ Eigen::Quaterniond AerodynamicAngleCalculator::getRotationQuaternionBetweenFrame // Initialize rotation to identity matrix. Eigen::Quaterniond rotationToFrame = Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ); - // Inertial frame is not specificied in current object. - if( originalFrame == inertial_frame || targetFrame == inertial_frame) - { - throw( "Error in AerodynamicAngleCalculator, cannot calculate to/from inertial frame" ); - } // Check if update settings are consistent with requested frames. if( !calculateVerticalToAerodynamicFrame_ && @@ -117,6 +220,19 @@ Eigen::Quaterniond AerodynamicAngleCalculator::getRotationQuaternionBetweenFrame { switch( currentFrameIndex ) { + case static_cast< int >( inertial_frame ): + + if( isTargetFrameUp ) + { + rotationToFrame = currentRotationFromCorotatingToInertialFrame_.inverse( ) * + rotationToFrame; + } + else + { + throw std::runtime_error( + "Error, inertial_frame is end frame in AerodynamicAngleCalculator" ); + } + break; case static_cast< int >( corotating_frame ): if( isTargetFrameUp ) @@ -129,8 +245,8 @@ Eigen::Quaterniond AerodynamicAngleCalculator::getRotationQuaternionBetweenFrame } else { - throw std::runtime_error( - "Error, corotating_frame is end frame in AerodynamicAngleCalculator" ); + rotationToFrame = currentRotationFromCorotatingToInertialFrame_ * + rotationToFrame; } break; case static_cast< int >( vertical_frame ): @@ -231,6 +347,7 @@ Eigen::Quaterniond AerodynamicAngleCalculator::getRotationQuaternionBetweenFrame { rotationToFrame = currentRotationMatrices_.at( currentRotationPair ); } + return rotationToFrame; } @@ -251,6 +368,51 @@ double AerodynamicAngleCalculator::getAerodynamicAngle( return angleValue; } +//! Function to set the trajectory<->body-fixed orientation angles. +void AerodynamicAngleCalculator::setOrientationAngleFunctions( + const boost::function< double( ) > angleOfAttackFunction, + const boost::function< double( ) > angleOfSideslipFunction, + const boost::function< double( ) > bankAngleFunction, + const boost::function< void( const double ) > angleUpdateFunction ) +{ + if( !angleOfAttackFunction.empty( ) ) + { + if( !angleOfAttackFunction_.empty( ) ) + { + std::cout << "Warning, overriding existing angle of attack function in AerodynamicAngleCalculator" << std::endl; + } + angleOfAttackFunction_ = angleOfAttackFunction; + } + + if( !angleOfSideslipFunction.empty( ) ) + { + if( !angleOfAttackFunction_.empty( ) ) + { + std::cerr << "Warning, overriding existing angle of sideslip function in AerodynamicAngleCalculator" << std::endl; + } + angleOfSideslipFunction_ = angleOfSideslipFunction; + } + + if( !bankAngleFunction.empty( ) ) + { + if( !angleOfAttackFunction_.empty( ) ) + { + std::cerr << "Warning, overriding existing bank angle function in AerodynamicAngleCalculator" << std::endl; + } + bankAngleFunction_ = bankAngleFunction; + } + + if( !angleUpdateFunction.empty( ) ) + { + if( !angleUpdateFunction_.empty( ) ) + { + std::cerr << "Warning, overriding existing aerodynamic angle update function in AerodynamicAngleCalculator" << std::endl; + } + angleUpdateFunction_ = angleUpdateFunction; + } + +} + //! Get a function to transform aerodynamic force from local to propagation frame. boost::function< Eigen::Vector3d( const Eigen::Vector3d& ) > getAerodynamicForceTransformationFunction( @@ -298,6 +460,59 @@ getAerodynamicForceTransformationFunction( return transformationFunction; } +//! Function to update the aerodynamic angles to current time. +void AerodynamicAnglesClosure::updateAngles( const double currentTime ) +{ + // Retrieve rotation matrix that is to be converted to orientation angles. + currentRotationFromBodyToTrajectoryFrame_ = + ( ( imposedRotationFromInertialToBodyFixedFrame_( currentTime ) * + aerodynamicAngleCalculator_->getRotationQuaternionBetweenFrames( + trajectory_frame, inertial_frame ) ).toRotationMatrix( ) ).transpose( ); + + // Compute associated Euler angles and set as orientation angles. + Eigen::Vector3d eulerAngles = reference_frames::get132EulerAnglesFromRotationMatrix( + currentRotationFromBodyToTrajectoryFrame_ ); + currentBankAngle_ = eulerAngles.x( ); + currentAngleOfSideslip_ = eulerAngles.y( ); + currentAngleOfAttack_ = -eulerAngles.z( ); +} + +//! Function to make aerodynamic angle computation consistent with imposed body-fixed to inertial rotation. +void setAerodynamicDependentOrientationCalculatorClosure( + const boost::function< Eigen::Quaterniond( const double ) > imposedRotationFromInertialToBodyFixedFrame, + boost::shared_ptr< AerodynamicAngleCalculator > aerodynamicAngleCalculator ) +{ + boost::shared_ptr< AerodynamicAnglesClosure > aerodynamicAnglesClosure = + boost::make_shared< AerodynamicAnglesClosure >( + imposedRotationFromInertialToBodyFixedFrame, aerodynamicAngleCalculator ); + aerodynamicAngleCalculator->setOrientationAngleFunctions( + boost::bind( &AerodynamicAnglesClosure::getCurrentAngleOfAttack, aerodynamicAnglesClosure ), + boost::bind( &AerodynamicAnglesClosure::getCurrentAngleOfSideslip, aerodynamicAnglesClosure ), + boost::bind( &AerodynamicAnglesClosure::getCurrentBankAngle, aerodynamicAnglesClosure ), + boost::bind( &AerodynamicAnglesClosure::updateAngles, aerodynamicAnglesClosure, _1 ) ); +} + +//! Function to make aerodynamic angle computation consistent with existing DependentOrientationCalculator +void setAerodynamicDependentOrientationCalculatorClosure( + boost::shared_ptr< DependentOrientationCalculator > dependentOrientationCalculator, + boost::shared_ptr< AerodynamicAngleCalculator > aerodynamicAngleCalculator ) +{ + setAerodynamicDependentOrientationCalculatorClosure( + boost::bind( &DependentOrientationCalculator::getRotationToLocalFrame, dependentOrientationCalculator, _1 ), + aerodynamicAngleCalculator ); +} + +//! Function to make aerodynamic angle computation consistent with existing rotational ephemeris +void setAerodynamicDependentOrientationCalculatorClosure( + boost::shared_ptr< ephemerides::RotationalEphemeris > rotationalEphemeris, + boost::shared_ptr< AerodynamicAngleCalculator > aerodynamicAngleCalculator ) +{ + setAerodynamicDependentOrientationCalculatorClosure( + boost::bind( &ephemerides::RotationalEphemeris::getRotationToTargetFrame, + rotationalEphemeris, _1, basic_astrodynamics::JULIAN_DAY_ON_J2000 ), + aerodynamicAngleCalculator ); +} + } // namespace reference_frames } // namespace tudat diff --git a/Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.h b/Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.h index a7e4a426ff..d8ace318a9 100644 --- a/Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.h +++ b/Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.h @@ -7,6 +7,7 @@ * a copy of the license with this file. If not, please or visit: * http://tudat.tudelft.nl/LICENSE. */ + #ifndef TUDAT_AERODYNAMICANGLECALCULATOR_H #define TUDAT_AERODYNAMICANGLECALCULATOR_H @@ -20,6 +21,11 @@ #include #include "Tudat/Mathematics/BasicMathematics/linearAlgebraTypes.h" +#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h" +#include "Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h" +#include "Tudat/Astrodynamics/Ephemerides/rotationalEphemeris.h" +#include "Tudat/Astrodynamics/ReferenceFrames/dependentOrientationCalculator.h" + namespace tudat { @@ -39,6 +45,14 @@ enum AerodynamicsReferenceFrames body_frame = 4 }; +//! Function to get a string representing a 'named identification' of a reference frame. +/*! + * Function to get a string representing a 'named identification' of a reference frame. + * \param frame Type of reference frame + * \return String with reference frame id. + */ +std::string getAerodynamicFrameName( const AerodynamicsReferenceFrames frame ); + //! Enum to define ids for various angles needed for converting between inertial and body-fixed //! frame, using transformation chain via aerodynamic frame. enum AerodynamicsReferenceFrameAngles @@ -52,11 +66,21 @@ enum AerodynamicsReferenceFrameAngles bank_angle = 6 }; +//! Function to get a string representing a 'named identification' of an aerodynamic angle +/*! + * Function to get a string representing a 'named identification' of an aerodynamic angle + * \param angle Type of aerodynamic angle + * \return String withaerodynamic angle id. + */ +std::string getAerodynamicAngleName( const AerodynamicsReferenceFrameAngles angle ); + + + //! Object to calculate aerodynamic orientation angles from current vehicle state. /*! * Object to calculate aerodynamic orientation angles from current vehicle state. */ -class AerodynamicAngleCalculator +class AerodynamicAngleCalculator: public DependentOrientationCalculator { public: @@ -64,35 +88,80 @@ class AerodynamicAngleCalculator /*! * Constructor. * \param bodyFixedStateFunction Vehicle state in a frame fixed w.r.t. the central body. Note - * that this state is w.r.t. the body itself, not w.r.t. the local atmosphere + * that this state is w.r.t. the body itself, not w.r.t. the local atmosphere. + * \param rotationFromCorotatingToInertialFrame Function returning the quaternion that rotates from the corotating to + * the inertial frame. + * \param centralBodyName Name of central body w.r.t. which the angles are computed. + * \param calculateVerticalToAerodynamicFrame Boolean to determine whether to determine + * vertical <-> aerodynamic frame conversion when calling update function. * \param angleOfAttackFunction Function to determine the angle of attack of the vehicle. * \param angleOfSideslipFunction Function to determine the angle of sideslip of the vehicle. * \param bankAngleFunction Function to determine the bank angle of the vehicle. - * \param calculateVerticalToAerodynamicFrame Boolean to determine whether to determine - * vertical <-> aerodynamic frame conversion when calling update function. + * \param angleUpdateFunction Function to update the aerodynamic angles to the current time (default none). */ AerodynamicAngleCalculator( const boost::function< basic_mathematics::Vector6d( ) > bodyFixedStateFunction, - const boost::function< double( ) > angleOfAttackFunction = - boost::lambda::constant ( 0.0 ), - const boost::function< double( ) > angleOfSideslipFunction = - boost::lambda::constant ( 0.0 ), - const boost::function< double( ) > bankAngleFunction = - boost::lambda::constant ( 0.0 ), - const bool calculateVerticalToAerodynamicFrame = 0 ): + const boost::function< Eigen::Quaterniond( ) > rotationFromCorotatingToInertialFrame, + const std::string centralBodyName, + const bool calculateVerticalToAerodynamicFrame = 0, + const boost::function< double( ) > angleOfAttackFunction = boost::function< double( ) >( ), + const boost::function< double( ) > angleOfSideslipFunction = boost::function< double( ) >( ), + const boost::function< double( ) > bankAngleFunction = boost::function< double( ) >( ), + const boost::function< void( const double ) > angleUpdateFunction = boost::function< void( const double ) >( ) ): + DependentOrientationCalculator( ), bodyFixedStateFunction_( bodyFixedStateFunction ), + rotationFromCorotatingToInertialFrame_( rotationFromCorotatingToInertialFrame ), + centralBodyName_( centralBodyName ), + calculateVerticalToAerodynamicFrame_( calculateVerticalToAerodynamicFrame ), angleOfAttackFunction_( angleOfAttackFunction ), angleOfSideslipFunction_( angleOfSideslipFunction ), bankAngleFunction_( bankAngleFunction ), - calculateVerticalToAerodynamicFrame_( calculateVerticalToAerodynamicFrame ){ } + angleUpdateFunction_( angleUpdateFunction ), + currentBodyAngleTime_( TUDAT_NAN ){ } + + //! Function to get the current rotation from the global (propagation/inertial) to the local (body-fixed) frame. + /*! + * Function to get the current rotation from the global (propagation/inertial) to the local (body-fixed) frame. + * Implementation of pure virtual base class function, calls getRotationQuaternionBetweenFrames function. + * \return Current rotation from the global (propagation/inertial) to the local (body-fixed) frame. + */ + Eigen::Quaterniond getRotationToLocalFrame( ) + { + return getRotationQuaternionBetweenFrames( inertial_frame, body_frame ); + } + + //! Function to get the current rotation from the local (body-fixed) to the global (propagation/inertial) frame. + /*! + * Function to get the current rotation from the local (body-fixed) to the global (propagation/inertial) frame. + * Implementation of pure virtual base class function, calls getRotationQuaternionBetweenFrames function. + * \return Current rotation from the local (body-fixed) to the global (propagation/inertial) frame. + */ + Eigen::Quaterniond getRotationToGlobalFrame( ) + { + return getRotationQuaternionBetweenFrames( body_frame, inertial_frame ); + } + + //! Function to update the orientation angles to the current state. + /*! + * Function to update the orientation angles to the current state, implements base class pure virtual function; + * calls update function. Note, when calling this function, the bank angle, angle of attack and sideslip angle are + * NOT updated (due to possible cross-dependency with aerodynamic coefficients). + * \sa AerodynamicAngleCalculator::update + */ + void updateCalculator( const double currentTime ) + { + update( currentTime, true ); + } //! Function to update the orientation angles to the current state. /*! * This function updates all requires orientation angles to the current state of the vehicle. * The current state is retrieved from the bodyFixedStateFunction_ member variable * function pointer. + * \param currentTime Time to which angle calculator is to be updated. + * \param updateBodyOrientation Boolean denoting whether the trajectory<->body-fixed angles are to be updated. */ - void update( ); + void update( const double currentTime, const bool updateBodyOrientation ); //! Function to get the rotation quaternion between two frames /*! @@ -116,17 +185,59 @@ class AerodynamicAngleCalculator */ double getAerodynamicAngle( const AerodynamicsReferenceFrameAngles angleId ); + //! Function to set the trajectory<->body-fixed orientation angles. + /*! + * Function to set the trajectory<->body-fixed orientation angles. + * \param angleOfAttackFunction Function to return the angle of attack. + * \param angleOfSideslipFunction Function to return the angle of sideslip. + * \param bankAngleFunction Function to return the bank angle. + * \param angleUpdateFunction Function to update the angles to the current time. + */ void setOrientationAngleFunctions( - const boost::function< double( ) > angleOfAttackFunction = - boost::lambda::constant ( 0.0 ), - const boost::function< double( ) > angleOfSideslipFunction = - boost::lambda::constant ( 0.0 ), - const boost::function< double( ) > bankAngleFunction = - boost::lambda::constant ( 0.0 ) ) + const boost::function< double( ) > angleOfAttackFunction = boost::function< double( ) >( ), + const boost::function< double( ) > angleOfSideslipFunction = boost::function< double( ) >( ), + const boost::function< double( ) > bankAngleFunction = boost::function< double( ) >( ), + const boost::function< void( const double ) > angleUpdateFunction = boost::function< void( const double ) >( ) ); + + //! Function to get the function returning the quaternion that rotates from the corotating to the inertial frame. + /*! + * Function to get the function returning the quaternion that rotates from the corotating to the inertial frame. + * \return Function returning the quaternion that rotates from the corotating to the inertial frame. + */ + boost::function< Eigen::Quaterniond( ) > getRotationFromCorotatingToInertialFrame( ) { - angleOfAttackFunction_ = angleOfAttackFunction; - angleOfSideslipFunction_ = angleOfSideslipFunction; - bankAngleFunction_ = bankAngleFunction; + return rotationFromCorotatingToInertialFrame_; + } + + //! Function to get the name of central body w.r.t. which the angles are computed. + /*! + * Function to get the name of central body w.r.t. which the angles are computed. + * \return Name of central body w.r.t. which the angles are computed. + */ + std::string getCentralBodyName( ) + { + return centralBodyName_; + } + + //! Function to get the current body-fixed state of vehicle, as set by previous call to update( ). + /*! + * Function to get the current body-fixed state of vehicle, as set by previous call to update( ). + * \return Current body-fixed state of vehicle, as set by previous call to update( ). + */ + basic_mathematics::Vector6d getCurrentBodyFixedState( ) + { + return currentBodyFixedState_; + } + + //! Function to reset the value of the currentBodyAngleTime_ variable + /*! + * Function to reset the value of the currentBodyAngleTime_ variable. Typically used to reset the time to NaN, + * signalling the need to recompute all quantities upon the next relevant function call. + * \param currentTime New current time. + */ + void resetDerivedClassTime( const double currentTime = TUDAT_NAN ) + { + currentBodyAngleTime_ = currentTime; } private: @@ -142,6 +253,8 @@ class AerodynamicAngleCalculator //! Current body-fixed state of vehicle, as set by previous call to update( ). basic_mathematics::Vector6d currentBodyFixedState_; + Eigen::Quaterniond currentRotationFromCorotatingToInertialFrame_; + //! Vehicle state in a frame fixed w.r.t. the central body. /*! * Vehicle state in a frame fixed w.r.t. the central body. @@ -149,6 +262,16 @@ class AerodynamicAngleCalculator */ boost::function< basic_mathematics::Vector6d( ) > bodyFixedStateFunction_; + //! Function returning the quaternion that rotates from the corotating to the inertial frame. + boost::function< Eigen::Quaterniond( ) > rotationFromCorotatingToInertialFrame_; + + //! Name of central body w.r.t. which the angles are computed. + std::string centralBodyName_; + + //! Boolean to determine whether to determine vertical <-> aerodynamic frame conversion + //! when calling update function. + bool calculateVerticalToAerodynamicFrame_; + //! Function to determine the angle of attack of the vehicle. boost::function< double( ) > angleOfAttackFunction_; @@ -158,9 +281,11 @@ class AerodynamicAngleCalculator //! Function to determine the bank angle of the vehicle. boost::function< double( ) > bankAngleFunction_; - //! Boolean to determine whether to determine vertical <-> aerodynamic frame conversion - //! when calling update function. - bool calculateVerticalToAerodynamicFrame_; + //! Function to update the bank, attack and sideslip angles to current time. + boost::function< void( const double ) > angleUpdateFunction_; + + //! Current time to which the bank, attack and sideslip angles have been updated. + double currentBodyAngleTime_; }; @@ -183,7 +308,125 @@ getAerodynamicForceTransformationFunction( const AerodynamicsReferenceFrames accelerationFrame, const boost::function< Eigen::Quaterniond( ) > bodyFixedToInertialFrameFunction = boost::lambda::constant( Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ) ), - const AerodynamicsReferenceFrames propagationFrame = inertial_frame); + const AerodynamicsReferenceFrames propagationFrame = inertial_frame ); + +//! Wrapper class to set closure between an imposed orientation of a body and its bank, sideslip and attack angles. +/*! + * Wrapper class to set closure between an imposed orientation of a body and its bank, sideslip and attack angles. + * Based on a given rotation matrix function and AerodynamicAngleCalculator, this class computes the required values + * for the ank, sideslip and attack angles so that the output from AerodynamicAngleCalculator is consistent with the + * given rotation matrix. + */ +class AerodynamicAnglesClosure +{ +public: + + //! Constructor + /*! + * Constructor + * \param imposedRotationFromInertialToBodyFixedFrame Inertial to body-fixed frame rotation to which the + * aerodynamicAngleCalculator object is to be made consistent + * \param aerodynamicAngleCalculator Object from which the aerodynamic angles are computed. + */ + AerodynamicAnglesClosure( + const boost::function< Eigen::Quaterniond( const double ) > imposedRotationFromInertialToBodyFixedFrame, + const boost::shared_ptr< AerodynamicAngleCalculator > aerodynamicAngleCalculator ): + imposedRotationFromInertialToBodyFixedFrame_( imposedRotationFromInertialToBodyFixedFrame ), + aerodynamicAngleCalculator_( aerodynamicAngleCalculator ) + { } + + //! Function to update the aerodynamic angles to current time. + /*! + * Function to update the aerodynamic angles to current time, using closure between + * imposedRotationFromInertialToBodyFixedFrame_ and aerodynamicAngleCalculator_. + * \param currentTime Time to which angles are to be updated. + */ + void updateAngles( const double currentTime ); + + //! Function returning the current angle of attack, as computed by last call to updateAngles function. + /*! + * Function returning the current angle of attack, as computed by last call to updateAngles function. + * \return Current angle of attack, as computed by last call to updateAngles function. + */ + double getCurrentAngleOfAttack( ) + { + return currentAngleOfAttack_; + } + + //! Function returning the current angle of sideslip, as computed by last call to updateAngles function. + /*! + * Function returning the current angle of sideslip, as computed by last call to updateAngles function. + * \return Current angle of sideslip, as computed by last call to updateAngles function. + */ + double getCurrentAngleOfSideslip( ) + { + return currentAngleOfSideslip_; + } + + //! Function returning the current bank angle, as computed by last call to updateAngles function. + /*! + * Function returning the current bank angle, as computed by last call to updateAngles function. + * \return Current bank angle, as computed by last call to updateAngles function. + */ + double getCurrentBankAngle( ) + { + return currentBankAngle_; + } + +private: + + //! Inertial to body-fixed frame rotation to which the aerodynamicAngleCalculator_ object is to be made consistent. + boost::function< Eigen::Quaterniond( const double ) > imposedRotationFromInertialToBodyFixedFrame_; + + //! Object from which the aerodynamic angles are computed. + boost::shared_ptr< AerodynamicAngleCalculator > aerodynamicAngleCalculator_; + + //! Current angle of attack, as computed by last call to updateAngles function. + double currentAngleOfAttack_; + + //! Current angle of sideslip, as computed by last call to updateAngles function. + double currentAngleOfSideslip_; + + //! Current bank angle, as computed by last call to updateAngles function. + double currentBankAngle_; + + //! Current rotation matrix from body-fixed to trajectory, as computed by last call to updateAngles function. + Eigen::Matrix3d currentRotationFromBodyToTrajectoryFrame_; +}; + +//! Function to make aerodynamic angle computation consistent with imposed body-fixed to inertial rotation. +/*! + * Function to make aerodynamic angle computation consistent with imposed body-fixed to inertial rotation. + * \param imposedRotationFromInertialToBodyFixedFrame Inertial to body-fixed frame rotation to which the + * aerodynamicAngleCalculator object is to be made consistent + * \param aerodynamicAngleCalculator Object from which the aerodynamic angles are computed. + */ +void setAerodynamicDependentOrientationCalculatorClosure( + const boost::function< Eigen::Quaterniond( const double ) > imposedRotationFromInertialToBodyFixedFrame, + boost::shared_ptr< AerodynamicAngleCalculator > aerodynamicAngleCalculator ); + +//! Function to make aerodynamic angle computation consistent with existing DependentOrientationCalculator +/*! + * Function to make aerodynamic angle computation consistent with existing DependentOrientationCalculator + * \param dependentOrientationCalculator Object computing the current orientation based on teh current state of the + * environment. Aerodynamic angles are to be computed from output given by this class. + * \param aerodynamicAngleCalculator Object from which the aerodynamic angles are computed. + */ +void setAerodynamicDependentOrientationCalculatorClosure( + boost::shared_ptr< DependentOrientationCalculator > dependentOrientationCalculator, + boost::shared_ptr< AerodynamicAngleCalculator > aerodynamicAngleCalculator ); + +//! Function to make aerodynamic angle computation consistent with existing rotational ephemeris +/*! + * Function to make aerodynamic angle computation consistent with existing rotational ephemeris + * \param rotationalEphemeris Object computing the current orientation of the body. Aerodynamic angles are to be computed + * from output given by this class. + * \param aerodynamicAngleCalculator Object from which the aerodynamic angles are computed. + */ +void setAerodynamicDependentOrientationCalculatorClosure( + boost::shared_ptr< ephemerides::RotationalEphemeris > rotationalEphemeris, + boost::shared_ptr< AerodynamicAngleCalculator > aerodynamicAngleCalculator ); + } // namespace reference_frames } // namespace tudat diff --git a/Tudat/Astrodynamics/ReferenceFrames/dependentOrientationCalculator.h b/Tudat/Astrodynamics/ReferenceFrames/dependentOrientationCalculator.h new file mode 100644 index 0000000000..b9adf8c9fd --- /dev/null +++ b/Tudat/Astrodynamics/ReferenceFrames/dependentOrientationCalculator.h @@ -0,0 +1,121 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + + +#ifndef TUDAT_DEPENDENTORIENTATIONCALCULATOR_H +#define TUDAT_DEPENDENTORIENTATIONCALCULATOR_H + +#include +#include + +namespace tudat +{ + +namespace reference_frames +{ + +//! Class that computes the orientation of a body as a function of the current state and time +/*! + * Class that computes the orientation of a body as a function of the current state and time. The functionality of this + * class is distinct from the RotationalEphemeris, in the sense that it is only defined during the numerical propagation, + * as it relies on the current state of the environment for the computation of the rotations. + */ +class DependentOrientationCalculator +{ +public: + + //! Constructor. + DependentOrientationCalculator( ): currentTime_( TUDAT_NAN ){ } + + //! Destructor + virtual ~DependentOrientationCalculator( ){ } + + //! Function to get the current rotation from the global (propagation/inertial) to the local (body-fixed) frame. + /*! + * Function to get the current rotation from the global (propagation/inertial) to the local (body-fixed) frame. + * Function is pure virtual and must be defined in derived class. + * \return Current rotation from the global (propagation/inertial) to the local (body-fixed) frame. + */ + virtual Eigen::Quaterniond getRotationToLocalFrame( ) = 0; + + + //! Function to get the current rotation from the local (body-fixed) to the global (propagation/inertial) frame. + /*! + * Function to get the current rotation from the local (body-fixed) to the global (propagation/inertial) frame. + * Function is pure virtual and must be defined in derived class. + * \return Current rotation from the local (body-fixed) to the global (propagation/inertial) frame. + */ + virtual Eigen::Quaterniond getRotationToGlobalFrame( ) = 0; + + //! Pure virtual function to update the object to the current state. + /*! + * Pure virtual function to update the object to the current state. + * \param currentTime Time to which angle calculator is to be updated. + */ + virtual void updateCalculator( const double currentTime ) = 0; + + //! Function to update the object to teh current time and retrieve the rotation to the local frame + /*! + * Function to update the object to teh current time and retrieve the rotation to the local frame + * Note that the environment must have been updated to the current time before calling this function. + * \param currentTime Time to which angle calculator is to be updated. + * \return Current rotation from the global (propagation/inertial) to the local (body-fixed) frame. + */ + Eigen::Quaterniond getRotationToLocalFrame( const double currentTime ) + { + updateCalculator( currentTime ); + return getRotationToLocalFrame( ); + } + + //! Function to update the object to teh current time and retrieve the rotation to the global frame + /*! + * Function to update the object to teh current time and retrieve the rotation to the global frame + * Note that the environment must have been updated to the current time before calling this function. + * \param currentTime Time to which angle calculator is to be updated. + * \return Current rotation from the local (body-fixed) to the global (propagation/inertial) frame. + */ + Eigen::Quaterniond getRotationToGlobalFrame( const double currentTime ) + { + updateCalculator( currentTime ); + return getRotationToGlobalFrame( ); + } + + //! Function to reset the current time in the derived class. + /*! + * Function to reset the current time in the derived class, must be implemented in the derived class. + * \sa resetCurrentTime + * \param currentTime New value of currentTime_ variable. + */ + virtual void resetDerivedClassTime( const double currentTime = TUDAT_NAN ){ } + + //! Function to reset the current time of the object + /*! + * Function to reset the current time of the object. Note that the orientation is not recomputed by calling this function + * only the currentTime_ (and any associated variables) are reset. Function is typically used to reset time to NaN, + * signalling the need to recompute all variables. + * \param currentTime New value of currentTime_ variable. + */ + void resetCurrentTime( const double currentTime = TUDAT_NAN ) + { + currentTime_ = currentTime; + resetDerivedClassTime( currentTime ); + } + +protected: + + //! Current simulation time. + double currentTime_; +}; + +} // namespace reference_frames + +} // namespace tudat + +#endif // TUDAT_DEPENDENTORIENTATIONCALCULATOR_H diff --git a/Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.cpp b/Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.cpp index 155d0c1472..eb59b3b6b1 100644 --- a/Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.cpp +++ b/Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.cpp @@ -52,6 +52,7 @@ */ #include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h" +#include "Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h" #include "Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h" namespace tudat @@ -59,6 +60,32 @@ namespace tudat namespace reference_frames { +//! Get classical 1-3-2 Euler angles set from rotation matrix +Eigen::Vector3d get132EulerAnglesFromRotationMatrix( + const Eigen::Matrix3d& rotationMatrix ) +{ + Eigen::Vector3d eulerAngles; + eulerAngles( 0 ) = std::atan2( -rotationMatrix( 2, 1 ), rotationMatrix( 1, 1 ) ); + eulerAngles( 1 ) = std::asin( rotationMatrix( 0, 1 ) ); + eulerAngles( 2 ) = std::atan2( -rotationMatrix( 0, 2 ), rotationMatrix( 0, 0 ) ); + return eulerAngles; +} + +//! Function to compute pole right ascension and declination, as well as prime meridian of date, from rotation matrix +Eigen::Vector3d calculateInertialToPlanetFixedRotationAnglesFromMatrix( + const Eigen::Matrix3d& rotationMatrixFromInertialToPlanetFixedFrame ) +{ + Eigen::Vector3d rotationAngles; + rotationAngles.x( ) = basic_mathematics::computeModulo( + std::atan2( rotationMatrixFromInertialToPlanetFixedFrame( 2, 0 ), + -rotationMatrixFromInertialToPlanetFixedFrame( 2, 1 ) ) - mathematical_constants::PI / 2.0, + 2.0 * mathematical_constants::PI );//right ascension + rotationAngles.y( ) = -std::acos( rotationMatrixFromInertialToPlanetFixedFrame( 2, 2 ) ) + mathematical_constants::PI / 2.0 ; //declination + rotationAngles.z( ) = std::atan2( rotationMatrixFromInertialToPlanetFixedFrame( 0, 2 ), + rotationMatrixFromInertialToPlanetFixedFrame( 1, 2 ) );//longitude of prime meridian + return rotationAngles; +} + //! Wrapper function to transform a vector to a different frame from a single rotation function. Eigen::Vector3d transformVectorFromQuaternionFunction( const Eigen::Vector3d& originalVector, @@ -358,6 +385,45 @@ Eigen::Quaterniond getAirspeedBasedAerodynamicToBodyFrameTransformationQuaternio angleOfAttack, angleOfSideslip ).inverse( ); } +//! Calculate current heading angle. +double calculateHeadingAngle( const Eigen::Vector3d& velocityInVerticalFrame ) +{ + return std::atan2( velocityInVerticalFrame( 1 ), velocityInVerticalFrame( 0 ) ); +} + +//! Calculate current flight path angle. +double calculateFlightPathAngle( const Eigen::Vector3d& velocityInVerticalFrame ) +{ + return -std::asin( velocityInVerticalFrame( 2 ) / velocityInVerticalFrame.norm( ) ); +} + +//! Get transformation quaternion ECEF to ENU V-frame +Eigen::Quaterniond getRotatingPlanetocentricToEnuLocalVerticalFrameTransformationQuaternion( + double longitude, double latitude ) +{ + return getEnuLocalVerticalToRotatingPlanetocentricFrameTransformationQuaternion( + longitude, latitude ).inverse( ); +} + +//! Get transformation quaternion between V-frame and ECEF +Eigen::Quaterniond getEnuLocalVerticalToRotatingPlanetocentricFrameTransformationQuaternion( + double longitude, double latitude ) +{ + // Compute transformation quaternion. + // source: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates + // Note the sign change (-1.0), because how angleAxisd is defined. + Eigen::AngleAxisd RotationAroundZaxis = Eigen::AngleAxisd( + longitude + mathematical_constants::PI / 2.0, Eigen::Vector3d::UnitZ( ) ); + Eigen::AngleAxisd RotationAroundXaxis = + Eigen::AngleAxisd( ( mathematical_constants::PI / 2.0 - latitude ), + Eigen::Vector3d::UnitX( ) ); + Eigen::Quaterniond frameTransformationQuaternion = Eigen::Quaterniond( + ( RotationAroundZaxis * RotationAroundXaxis ) ); + + // Return transformation quaternion. + return frameTransformationQuaternion; +} + } // namespace reference_frames } // namespace tudat diff --git a/Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h b/Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h index cf2164d5ea..16f7cf6bdd 100644 --- a/Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h +++ b/Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h @@ -60,11 +60,32 @@ #include #include +#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h" + namespace tudat { namespace reference_frames { +//! Get classical 1-3-2 Euler angles set from rotation matrix +/*! + * Get classical 1-3-2 Euler angles set from rotation matrix R. That is, the Euler angles x, y, z are returned such that + * R = R_{1}(x)R_{3}(y)R_{2}(z) + * \param rotationMatrix Rotation matrix for which the equivalent Euler angles are to be computed. + * \return Euler angles x,y,z (about 1, 3 and 2 axes, respectively). + */ +Eigen::Vector3d get132EulerAnglesFromRotationMatrix( + const Eigen::Matrix3d& rotationMatrix ); + +//! Function to compute pole right ascension and declination, as well as prime meridian of date, from rotation matrix +/*! + * Function to compute pole right ascension and declination, as well as prime meridian of date, from rotation matrix + * \param rotationMatrixFromInertialToPlanetFixedFrame Rotation matrix from which Euler angles are to be determined. + * \return Pole right ascension and declination, and prime meridian of date, from rotation matrix + */ +Eigen::Vector3d calculateInertialToPlanetFixedRotationAnglesFromMatrix( + const Eigen::Matrix3d& rotationMatrixFromInertialToPlanetFixedFrame ); + //! Wrapper function to transform a vector to a different frame from a single rotation function. /*! * Wrapper function to transform a vector to a different frame from a single rotation function. @@ -398,8 +419,46 @@ Eigen::Matrix3d getAirspeedBasedAerodynamicToBodyFrameTransformationMatrix( Eigen::Quaterniond getAirspeedBasedAerodynamicToBodyFrameTransformationQuaternion( const double angleOfAttack, const double angleOfSideslip ); +//! Calculate current heading angle. +/*! + * Calculate heading angle from velocity in vertical (LVLH) frame. + * \param velocityInVerticalFrame Current Cartesian velocity in vertical frame. + * \return Current heading angle. + */ +double calculateHeadingAngle( const Eigen::Vector3d& velocityInVerticalFrame ); + +//! Calculate current flight path angle. Angle is defined positive upwards. +/*! + * Calculate flight path angle from velocity in vertical (LVLH) frame. + * Angle is defined positive upwards. + * \param velocityInVerticalFrame Current Cartesian velocity in vertical frame. + * \return Current flight path angle. + */ +double calculateFlightPathAngle( const Eigen::Vector3d& velocityInVerticalFrame ); + +//! Get ECEF to V-frame quaternion +/*! + * Get the transformation from the co-rotating planetocentric frame to local vertical. + * \param longitude Longitude of position. + * \param latitude Latitude of position. + * \return Transformation quaternion. + */ +Eigen::Quaterniond getRotatingPlanetocentricToEnuLocalVerticalFrameTransformationQuaternion( + const double longitude, const double latitude ); + +//! Get V-frame to ECEF quaternion +/*! + * Get the transformation from the local vertical to the co-rotating planetocentric frame. + * \sa http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates + * \param longitude Longitude of position. + * \param latitude Latitude of position. + * \return Transformation quaternion. + */ +Eigen::Quaterniond getEnuLocalVerticalToRotatingPlanetocentricFrameTransformationQuaternion( + const double longitude, const double latitude ); } // namespace reference_frames + } // namespace tudat #endif // TUDAT_REFERENCE_FRAME_TRANSFORMATIONS_H diff --git a/Tudat/Astrodynamics/Relativity/CMakeLists.txt b/Tudat/Astrodynamics/Relativity/CMakeLists.txt new file mode 100644 index 0000000000..a2678c8419 --- /dev/null +++ b/Tudat/Astrodynamics/Relativity/CMakeLists.txt @@ -0,0 +1,29 @@ + # Copyright (c) 2010-2016, Delft University of Technology + # All rigths reserved + # + # This file is part of the Tudat. Redistribution and use in source and + # binary forms, with or without modification, are permitted exclusively + # under the terms of the Modified BSD license. You should have received + # a copy of the license with this file. If not, please or visit: + # http://tudat.tudelft.nl/LICENSE. + # + +# Set the source files. +set(RELATIVITY_SOURCES + "${SRCROOT}${RELATIVITYDIR}/relativisticLightTimeCorrection.cpp" +) + +# Set the header files. +set(RELATIVITY_HEADERS + "${SRCROOT}${RELATIVITYDIR}/relativisticLightTimeCorrection.h" +) + +# Add static libraries. +add_library(tudat_relativity STATIC ${RELATIVITY_SOURCES} ${RELATIVITY_HEADERS}) +setup_tudat_library_target(tudat_relativity "${SRCROOT}${RELATIVITYDIR}") + + +# Add unit tests +add_executable(test_ShapiroTimeDelay "${SRCROOT}${RELATIVITYDIR}/UnitTests/unitTestShapiroTimeDelay.cpp") +setup_custom_test_program(test_ShapiroTimeDelay "${SRCROOT}${RELATIVITYDIR}") +target_link_libraries(test_ShapiroTimeDelay tudat_relativity tudat_gravitation tudat_observation_models tudat_ephemerides ${Boost_LIBRARIES}) diff --git a/Tudat/Astrodynamics/Relativity/UnitTests/unitTestShapiroTimeDelay.cpp b/Tudat/Astrodynamics/Relativity/UnitTests/unitTestShapiroTimeDelay.cpp new file mode 100644 index 0000000000..39f3480c35 --- /dev/null +++ b/Tudat/Astrodynamics/Relativity/UnitTests/unitTestShapiroTimeDelay.cpp @@ -0,0 +1,77 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + + +#define BOOST_TEST_MAIN + +#include +#include +#include +#include + +#include "Tudat/Astrodynamics/Relativity/relativisticLightTimeCorrection.h" +#include "Tudat/Astrodynamics/Ephemerides/constantEphemeris.h" +#include "Tudat/Astrodynamics/ObservationModels/ObservableCorrections/firstOrderRelativisticLightTimeCorrection.h" + +namespace tudat +{ + +namespace unit_tests +{ + +using namespace tudat::observation_models; +using namespace tudat::relativity; +using namespace tudat::ephemerides; + +BOOST_AUTO_TEST_SUITE( test_shapiro_delay ) + +BOOST_AUTO_TEST_CASE( testShapiroDelay ) +{ + basic_mathematics::Vector6d groundStationState; + groundStationState << 0.0, 0.0, 6378.0, 0.0, 0.0, 0.0; + basic_mathematics::Vector6d satelliteState; + satelliteState << 0.0, 0.0, 26600.0, 0.0, 0.0, 0.0; + basic_mathematics::Vector6d centralBodyPosition = basic_mathematics::Vector6d::Zero( ); + + boost::shared_ptr< ConstantEphemeris > ephemeris = boost::make_shared< ConstantEphemeris >( + boost::lambda::constant( centralBodyPosition ) ); + + double earthGravitationalParameter = 398600.44189E9; + + double directCalculation = calculateFirstOrderLightTimeCorrectionFromCentralBody( + earthGravitationalParameter, groundStationState.segment( 0, 3 ), + satelliteState.segment( 0, 3 ), centralBodyPosition.segment( 0, 3 ) ); + + std::vector< boost::function< basic_mathematics::Vector6d( const double ) > > perturbingBodyStateFunctions; + std::vector< boost::function< double( ) > > perturbingBodyGravitationalParameterFunctions; + + perturbingBodyStateFunctions.push_back( boost::bind( &Ephemeris::getCartesianStateFromEphemeris, ephemeris, _1, + basic_astrodynamics::JULIAN_DAY_ON_J2000 ) ); + perturbingBodyGravitationalParameterFunctions.push_back( boost::lambda::constant( earthGravitationalParameter ) ); + + FirstOrderLightTimeCorrectionCalculator correctionCalculator( + perturbingBodyStateFunctions, perturbingBodyGravitationalParameterFunctions, + boost::assign::list_of( "Earth" ), "Satellite", "Earth" ); + + double classInterfaceCalculation = correctionCalculator.calculateLightTimeCorrection( + groundStationState, satelliteState, 0.0, 0.0 ); + + // Living reviews in relativity, GPS. + double expectedResult = 6.3E-3; + + BOOST_CHECK_CLOSE_FRACTION( 0.5 * classInterfaceCalculation * physical_constants::SPEED_OF_LIGHT, expectedResult, 6.0E-2 ); + BOOST_CHECK_CLOSE_FRACTION( 0.5 * directCalculation * physical_constants::SPEED_OF_LIGHT, expectedResult, 6.0E-2 ); +} + +BOOST_AUTO_TEST_SUITE_END( ) + +} + +} diff --git a/Tudat/Astrodynamics/Relativity/relativisticLightTimeCorrection.cpp b/Tudat/Astrodynamics/Relativity/relativisticLightTimeCorrection.cpp new file mode 100644 index 0000000000..02c3a3b3f8 --- /dev/null +++ b/Tudat/Astrodynamics/Relativity/relativisticLightTimeCorrection.cpp @@ -0,0 +1,42 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h" +#include "Tudat/Astrodynamics/Relativity/relativisticLightTimeCorrection.h" + +namespace tudat +{ + +namespace relativity +{ + +//! Function to calculate first order relativistic light time correction due to a gravitating point mass. +double calculateFirstOrderLightTimeCorrectionFromCentralBody( const double bodyGravitationalParameter, + const Eigen::Vector3d& transmitterPosition, + const Eigen::Vector3d& receiverPosition, + const Eigen::Vector3d& centralBodyPosition, + const double ppnParameterGamma ) +{ + // Calculate Euclidean geometric distances between transmitter, receiver and gravitating body. + double distanceToReceiver = ( receiverPosition - centralBodyPosition ).norm( ); + double distanceToTransmitter = ( transmitterPosition - centralBodyPosition ).norm( ); + double linkEuclideanDistance = ( transmitterPosition - receiverPosition ).norm( ); + + // Calculate and return light time correction. + return ( 1.0 + ppnParameterGamma ) * bodyGravitationalParameter * physical_constants::INVERSE_CUBIC_SPEED_OF_LIGHT * std::log( + ( distanceToReceiver + distanceToTransmitter + linkEuclideanDistance ) / + ( distanceToReceiver + distanceToTransmitter - linkEuclideanDistance ) ); + +} + +} + +} + diff --git a/Tudat/Astrodynamics/Relativity/relativisticLightTimeCorrection.h b/Tudat/Astrodynamics/Relativity/relativisticLightTimeCorrection.h new file mode 100644 index 0000000000..1d872879d8 --- /dev/null +++ b/Tudat/Astrodynamics/Relativity/relativisticLightTimeCorrection.h @@ -0,0 +1,50 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_RELATIVISTICLIGHTTIMECORRECTIONS_H +#define TUDAT_RELATIVISTICLIGHTTIMECORRECTIONS_H + +#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h" +#include +#include + +#include +#include + +#include + +#include "Tudat/Mathematics/BasicMathematics/linearAlgebraTypes.h" + +namespace tudat +{ + +namespace relativity +{ + +//! Function to calculate first order relativistic light time correction due to a gravitating point mass. +/*! + * Function to calculate first order relativistic light time correction due to a gravitating point mass, + * according to Eq. (11.17) of 2010 IERS conventions. + * \param bodyGravitationalParameter Gravitational parameter of gravitating body. + * \param transmitterPosition Position of origin of electromagnetic signal (at time of transmission). + * \param receiverPosition Position of target of electromagentic signal (at time of reception) + * \param centralBodyPosition Position of perturbing body (at certain time during signal propagation). + * \param ppnParameterGamma Parametric post-Newtonian parameter gamma, a measure for the space-time curvature due to a unit rest mass (1.0 in GR) + * \return Light time correction (in seconds) due to the gravitating point mass. + */ +double calculateFirstOrderLightTimeCorrectionFromCentralBody( const double bodyGravitationalParameter, + const Eigen::Vector3d& transmitterPosition, + const Eigen::Vector3d& receiverPosition, + const Eigen::Vector3d& centralBodyPosition, + const double ppnParameterGamma = 1.0 ); +} // namespace relativity + +} // namespace tudat +#endif // TUDAT_RELATIVISTICLIGHTTIMECORRECTIONS_H diff --git a/Tudat/Astrodynamics/SystemModels/CMakeLists.txt b/Tudat/Astrodynamics/SystemModels/CMakeLists.txt new file mode 100644 index 0000000000..70f675faa4 --- /dev/null +++ b/Tudat/Astrodynamics/SystemModels/CMakeLists.txt @@ -0,0 +1,26 @@ + # Copyright (c) 2010-2016, Delft University of Technology + # All rigths reserved + # + # This file is part of the Tudat. Redistribution and use in source and + # binary forms, with or without modification, are permitted exclusively + # under the terms of the Modified BSD license. You should have received + # a copy of the license with this file. If not, please or visit: + # http://tudat.tudelft.nl/LICENSE. + # + +# Set the source files. +set(SYSTEMMODELS_SOURCES + "${SRCROOT}${SYSTEMMODELSDIR}/engineModel.cpp" +) + +# Set the header files. +set(SYSTEMMODELS_HEADERS + "${SRCROOT}${SYSTEMMODELSDIR}/engineModel.h" + "${SRCROOT}${SYSTEMMODELSDIR}/vehicleSystems.h" +) + +# Add static libraries. +add_library(tudat_system_models STATIC ${SYSTEMMODELS_SOURCES} ${SYSTEMMODELS_HEADERS}) +setup_tudat_library_target(tudat_system_models "${SRCROOT}${SYSTEMMODELSDIR}") + + diff --git a/Tudat/Astrodynamics/SystemModels/engineModel.cpp b/Tudat/Astrodynamics/SystemModels/engineModel.cpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tudat/Astrodynamics/SystemModels/engineModel.h b/Tudat/Astrodynamics/SystemModels/engineModel.h new file mode 100644 index 0000000000..ea044e9c1b --- /dev/null +++ b/Tudat/Astrodynamics/SystemModels/engineModel.h @@ -0,0 +1,167 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_ENGINEMODEL_H +#define TUDAT_ENGINEMODEL_H + +#include +#include + +#include "Tudat/Astrodynamics/Propulsion/thrustFunctions.h" + +#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h" + +namespace tudat +{ +namespace system_models +{ + +//! Base class for a model of an engine, the mass rate/thrust computations are not implemented here. +/*! + * Base class for a model of an engine, the mass rate/thrust computations are not implemented here, and should be + * performed in a derived class. + */ +class EngineModel +{ +public: + + //! Constructor + /*! + * Constructor + * \param bodyFixedThrustDirection Vector denoting the direction of the thrust delivered by the engine in the body-fixed + * frame (for an ideal engine, this is the opposite direction of the nozzle direction). By default, this vector is set + * to the positive x-direction, along the longitudinal axis. + */ + EngineModel( + const Eigen::Vector3d bodyFixedThrustDirection = Eigen::Vector3d::UnitX( ) ): + currentThrust_( TUDAT_NAN ), + bodyFixedThrustDirection_( bodyFixedThrustDirection.normalized( ) ) + { } + + //! Destructor. + virtual ~EngineModel( ){ } + + //! Function to retrive the magnitude of the current engine thrust + /*! + * Function to retrive the magnitude of the current engine thrust, which was set by the last call to the + * updateEngineModel function (to be defined in the derived class). + * \return Current engine thrust. + */ + double getCurrentThrust( ) + { + return currentThrust_; + } + + //! Pure virtual function to update the engine model to the current time + /*! + * Pure virtual function to update the engine model to the current time. This function must be implemented in teh derived + * class, wehre it typically resets the currentThrust_ variable (and any associated variables). + * \param currentTime Current itme in simulation. + */ + virtual void updateEngineModel( const double currentTime ) = 0; + + //! Pure virtual function to retrieve the propellant mass rate. + /*! + * Pure virtual function to retrieve the propellant mass rate. + * \return Propellant mass rate. + */ + virtual double getCurrentMassRate( ) = 0; + + //! Function to retrieve the vector denoting the direction of the thrust delivered by the engine in the body-fixed frame + /*! + * Function to retrieve the vector denoting the direction of the thrust delivered by the engine in the body-fixed frame + * \return Vector denoting the direction of the thrust delivered by the engine in the body-fixed frame + */ + Eigen::Vector3d getBodyFixedThrustDirection( ) + { + return bodyFixedThrustDirection_; + } + +protected: + + //! Current magnitude of the thrust delivered by the engine. + double currentThrust_; + + //! Vector denoting the direction of the thrust delivered by the engine in the body-fixed frame + /*! + * Vector denoting the direction of the thrust delivered by the engine in the body-fixed + * frame (for an ideal engine, this is the opposite direction of the nozzle direction). By default, this vector is set + * to the positive x-direction, along the longitudinal axis + */ + Eigen::Vector3d bodyFixedThrustDirection_; + +}; + +//! Engine model derived class in which the thrust is computed directly from teh propellant mass flow and specific impulse +/*! + * Engine model derived class in which the thrust is computed directly from teh propellant mass flow and specific impulse + * These two variables may be either constant or variable (magnitudes controlled by associated class defining e.g. GNC + * system. + */ +class DirectEngineModel: public EngineModel +{ +public: + + //! Constructor + /*! + * Constructor + * \param specificImpulseFunction Variable specific impulse of engine. (no input arguments provided; must be updated by + * associated guidance law). + * \param massFlowFunction Variable mass flow function. (no input arguments provided; must be updated by associated + * guidance law). + * \param bodyFixedThrustDirection Vector denoting the direction of the thrust delivered by the engine in the body-fixed + * frame (for an ideal engine, this is the opposite direction of the nozzle direction). By default, this vector is set + * to the positive x-direction, along the longitudinal axis. + */ + DirectEngineModel( + const boost::function< double( ) > specificImpulseFunction, + const boost::function< double( ) > massFlowFunction, + const Eigen::Vector3d bodyFixedThrustDirection = Eigen::Vector3d::UnitX( ) ): + EngineModel( bodyFixedThrustDirection ), + specificImpulseFunction_( specificImpulseFunction ), + massFlowFunction_( massFlowFunction ) + { } + + //! Function to update the engine model to the current time + /*! + * Function to update the engine model to the current time. Retrieves the current mass flow and specific impulse; + * consequently computes the current thrust. + * \param currentTime Current itme in simulation. + */ + void updateEngineModel( const double currentTime ) + { + currentThrust_ = propulsion::computeThrustFromSpecificImpulse( massFlowFunction_( ), specificImpulseFunction_( ) ); + } + + //! Function to retrieve the propellant mass rate. + /*! + * Function to retrieve the propellant mass rate. + * \return Propellant mass rate. + */ + double getCurrentMassRate( ) + { + return massFlowFunction_( ); + } + + +protected: + + //! Variable specific impulse of engine (no input arguments provided; must be updated by associated guidance law). + boost::function< double( ) > specificImpulseFunction_; + + //! Variable mass flow function (no input arguments provided; must be updated by associated guidance law). + boost::function< double( ) > massFlowFunction_; +}; + +} // namespace system_models + +} // namespace tudat + +#endif // TUDAT_ENGINEMODEL_H diff --git a/Tudat/Astrodynamics/SystemModels/vehicleSystems.h b/Tudat/Astrodynamics/SystemModels/vehicleSystems.h new file mode 100644 index 0000000000..b3ca7029ef --- /dev/null +++ b/Tudat/Astrodynamics/SystemModels/vehicleSystems.h @@ -0,0 +1,87 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_VEHICLESYSTEMS_H +#define TUDAT_VEHICLESYSTEMS_H + +#include + +#include + +#include "Tudat/Astrodynamics/SystemModels/engineModel.h" + +namespace tudat +{ + +namespace system_models +{ + +//! Wrapper class that contains the relevant hardware systems of a vehicle. +/*! + * Wrapper class that contains the relevant hardware systems of a vehicle. Not all member objects need to be set; NULL + * member objects denote that the vehicle does not contain the associated hardware. + */ +class VehicleSystems +{ +public: + + //! Constructor + /*! + * Constructor + * \param dryMass Total dry mass of the vehicle (not defined; NaN by default). + */ + VehicleSystems( const double dryMass = TUDAT_NAN ): + dryMass_( dryMass ){ } + + //! Function to retrieve the engine models + /*! + * Function to retrieve the engine models + * \return Named list of engine models in the vehicle + */ + std::map< std::string, boost::shared_ptr< EngineModel > > getEngineModels( ) + { + return engineModels_; + } + + //! Function to set a single engine in the vehicle + /*! + * Function to set a single engine in the vehicle. Each engine can be identified by a string. If only a single + * engine is set, the default (empty string) can be used. + * \param engineModel Model of engine that is to be set + * \param engineName Reference id of the engine that is to be set. + */ + void setEngineModel( + const boost::shared_ptr< EngineModel > engineModel, const std::string engineName = "" ) + { + // Check if engine with this name already exists. + if( engineModels_.count( engineName ) ) + { + std::cerr<<"Warning, engine model of name "< > engineModels_; + + //! Total dry mass of the vehicle + double dryMass_; + +}; + + +} // namespace system_models + +} // namespace tudat + +#endif // TUDAT_VEHICLESYSTEMS_H diff --git a/Tudat/Basics/CMakeLists.txt b/Tudat/Basics/CMakeLists.txt index 3f401b2789..50c41a93d1 100644 --- a/Tudat/Basics/CMakeLists.txt +++ b/Tudat/Basics/CMakeLists.txt @@ -40,6 +40,7 @@ set(BASICSDIR_SOURCES # Add header files. set(BASICSDIR_HEADERS + "${SRCROOT}${BASICSDIR}/utilities.h" "${SRCROOT}${BASICSDIR}/testMacros.h" "${SRCROOT}${BASICSDIR}/utilityMacros.h" ) diff --git a/Tudat/Basics/utilities.h b/Tudat/Basics/utilities.h new file mode 100644 index 0000000000..c3d5e5eba2 --- /dev/null +++ b/Tudat/Basics/utilities.h @@ -0,0 +1,140 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_UTILITIES_H +#define TUDAT_UTILITIES_H + +#include + +#include +#include + +#include + +#include + +namespace tudat +{ + +namespace utilities +{ + +//! Function to create a vector from the values of a map +/*! + * Function to create a vector from the values of a map. The output vector is in the order of the map entries, i.e. as provided by a forward iterator. + * The map keys are not used for the return vector. + * \param inputMap Original map from which the vector is to be created + * \return Vector created from the map values + */ +template< typename VectorArgument, typename KeyType > +std::vector< VectorArgument > createVectorFromMapValues( const std::map< KeyType, VectorArgument >& inputMap ) +{ + // Create and size return vector. + std::vector< VectorArgument > outputVector; + outputVector.resize( inputMap.size( ) ); + + // Iterate over all map entries and create vector + int currentIndex = 0; + for( typename std::map< KeyType, VectorArgument >::const_iterator mapIterator = inputMap.begin( ); + mapIterator != inputMap.end( ); mapIterator++ ) + { + outputVector[ currentIndex ] = mapIterator->second; + currentIndex++; + } + + return outputVector; + +} + +//! Function to create a vector from the keys of a map +/*! + * Function to create a vector from the keys of a map. The output vector is in the order of the map entries, i.e. as provided by a forward iterator. + * The map values are not used for the return vector. + * \param inputMap Original map from which the vector is to be created + * \return Vector created from the map keys + */ +template< typename VectorArgument, typename KeyType > +std::vector< KeyType > createVectorFromMapKeys( const std::map< KeyType, VectorArgument >& inputMap ) +{ + // Create and size return vector. + std::vector< KeyType > outputVector; + outputVector.resize( inputMap.size( ) ); + + // Iterate over all map entries and create vector + int currentIndex = 0; + for( typename std::map< KeyType, VectorArgument >::const_iterator mapIterator = inputMap.begin( ); + mapIterator != inputMap.end( ); mapIterator++ ) + { + outputVector[ currentIndex ] = mapIterator->first; + currentIndex++; + } + + return outputVector; +} + +//! Function to sum the return values of two boost function with empty input argument list. +/*! + * Function to sum the return values of two boost function with empty input argument list. + * \param function1 First function to be added. + * \param function2 Second function to be added. + * \return Sum of return values of function1 and function2 + */ +template< typename S > +S sumFunctionReturn( const boost::function< S( ) > function1, const boost::function< S( ) > function2 ) +{ + return function1( ) + function2( ); +} + +//! Function to create a vector block history from full matrix history. +/*! + * Function to create a vector matrix block history from full matrix history. + * \param matrixHistory Full matrix history + * \param blockMatrixHistory Block vector history (return by reference). + * \param startIndices Starting point (row,column) in matrix of return vector blocks. + * \param segmentSize Number of rows in vector. + */ +template< typename S, typename T > +void createVectorBlockMatrixHistory( + const std::map< S, Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > >& matrixHistory, + std::map< S, Eigen::Matrix< T, Eigen::Dynamic, 1 > >& blockMatrixHistory, + const std::pair< int, int > startIndices, const int segmentSize ) +{ + blockMatrixHistory.clear( ); + + // Loop over integration output and put results in corresponding data structures. + for( typename std::map< S, Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > >::const_iterator matrixIterator = matrixHistory.begin( ); + matrixIterator != matrixHistory.end( ); matrixIterator++ ) + { + // Set numerically integrated states of bodies. + blockMatrixHistory[ matrixIterator->first ] = + matrixIterator->second.block( startIndices.first, startIndices.second, segmentSize, 1 ); + } +} + +//! Function to print the contents of a map, line by line +/*! + * Function to print the contents of a map, line by line. Both the key and value types must have the << operator defined + * \param mapToPrint Map that is to be printed. + */ +template< typename S, typename T > +void printMapContents( const std::map< S, T >& mapToPrint) +{ + for( typename std::map< S, T >::const_iterator mapIterator = mapToPrint.begin( ); + mapIterator != mapToPrint.end( ); mapIterator++ ) + { + std::cout<first<<", "<second<( i ); + totalPath = outputFileName + "_" + boost::lexical_cast< std::string >( i ); tudatFile = readMatrixFromFile( totalPath ); // Read benchmark file totalPath = getTudatRootPath( ) + "InputOutput/UnitTests/benchmark_" - + boost::lexical_cast< string >( i ); + + boost::lexical_cast< std::string >( i ); benchmarkFile = readMatrixFromFile( totalPath ); // Iterate over all entries and check values. diff --git a/Tudat/InputOutput/fixedWidthParser.cpp b/Tudat/InputOutput/fixedWidthParser.cpp index 719eb32d98..7d6005a964 100644 --- a/Tudat/InputOutput/fixedWidthParser.cpp +++ b/Tudat/InputOutput/fixedWidthParser.cpp @@ -116,7 +116,7 @@ void FixedWidthParser::parseLine( std::string& line ) } // Verify that number of individual vectors corresponds to the specified number of fields. - if ( vectorOfIndividualStrings_.size() != numberOfFields_ ) + if ( vectorOfIndividualStrings_.size( ) != numberOfFields_ ) { std::cerr << "Number of elements in the line (" << vectorOfIndividualStrings_.size( ) << ") does not match the specified number of fields (" << numberOfFields_ << ")" diff --git a/Tudat/InputOutput/missileDatcomData.cpp b/Tudat/InputOutput/missileDatcomData.cpp index c642ad3949..fb8da89df7 100644 --- a/Tudat/InputOutput/missileDatcomData.cpp +++ b/Tudat/InputOutput/missileDatcomData.cpp @@ -54,8 +54,6 @@ namespace input_output using unit_conversions::convertDegreesToRadians; using std::iterator; -using std::string; -using std::vector; //! Constructor that reads and processes Missile Datcom output. MissileDatcomData::MissileDatcomData( const std::string& fileNameAndPath ) @@ -65,13 +63,13 @@ MissileDatcomData::MissileDatcomData( const std::string& fileNameAndPath ) } //! Function to convert the MissileDatcomData. -void MissileDatcomData::convertDatcomData( const vector< double >& datcomData ) +void MissileDatcomData::convertDatcomData( const std::vector< double >& datcomData ) { - vector< double > datcomDataToBeConverted = datcomData; + std::vector< double > datcomDataToBeConverted = datcomData; // Add an extra item at the begin, such that the first real value is at entry 1. // The same indices as described in the MissileDatcom user manual can now be used. - vector< double >::iterator it; + std::vector< double >::iterator it; it = datcomDataToBeConverted.begin( ); datcomDataToBeConverted.insert( it,-0.0 ); @@ -210,7 +208,7 @@ void MissileDatcomData::writeCoefficientsToFile( const std::string& fileNameBase // Make filename for specific angle of attack. std::ostringstream stringstream; stringstream << fileNameBase << "_" << i; - string fileName = stringstream.str( ); + std::string fileName = stringstream.str( ); // Open output file. std::ofstream outputFile; diff --git a/Tudat/InputOutput/missileDatcomReader.cpp b/Tudat/InputOutput/missileDatcomReader.cpp index ef8b1a6c16..f9baa64df9 100644 --- a/Tudat/InputOutput/missileDatcomReader.cpp +++ b/Tudat/InputOutput/missileDatcomReader.cpp @@ -52,10 +52,6 @@ namespace tudat namespace input_output { -using std::string; -using std::stringstream; -using std::vector; - //! Default constructor. MissileDatcomReader::MissileDatcomReader ( const std::string& fileNameAndPath ) : dataFile_( ) { @@ -93,7 +89,7 @@ void MissileDatcomReader::readFor004( const std::string& fileNameAndPath ) } //! Open data file. -void MissileDatcomReader::openFile( const string& fileNameAndPath ) +void MissileDatcomReader::openFile( const std::string& fileNameAndPath ) { // Open data file. dataFile_.open( fileNameAndPath.c_str( ), std::ios::binary ); @@ -114,16 +110,16 @@ void MissileDatcomReader::openFile( const string& fileNameAndPath ) } //! Function to split a string consisting of line of entries in smaller strings. -void MissileDatcomReader::split( const string& dataString, char separator, - vector< string >& dataVector ) +void MissileDatcomReader::split( const std::string& dataString, char separator, + std::vector< std::string >& dataVector ) { - string::size_type i = 0; - string::size_type j = dataString.find( separator ); + std::string::size_type i = 0; + std::string::size_type j = dataString.find( separator ); // Loop over the whole string and place characters into new string until the separator is // found. Then start with a new string at the next entry in the vector. Until the end of the // original string is reached. - while ( j != string::npos ) + while ( j != std::string::npos ) { // to prevent placements of empty entries if multiple seperators after eachother are placed. if ( !dataString.substr( i, j-i ).empty() ) @@ -134,7 +130,7 @@ void MissileDatcomReader::split( const string& dataString, char separator, i = ++j; j = dataString.find( separator, j ); - if ( j == string::npos ) + if ( j == std::string::npos ) { dataVector.push_back( dataString.substr(i, dataString.length( ) ) ); } @@ -142,7 +138,7 @@ void MissileDatcomReader::split( const string& dataString, char separator, } //! Read and store data. -void MissileDatcomReader::readAndStoreData( const string& skipKeyword ) +void MissileDatcomReader::readAndStoreData( const std::string& skipKeyword ) { // Local variable for reading a single line std::string stringOfData; @@ -158,7 +154,7 @@ void MissileDatcomReader::readAndStoreData( const string& skipKeyword ) // Check if string doesn't start with set starting character, if string // is not empty, and if the skip keyword is not in the string. - if ( ( ( !skipKeyword.empty( ) && stringOfData.find( skipKeyword ) == string::npos ) + if ( ( ( !skipKeyword.empty( ) && stringOfData.find( skipKeyword ) == std::string::npos ) || ( skipKeyword.empty( ) ) ) && !stringOfData.empty( ) ) { // Store string in container. diff --git a/Tudat/InputOutput/parseSolarActivityData.cpp b/Tudat/InputOutput/parseSolarActivityData.cpp index b10401862b..61bd49e9f6 100644 --- a/Tudat/InputOutput/parseSolarActivityData.cpp +++ b/Tudat/InputOutput/parseSolarActivityData.cpp @@ -38,6 +38,7 @@ #include "Tudat/InputOutput/fixedWidthParser.h" #include +#include #include diff --git a/Tudat/InputOutput/parseSolarActivityData.h b/Tudat/InputOutput/parseSolarActivityData.h index 77c05f3c47..952f434a73 100644 --- a/Tudat/InputOutput/parseSolarActivityData.h +++ b/Tudat/InputOutput/parseSolarActivityData.h @@ -39,7 +39,6 @@ #define TUDAT_PARSESOLARACTIVITY_H #include -#include #include @@ -132,8 +131,6 @@ namespace input_output namespace solar_activity { -using std::string; - //! Solar activity parser class. /*! * This class implements a fixed width parser specifically designed for parsing solar activity diff --git a/Tudat/InputOutput/solarActivityData.h b/Tudat/InputOutput/solarActivityData.h index 4023706231..ec6bc93a6f 100644 --- a/Tudat/InputOutput/solarActivityData.h +++ b/Tudat/InputOutput/solarActivityData.h @@ -1,26 +1,11 @@ -/* Copyright (c) 2010-2012, Delft University of Technology - * All rights reserved. +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved * - * Redistribution and use in source and binary forms, with or without modification, are - * permitted provided that the following conditions are met: - * - Redistributions of source code must retain the above copyright notice, this list of - * conditions and the following disclaimer. - * - Redistributions in binary form must reproduce the above copyright notice, this list of - * conditions and the following disclaimer in the documentation and/or other materials - * provided with the distribution. - * - Neither the name of the Delft University of Technology nor the names of its contributors - * may be used to endorse or promote products derived from this software without specific - * prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS - * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE - * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING - * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED - * OF THE POSSIBILITY OF SUCH DAMAGE. + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. * * Changelog * YYMMDD Author Comment @@ -201,4 +186,4 @@ SolarActivityDataMap readSolarActivityData( std::string filePath ) ; } // namespace input_output } // namespace tudat -#endif //TUDAT_SOLAR_ACTIVITY_DATA_H +#endif // TUDAT_SOLAR_ACTIVITY_DATA_H diff --git a/Tudat/InputOutput/streamFilters.h b/Tudat/InputOutput/streamFilters.h index ea9e0c98df..e662566fd8 100644 --- a/Tudat/InputOutput/streamFilters.h +++ b/Tudat/InputOutput/streamFilters.h @@ -33,8 +33,6 @@ * References * * Notes - * Backwards compatibility of namespaces is implemented for Tudat Core 2 in this file. The - * code block marked "DEPRECATED!" at the end of the file should be removed in Tudat Core 3. * */ diff --git a/Tudat/InputOutput/twoLineElementData.cpp b/Tudat/InputOutput/twoLineElementData.cpp index f4dd526d3d..af4fc33281 100644 --- a/Tudat/InputOutput/twoLineElementData.cpp +++ b/Tudat/InputOutput/twoLineElementData.cpp @@ -76,21 +76,19 @@ TwoLineElementData::TwoLineElementData( ) std::ostream& operator<<( std::ostream& stream, TwoLineElementData& twoLineElementData ) { - // Using declarations. - using std::endl; - stream << "This is a TLE data object." << endl; - stream << "The converted TLE information is stored as: " << endl; + stream << "This is a TLE data object." << std::endl; + stream << "The converted TLE information is stored as: " << std::endl; - stream << "TLE line 0: " << endl; + stream << "TLE line 0: " << std::endl; for ( unsigned int i = 0; i < twoLineElementData.objectName.size( ); i++ ) { stream << twoLineElementData.objectName.at( i ) << " "; } - stream << endl; - stream << twoLineElementData.objectNameString << endl; + stream << std::endl; + stream << twoLineElementData.objectNameString << std::endl; - stream << "TLE line 1: " << endl; + stream << "TLE line 1: " << std::endl; stream << twoLineElementData.lineNumberLine1 << " "; stream << twoLineElementData.objectIdentificationNumber << " "; stream << twoLineElementData.tleClassification << " "; @@ -110,9 +108,9 @@ std::ostream& operator<<( std::ostream& stream, stream << twoLineElementData.bStar << " "; stream << twoLineElementData.orbitalModel << " "; stream << twoLineElementData.tleNumber << " "; - stream << twoLineElementData.modulo10CheckSumLine1 << endl; + stream << twoLineElementData.modulo10CheckSumLine1 << std::endl; - stream << "TLE line 2: " << endl; + stream << "TLE line 2: " << std::endl; stream << twoLineElementData.lineNumberLine2 << " "; stream << twoLineElementData.objectIdentificationNumberLine2 << " "; stream << twoLineElementData.TLEKeplerianElements( @@ -127,14 +125,14 @@ std::ostream& operator<<( std::ostream& stream, stream << twoLineElementData.meanMotionInRevolutionsPerDay << " "; stream << twoLineElementData.revolutionNumber << " "; stream << twoLineElementData.totalRevolutionNumber << " "; - stream << twoLineElementData.modulo10CheckSumLine2 << endl; + stream << twoLineElementData.modulo10CheckSumLine2 << std::endl; - stream << "Calculated values: " << endl; + stream << "Calculated values: " << std::endl; stream << "Semi-Major Axis = " << twoLineElementData.TLEKeplerianElements( orbital_element_conversions::semiMajorAxisIndex ) - << endl; - stream << "Perigee = " << twoLineElementData.perigee << endl; - stream << "Apogee = " << twoLineElementData.apogee << endl; + << std::endl; + stream << "Perigee = " << twoLineElementData.perigee << std::endl; + stream << "Apogee = " << twoLineElementData.apogee << std::endl; // Return stream. return stream; diff --git a/Tudat/InputOutput/twoLineElementsTextFileReader.cpp b/Tudat/InputOutput/twoLineElementsTextFileReader.cpp index 6d4bd7a735..c5ba206b24 100644 --- a/Tudat/InputOutput/twoLineElementsTextFileReader.cpp +++ b/Tudat/InputOutput/twoLineElementsTextFileReader.cpp @@ -85,13 +85,6 @@ namespace input_output // Using declarations. using mathematical_constants::PI; -using std::string; -using std::stringstream; -using std::endl; -using std::cerr; -using std::vector; -using std::multimap; -using std::pair; //! Open data file. void TwoLineElementsTextFileReader::openFile( ) @@ -166,7 +159,7 @@ void TwoLineElementsTextFileReader::readAndStoreData( ) if ( ( ( !startingCharacter_.empty( ) && stringOfData_.substr( 0, 1 ) .compare( startingCharacter_ ) != 0 ) || ( !skipKeyword_.empty( ) && stringOfData_.find( skipKeyword_ ) - == string::npos ) + == std::string::npos ) || ( startingCharacter_.empty( ) && skipKeyword_.empty( ) ) ) && !stringOfData_.empty( ) ) { @@ -207,7 +200,7 @@ void TwoLineElementsTextFileReader::stripEndOfLineCharacters( { // Declare local variables. // Declare string iterator. - string::iterator iteratorString_; + std::string::iterator iteratorString_; // Loop through all the strings stored in the container. for ( LineBasedStringDataMap::iterator iteratorContainerOfDataFromFile_ @@ -251,7 +244,7 @@ void TwoLineElementsTextFileReader::storeTwoLineElementData( ) twoLineElementData_.resize( numberOfObjects_ ); // Create vector of the three lines of a single object's TLE data as strings. - vector< string > twoLineElementString_( 3 ); + std::vector< std::string > twoLineElementString_( 3 ); // Create the object counter. unsigned int objectNumberCounter_ = 0; @@ -304,9 +297,9 @@ void TwoLineElementsTextFileReader::storeTwoLineElementData( ) } // Initiate a stringstream for each line. - stringstream line0StringStream_( stringstream::in | stringstream::out ); - stringstream line1StringStream_( stringstream::in | stringstream::out ); - stringstream line2StringStream_( stringstream::in | stringstream::out ); + std::stringstream line0StringStream_( std::stringstream::in | std::stringstream::out ); + std::stringstream line1StringStream_( std::stringstream::in | std::stringstream::out ); + std::stringstream line2StringStream_( std::stringstream::in | std::stringstream::out ); // Insert the strings into the stringstreams if ( numberOfLinesPerTwoLineElementDatum_ == 3 ) @@ -321,7 +314,7 @@ void TwoLineElementsTextFileReader::storeTwoLineElementData( ) if ( numberOfLinesPerTwoLineElementDatum_ == 3 ) { // Declare string containing part of name of object. - string namePart_; + std::string namePart_; // Loop through the stringstream and read words that constitute name of // object. Store name parts in objectName storage container. @@ -585,19 +578,19 @@ void TwoLineElementsTextFileReader::storeTwoLineElementData( ) } //! Checks the integrity of the TLE input file. -multimap< int, string > TwoLineElementsTextFileReader::checkTwoLineElementsFileIntegrity( ) +std::multimap< int, std::string > TwoLineElementsTextFileReader::checkTwoLineElementsFileIntegrity( ) { // Create vector of the three lines of a TLE as strings - vector< string > twoLineElementString_( 3 ); + std::vector< std::string > twoLineElementString_( 3 ); // Boolean which turns to true if one of the tests is not passed. bool isObjectErroneous = false; // Vector of object numbers of objects with corrupted TLE data. - vector< int > corruptedTwoLineElementDataPositions_; + std::vector< int > corruptedTwoLineElementDataPositions_; // Multimap of corrupted TLE errors. - multimap< int, string > corruptedTwoLineElementDataErrors_; + std::multimap< int, std::string > corruptedTwoLineElementDataErrors_; // Loop is started over the entire structure of objects, // multiple checks are performed per TLE, if a check is not passed boolean @@ -625,7 +618,7 @@ multimap< int, string > TwoLineElementsTextFileReader::checkTwoLineElementsFileI if ( twoLineElementData_.at( i ).lineNumberLine1 != 1 ) { corruptedTwoLineElementDataErrors_.insert( - pair< int, string >( i, "Incorrect line-1 leading integer." ) ); + std::pair< int, std::string >( i, "Incorrect line-1 leading integer." ) ); isObjectErroneous = true; } @@ -634,7 +627,7 @@ multimap< int, string > TwoLineElementsTextFileReader::checkTwoLineElementsFileI if ( twoLineElementData_.at( i ).lineNumberLine2 != 2 ) { corruptedTwoLineElementDataErrors_.insert( - pair< int, string >( i, "Incorrect line-2 leading integer." ) ); + std::pair< int, std::string >( i, "Incorrect line-2 leading integer." ) ); isObjectErroneous = true; } @@ -643,7 +636,7 @@ multimap< int, string > TwoLineElementsTextFileReader::checkTwoLineElementsFileI && twoLineElementData_.at( i ).tleClassification != 'C' ) { corruptedTwoLineElementDataErrors_.insert( - pair< int, string >( i, "Invalid TLE classification." ) ); + std::pair< int, std::string >( i, "Invalid TLE classification." ) ); isObjectErroneous = true; } @@ -652,7 +645,7 @@ multimap< int, string > TwoLineElementsTextFileReader::checkTwoLineElementsFileI if ( twoLineElementData_.at( i ).orbitalModel != 0 ) { corruptedTwoLineElementDataErrors_.insert( - pair< int, string >( i, "Incorrect orbital model." ) ); + std::pair< int, std::string >( i, "Incorrect orbital model." ) ); isObjectErroneous = true; } @@ -696,7 +689,7 @@ multimap< int, string > TwoLineElementsTextFileReader::checkTwoLineElementsFileI if ( line1Modulo10Sum_ != twoLineElementData_.at( i ).modulo10CheckSumLine1 ) { corruptedTwoLineElementDataErrors_.insert( - pair< int, string >( i, "Incorrect line-1 modulo-10 checksum." ) ); + std::pair< int, std::string >( i, "Incorrect line-1 modulo-10 checksum." ) ); isObjectErroneous = true; } @@ -726,7 +719,7 @@ multimap< int, string > TwoLineElementsTextFileReader::checkTwoLineElementsFileI if ( line2Modulo10Sum_ != twoLineElementData_.at( i ).modulo10CheckSumLine2 ) { corruptedTwoLineElementDataErrors_.insert( - pair< int, string >( i, "Incorrect line-2 modulo-10 checksum." ) ); + std::pair< int, std::string >( i, "Incorrect line-2 modulo-10 checksum." ) ); isObjectErroneous = true; } @@ -737,7 +730,7 @@ multimap< int, string > TwoLineElementsTextFileReader::checkTwoLineElementsFileI twoLineElementData_.at( i ).objectIdentificationNumberLine2 ) { corruptedTwoLineElementDataErrors_.insert( - pair< int, string >( + std::pair< int, std::string >( i, "Line-1 and line-2 object idenfitication number mismatch." ) ); isObjectErroneous = true; diff --git a/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestCoordinateConversions.cpp b/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestCoordinateConversions.cpp index b9d2de3c17..2e332c469b 100644 --- a/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestCoordinateConversions.cpp +++ b/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestCoordinateConversions.cpp @@ -64,6 +64,7 @@ #define BOOST_TEST_MAIN +#include #include #include @@ -595,6 +596,60 @@ BOOST_AUTO_TEST_CASE( test_SphericalToCartesianGradientConversion ) TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedCartesianGradient, cartesianGradient, 1e-15 ); } +// Test derivative of Cartesian gradient, keeping spherical gradient constant. +BOOST_AUTO_TEST_CASE( test_SphericalToCartesianGradientPartialDerivatives ) +{ + // Define an arbitrary spherical gradient. + const Eigen::Vector3d sphericalGradient( -2.438146967844150, 1.103964186650749e4, + 5.932496087870976e1 ); + + // Define an arbitrary spherical position vector. + const Eigen::Vector3d cartesianCoordinates( -7.0e6, 8.5e6, -6.5e6 ); + + + // Define perturbation for numerical computation of partial + const Eigen::Vector3d cartesianCoordinatePerturbation( 10.0, 10.0, 10.0 ); + Eigen::Vector3d perturbedCartesianCoordinates; + + std::vector< Eigen::Matrix3d > matrixPartials; + matrixPartials.resize( 3 ); + + std::vector< Eigen::Matrix3d > subMatrices; + subMatrices.resize( 3 ); + // Compute analytical partial (with sub-components) + coordinate_conversions::getDerivativeOfSphericalToCartesianGradient( sphericalGradient, cartesianCoordinates, subMatrices ); + + Eigen::Matrix3d uppPerturbedMatrix; + for( unsigned int i = 0; i < 3; i++ ) + { + // Compute partial numerically. + perturbedCartesianCoordinates = cartesianCoordinates; + perturbedCartesianCoordinates( i ) += cartesianCoordinatePerturbation( i ); + matrixPartials[ i ] = coordinate_conversions::getSphericalToCartesianGradientMatrix( + perturbedCartesianCoordinates ); + + perturbedCartesianCoordinates = cartesianCoordinates; + perturbedCartesianCoordinates( i ) -= cartesianCoordinatePerturbation( i ); + matrixPartials[ i ] -= coordinate_conversions::getSphericalToCartesianGradientMatrix( + perturbedCartesianCoordinates ); + + matrixPartials[ i ] /= ( 2.0 * cartesianCoordinatePerturbation( i ) ); + + // Normalize components for uniform tolerance. + matrixPartials[ i ].block( 0, 0, 3, 1 ) = matrixPartials[ i ].block( 0, 0, 3, 1 ) / cartesianCoordinates.norm( ); + subMatrices[ i ].block( 0, 0, 3, 1 ) = subMatrices[ i ].block( 0, 0, 3, 1 ) / cartesianCoordinates.norm( ); + + for( unsigned k = 0; k < 3; k++ ) + { + for( unsigned l = 0; l < 3; l++ ) + { + BOOST_CHECK_SMALL( std::fabs( matrixPartials[ i ]( k, l ) - subMatrices[ i ]( k, l ) ), 1.0E-23 ); + } + + } + } +} + // Test conversion from Cartesian (x, y, z, xdot, ydot, zdot) to Spherical (radius, azimuth, // elevation, radial velocity, azimuthal velocity, elevational velocity) state. BOOST_AUTO_TEST_CASE( testCartesianToSphericalStateConversion ) diff --git a/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestLegendrePolynomials.cpp b/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestLegendrePolynomials.cpp index fa9a62634b..5c88449cd8 100644 --- a/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestLegendrePolynomials.cpp +++ b/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestLegendrePolynomials.cpp @@ -44,6 +44,7 @@ #include #include +#include #include @@ -74,6 +75,7 @@ BOOST_AUTO_TEST_CASE( test_LegendrePolynomial ) { // Declare test values vector. Vector12d computedTestValues; + Vector12d computedTestValuesDirect; // Define degree and order vectors. const Vector12i degree @@ -81,14 +83,21 @@ BOOST_AUTO_TEST_CASE( test_LegendrePolynomial ) const Vector12i order = ( Eigen::VectorXi( 12 ) << 0, 0, 1, 0, 1, 2, 0, 1, 2, 3, 0, 150 ).finished( ); + basic_mathematics::LegendreCache legendreCache = basic_mathematics::LegendreCache( 150, 150, 0 ); + // Define polynomial parameter. const double polynomialParameter = 0.5; + legendreCache.update( polynomialParameter ); // Loop through degrees and orders. for ( int index = 0; index < degree.size( ); index++ ) { // Compute test value of Legendre polynomial. computedTestValues( index ) = basic_mathematics::computeLegendrePolynomial( + degree( index ), + order( index ), + legendreCache ); + computedTestValuesDirect( index ) = basic_mathematics::computeLegendrePolynomial( degree( index ), order( index ), polynomialParameter ); @@ -118,12 +127,16 @@ BOOST_AUTO_TEST_CASE( test_LegendrePolynomial ) // Check if test values match expected values. TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedValues, computedTestValues, 1.0e-14 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedValues, computedTestValuesDirect, 1.0e-14 ); } BOOST_AUTO_TEST_CASE( test_GeodesyLegendrePolynomial ) { // Declare test values vector. Vector10d computedTestValues; + Vector10d computedTestValuesDirect; + + basic_mathematics::LegendreCache legendreCache = basic_mathematics::LegendreCache( 4, 4, 1 ); // Define degree and order vectors. const Vector10i degree = ( Eigen::VectorXi( 10 ) << 0, 1, 1, 2, 2, 2, 3, 3, 3, 3 ).finished( ); @@ -132,14 +145,17 @@ BOOST_AUTO_TEST_CASE( test_GeodesyLegendrePolynomial ) // Define polynomial parameter. const double polynomialParameter = 0.5; + legendreCache.update( polynomialParameter ); // Loop through degrees and orders. for ( int index = 0; index < degree.size( ); index++ ) { // Compute test value of Legendre polynomial. computedTestValues( index ) = basic_mathematics::computeGeodesyLegendrePolynomial( degree( index ), - order( index ), - polynomialParameter ); + order( index ), legendreCache ); + computedTestValuesDirect( index ) = basic_mathematics::computeGeodesyLegendrePolynomial( + degree( index ), + order( index ), polynomialParameter ); } // Set expected values. These values have been obtained from the MATLAB subfunction @@ -160,6 +176,8 @@ BOOST_AUTO_TEST_CASE( test_GeodesyLegendrePolynomial ) // Check if test values match expected values. TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedValues, computedTestValues, 1.0e-15 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedValues, computedTestValuesDirect, 1.0e-15 ); + } BOOST_AUTO_TEST_CASE( test_LegendrePolynomialDerivative ) diff --git a/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestSphericalHarmonics.cpp b/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestSphericalHarmonics.cpp index 30b6a35c7d..86e0959da6 100644 --- a/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestSphericalHarmonics.cpp +++ b/Tudat/Mathematics/BasicMathematics/UnitTests/unitTestSphericalHarmonics.cpp @@ -128,11 +128,34 @@ BOOST_AUTO_TEST_CASE( test_SphericalHarmonics_PotentialGradient ) -2.717133139910520 ).finished( ); + boost::shared_ptr< basic_mathematics::SphericalHarmonicsCache > sphericalHarmonicsCache = + boost::make_shared< basic_mathematics::SphericalHarmonicsCache >( 4, 4 ); + sphericalHarmonicsCache->update( + sphericalPosition( 0 ), std::sin( sphericalPosition( 1 ) ), sphericalPosition( 2 ), referenceRadius ); + // Compute to be tested potential gradient. Eigen::MatrixXd testPotentialGradient( 10, 3 ); + Eigen::MatrixXd testPotentialGradient2( 10, 3 ); + Eigen::MatrixXd testPotentialGradient3( 10, 3 ); + + // Compute potential gradients (using each of the three functions). for ( int index = 0; index < degree.size( ); index++ ) { - Eigen::Vector3d placeholder = basic_mathematics::computePotentialGradient( + testPotentialGradient.block( index, 0, 1, 3 ) = basic_mathematics::computePotentialGradient( + sphericalPosition( 0 ), + std::pow( referenceRadius / sphericalPosition( 0 ), degree( index ) + 1 ), + std::cos( static_cast< double >( order( index ) ) * sphericalPosition( 2 ) ), + std::sin( static_cast< double >( order( index ) ) * sphericalPosition( 2 ) ), + std::cos( sphericalPosition( 1 ) ), + preMultiplier, + degree( index ), + order( index ), + cosineHarmonicCoefficient( index ), + sineHarmonicCoefficient( index ), + legendrePolynomial( index ), + legendrePolynomialDerivative( index ) ).transpose( ); + + testPotentialGradient2.block( index, 0, 1, 3 ) = basic_mathematics::computePotentialGradient( sphericalPosition, referenceRadius, preMultiplier, @@ -141,10 +164,20 @@ BOOST_AUTO_TEST_CASE( test_SphericalHarmonics_PotentialGradient ) cosineHarmonicCoefficient( index ), sineHarmonicCoefficient( index ), legendrePolynomial( index ), - legendrePolynomialDerivative( index ) ); + legendrePolynomialDerivative( index ) ).transpose( ); + + testPotentialGradient3.block( index, 0, 1, 3 ) = basic_mathematics::computePotentialGradient( + sphericalPosition, + preMultiplier, + degree( index ), + order( index ), + cosineHarmonicCoefficient( index ), + sineHarmonicCoefficient( index ), + legendrePolynomial( index ), + legendrePolynomialDerivative( index ), sphericalHarmonicsCache ).transpose( ); + + - // Transpose test values matrix. - testPotentialGradient.row( index ) = placeholder.transpose( ); } // Define expected radius gradient values. @@ -197,6 +230,9 @@ BOOST_AUTO_TEST_CASE( test_SphericalHarmonics_PotentialGradient ) // Check if test values match expected values. TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPotentialGradient, expectedValues, 1.0e-15 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPotentialGradient2, expectedValues, 1.0e-15 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPotentialGradient3, expectedValues, 1.0e-15 ); + } BOOST_AUTO_TEST_SUITE_END( ) diff --git a/Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h b/Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h index 8556c4a52a..1550d395bc 100644 --- a/Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h +++ b/Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h @@ -102,6 +102,51 @@ ScalarType computeModulo( const ScalarType dividend, const ScalarType divisor ) return dividend - divisor * std::floor( dividend / divisor ); } +//! Raise floating point variable to integer power. +template< typename ScalarType > +ScalarType raiseToIntegerPower( const ScalarType baseValue, + const int integerPower ) +{ + // Declare local variable. + // Declare result of raising base to integer power. + // Initialise with value. + ScalarType resultOfRaisingBaseToIntegerPower = 1; + // Declare absolute value of integerPower. + int absoluteValueOfIntegerPower + = std::abs( integerPower ); + // Declare copy of base value. + ScalarType copyOfBaseValue = baseValue; + + // Compute the result here using exponentiation by squares. + // Stop loop when absolute value of integer power is equal to zero. + while ( absoluteValueOfIntegerPower ) + { + // Check that absolute value of integer power. + if ( absoluteValueOfIntegerPower & 1 ) + { + // Compute intermediate result. + resultOfRaisingBaseToIntegerPower *= copyOfBaseValue; + } + + // Divide integer power by two. + absoluteValueOfIntegerPower >>= 1; + + // Square base value. + copyOfBaseValue *= copyOfBaseValue; + } + + // Check if sign of integerPower is negative. + if ( integerPower < 0 ) + { + // Switch sign of result. + resultOfRaisingBaseToIntegerPower + = 1.0 / resultOfRaisingBaseToIntegerPower; + } + + // Return result of raising base to integer power. + return resultOfRaisingBaseToIntegerPower; +} + } // namespace basic_mathematics } // namespace tudat diff --git a/Tudat/Mathematics/BasicMathematics/coordinateConversions.cpp b/Tudat/Mathematics/BasicMathematics/coordinateConversions.cpp index 4e56e6b3f1..d939ca006f 100644 --- a/Tudat/Mathematics/BasicMathematics/coordinateConversions.cpp +++ b/Tudat/Mathematics/BasicMathematics/coordinateConversions.cpp @@ -101,33 +101,6 @@ Eigen::Vector3d convertSphericalToCartesian( const Eigen::Vector3d& sphericalCoo return convertedCartesianCoordinates_; } -//! Convert Cartesian (x,y,z) to spherical (radius, zenith, azimuth) coordinates. -Eigen::Vector3d convertCartesianToSpherical( const Eigen::Vector3d& cartesianCoordinates ) -{ - // Create output Vector3d. - Eigen::Vector3d convertedSphericalCoordinates_ = Eigen::Vector3d::Zero( 3 ); - - // Compute transformation of Cartesian coordinates to spherical coordinates. - convertedSphericalCoordinates_( 0 ) = cartesianCoordinates.norm( ); - - // Check if coordinates are at origin. - if ( convertedSphericalCoordinates_( 0 ) < std::numeric_limits< double >::epsilon( ) ) - { - convertedSphericalCoordinates_( 1 ) = 0.0; - convertedSphericalCoordinates_( 2 ) = 0.0; - } - // Else compute coordinates using trigonometric relationships. - else - { - convertedSphericalCoordinates_( 1 ) = std::acos( cartesianCoordinates( 2 ) - / convertedSphericalCoordinates_( 0 ) ); - convertedSphericalCoordinates_( 2 ) = std::atan2( cartesianCoordinates( 1 ), - cartesianCoordinates( 0 ) ); - } - - return convertedSphericalCoordinates_; -} - //! Convert cylindrical to Cartesian coordinates. Eigen::Vector3d convertCylindricalToCartesian( const double radius, @@ -139,8 +112,7 @@ Eigen::Vector3d convertCylindricalToCartesian( const double radius, // If radius < 0, then give warning. if ( radius < 0.0 ) { - std::cerr << "Warning: cylindrical radial coordinate is negative!\n" - << "This could give incorrect results!\n"; + std::cerr << "Warning: cylindrical radial coordinate is negative!, This could give incorrect results!"<::epsilon( ) && std::fabs(cylindricalState( 4 )) > std::numeric_limits< double >::epsilon( ) ) { - std::cerr << "Warning: cylindrical velocity Vtheta (r*thetadot) does not equal zero while " - << "the radius (r) is zero! Vtheta is taken equal to zero!\n"; + std::cerr << "Warning: cylindrical velocity Vtheta (r*thetadot) does not equal zero while the radius (r) is zero! Vtheta is taken equal to zero!"<& subMatrices ) +{ + Eigen::Matrix3d totalPartialMatrix; + totalPartialMatrix.setZero( ); + + Eigen::Matrix3d currentPartialMatrix; + + // Precomputed quantities + double radius = cartesianCoordinates.norm( ); + const double xyDistanceSquared = cartesianCoordinates( 0 ) * cartesianCoordinates( 0 ) + + cartesianCoordinates( 1 ) * cartesianCoordinates( 1 ); + const double xyDistance = std::sqrt( xyDistanceSquared ); + const double radiusSquaredXyDistance = xyDistance * radius * radius; + + // Precompute partials + Eigen::Vector3d oneOverRPartial = -cartesianCoordinates / ( radius * radius * radius ); + Eigen::Vector3d oneOverRSquaredPartial = -2.0 * cartesianCoordinates / ( radius * radius * radius * radius ); + Eigen::Vector3d oneOverXyDistancePartial = + -( Eigen::Vector3d( ) << cartesianCoordinates( 0 ), cartesianCoordinates( 1 ), 0.0 ).finished( )/ + ( xyDistanceSquared * xyDistance ); + Eigen::Vector3d oneOverXyDistanceSquaredPartial = + -2.0 * ( Eigen::Vector3d( ) << cartesianCoordinates( 0 ), cartesianCoordinates( 1 ), 0.0 ).finished( )/ + ( xyDistanceSquared * xyDistanceSquared ); + Eigen::Vector3d oneOverRSquaredXyDistancePartial = + oneOverRSquaredPartial / xyDistance + oneOverXyDistancePartial / ( radius * radius ); + + + Eigen::Vector3d xyDistancePartial = + ( Eigen::Vector3d( ) << cartesianCoordinates( 0 ), cartesianCoordinates( 1 ), 0.0 ).finished( ) / xyDistance; + + // Compute partials w.r.t x, y and z components. + for( unsigned int i = 0; i < 3; i++ ) + { + currentPartialMatrix.setZero( ); + switch( i ) + { + case 0: + { + currentPartialMatrix << 1.0 / radius + cartesianCoordinates( 0 ) * oneOverRPartial ( 0 ), + - cartesianCoordinates( 2 ) / radiusSquaredXyDistance - + cartesianCoordinates( 0 ) * cartesianCoordinates( 2 ) * oneOverRSquaredXyDistancePartial( 0 ), + - cartesianCoordinates( 1 ) * oneOverXyDistanceSquaredPartial( 0 ), + cartesianCoordinates( 1 ) * oneOverRPartial( 0 ), + - cartesianCoordinates( 1 ) * cartesianCoordinates( 2 ) * oneOverRSquaredXyDistancePartial( 0 ), + 1.0 / ( xyDistanceSquared ) + cartesianCoordinates( 0 ) * oneOverXyDistanceSquaredPartial( 0 ), + cartesianCoordinates( 2 ) * oneOverRPartial( 0 ), + xyDistance * oneOverRSquaredPartial( 0 ) + 1.0 / ( radius * radius ) * xyDistancePartial( 0 ), + 0.0 ; + break; + } + case 1: + { + currentPartialMatrix << cartesianCoordinates( 0 ) * oneOverRPartial ( 1 ), + - cartesianCoordinates( 0 ) * cartesianCoordinates( 2 ) * oneOverRSquaredXyDistancePartial( 1 ), + -1.0 / ( xyDistanceSquared ) - cartesianCoordinates( 1 ) * oneOverXyDistanceSquaredPartial( 1 ), + 1.0 / radius + cartesianCoordinates( 1 ) * oneOverRPartial ( 1 ), + - cartesianCoordinates( 2 ) / radiusSquaredXyDistance - + cartesianCoordinates( 1 ) * cartesianCoordinates( 2 ) * oneOverRSquaredXyDistancePartial( 1 ), + cartesianCoordinates( 0 ) * oneOverXyDistanceSquaredPartial( 1 ), + cartesianCoordinates( 2 ) * oneOverRPartial( 1 ), + xyDistance * oneOverRSquaredPartial( 1 ) + 1.0 / ( radius * radius ) * xyDistancePartial( 1 ), + 0.0 ; + break; + } + case 2: + { + currentPartialMatrix << cartesianCoordinates( 0 ) * oneOverRPartial ( 2 ), + - cartesianCoordinates( 0 ) / radiusSquaredXyDistance - + cartesianCoordinates( 0 ) * cartesianCoordinates( 2 ) * oneOverRSquaredXyDistancePartial( 2 ), + - cartesianCoordinates( 1 ) * oneOverXyDistanceSquaredPartial( 2 ), + cartesianCoordinates( 1 ) * oneOverRPartial( 2 ), + - cartesianCoordinates( 1 ) / radiusSquaredXyDistance - + cartesianCoordinates( 1 ) * cartesianCoordinates( 2 ) * oneOverRSquaredXyDistancePartial( 2 ), + cartesianCoordinates( 0 ) * oneOverXyDistanceSquaredPartial( 2 ), + 1.0 / radius + cartesianCoordinates( 2 ) * oneOverRPartial( 2 ), + xyDistance * oneOverRSquaredPartial( 2 ) + + 1.0 / ( radius * radius ) * xyDistancePartial( 2 ), + 0.0 ; + break; + } + } + + // Save computed matrix + if( subMatrices.size( ) == 3 ) + { + subMatrices[ i ] = currentPartialMatrix; + } + + // Add current entry to results. + totalPartialMatrix.block( 0, i, 3, 1 ) = currentPartialMatrix * sphericalGradient; + } + + return totalPartialMatrix; +} + +Eigen::Matrix3d getDerivativeOfSphericalToCartesianGradient( const Eigen::Vector3d& sphericalGradient, + const Eigen::Vector3d& cartesianCoordinates ) +{ + static std::vector< Eigen::Matrix3d > subMatrices( 3 ); + return getDerivativeOfSphericalToCartesianGradient( + sphericalGradient, cartesianCoordinates, subMatrices ); } //! Convert spherical to Cartesian state. diff --git a/Tudat/Mathematics/BasicMathematics/coordinateConversions.h b/Tudat/Mathematics/BasicMathematics/coordinateConversions.h index 97cc200ecb..9a522dcd3a 100644 --- a/Tudat/Mathematics/BasicMathematics/coordinateConversions.h +++ b/Tudat/Mathematics/BasicMathematics/coordinateConversions.h @@ -66,9 +66,12 @@ #ifndef TUDAT_COORDINATE_CONVERSIONS_H #define TUDAT_COORDINATE_CONVERSIONS_H +#include + #include #include "Tudat/Mathematics/BasicMathematics/linearAlgebraTypes.h" +#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h" namespace tudat { @@ -107,7 +110,33 @@ Eigen::Vector3d convertSphericalToCartesian( const Eigen::Vector3d& sphericalCoo * \return Vector containing sphericalCoordinates radius, zenith and azimuth (in that * order), as calculated from sphericalCoordinates. */ -Eigen::Vector3d convertCartesianToSpherical( const Eigen::Vector3d& cartesianCoordinates ); +template< typename ScalarType = double > +Eigen::Matrix< ScalarType, 3, 1 > convertCartesianToSpherical( const Eigen::Matrix< ScalarType, 3, 1 > & cartesianCoordinates ) + +{ + // Create output Vector3d. + Eigen::Matrix< ScalarType, 3, 1 > convertedSphericalCoordinates_ = Eigen::Matrix< ScalarType, 3, 1 >::Zero( ); + + // Compute transformation of Cartesian coordinates to spherical coordinates. + convertedSphericalCoordinates_( 0 ) = cartesianCoordinates.norm( ); + + // Check if coordinates are at origin. + if ( convertedSphericalCoordinates_( 0 ) < std::numeric_limits< ScalarType >::epsilon( ) ) + { + convertedSphericalCoordinates_( 1 ) = mathematical_constants::getFloatingInteger< ScalarType >( 0 ); + convertedSphericalCoordinates_( 2 ) = mathematical_constants::getFloatingInteger< ScalarType >( 0 ); + } + // Else compute coordinates using trigonometric relationships. + else + { + convertedSphericalCoordinates_( 1 ) = std::acos( cartesianCoordinates( 2 ) + / convertedSphericalCoordinates_( 0 ) ); + convertedSphericalCoordinates_( 2 ) = std::atan2( cartesianCoordinates( 1 ), + cartesianCoordinates( 0 ) ); + } + + return convertedSphericalCoordinates_; +} //! Spherical coordinate indices. /*! @@ -248,6 +277,14 @@ Eigen::Vector3d convertCartesianToCylindrical( const Eigen::Vector3d& cartesianC basic_mathematics::Vector6d convertCartesianToCylindricalState( const basic_mathematics::Vector6d& cartesianState ); +//! Compute matrix by which to precompute a spherical gradient vector to obtain the Cartesian gradient +/*! + * Compute matrix by which to precompute a spherical gradient vector to obtain the Cartesian gradient + * \param cartesianCoordinates Vector with Cartesian position at which gradient is computed. + * \return Matrix by which to precompute a spherical gradient vector to obtain the Cartesian gradient + */ +Eigen::Matrix3d getSphericalToCartesianGradientMatrix( const Eigen::Vector3d& cartesianCoordinates ); + //! Convert spherical to Cartesian gradient. /*! * This function converts a gradient vector with respect to spherical coordinates to a gradient @@ -290,6 +327,33 @@ basic_mathematics::Vector6d convertCartesianToCylindricalState( Eigen::Vector3d convertSphericalToCartesianGradient( const Eigen::Vector3d& sphericalGradient, const Eigen::Vector3d& cartesianCoordinates ); +//! Function to compute the derivative of the Cartesian gradient w.r.t. the Cartesian position, keeping the +//! spherical gradient constant. +/*! + * Function to compute the derivative of the Cartesian gradient w.r.t. the Cartesian position, keeping the + * spherical gradient constant. + * \param sphericalGradient Value of spherical gradient (derivatives w.r.t. radius, latitude and longitude). + * \param cartesianCoordinates Cartesian position at whichr result is to be computed. + * \param subMatrices Subcomputations performed by this function: derivatives of results of + * getSphericalToCartesianGradientMatrix, w.r.t. x, y and z component of cartesianCoordinates, respectively. + * \return Require derivative of cartesian gradient. + */ +Eigen::Matrix3d getDerivativeOfSphericalToCartesianGradient( const Eigen::Vector3d& sphericalGradient, + const Eigen::Vector3d& cartesianCoordinates, + std::vector< Eigen::Matrix3d >& subMatrices ); + +//! Function to compute the derivative of the Cartesian gradient w.r.t. the Cartesian position, keeping the +//! spherical gradient constant. +/*! + * Function to compute the derivative of the Cartesian gradient w.r.t. the Cartesian position, keeping the + * spherical gradient constant. + * \param sphericalGradient Value of spherical gradient (derivatives w.r.t. radius, latitude and longitude). + * \param cartesianCoordinates Cartesian position at whichr result is to be computed.. + * \return Require derivative of cartesian gradient. + */ +Eigen::Matrix3d getDerivativeOfSphericalToCartesianGradient( const Eigen::Vector3d& sphericalGradient, + const Eigen::Vector3d& cartesianCoordinates ); + //! Convert spherical to Cartesian state. /*! * Converts a spherical state to a Cartesian state. The transformation matrices are computed diff --git a/Tudat/Mathematics/BasicMathematics/legendrePolynomials.cpp b/Tudat/Mathematics/BasicMathematics/legendrePolynomials.cpp index 8a636d40ea..78ea4fd988 100644 --- a/Tudat/Mathematics/BasicMathematics/legendrePolynomials.cpp +++ b/Tudat/Mathematics/BasicMathematics/legendrePolynomials.cpp @@ -48,16 +48,300 @@ namespace tudat namespace basic_mathematics { -// Define maximum size of Legendre polynomials back-end cache. -#ifndef MAXIMUM_CACHE_ENTRIES -#define MAXIMUM_CACHE_ENTRIES 12000 -#endif + + +//! Default constructor, initializes cache object with 0 maximum degree and order. +LegendreCache::LegendreCache( const bool useGeodesyNormalization ) +{ + useGeodesyNormalization_ = useGeodesyNormalization; + + if( useGeodesyNormalization_ ) + { + legendrePolynomialFunction_ = geodesyNormalizedLegendrePolynomialFunction; + } + else + { + legendrePolynomialFunction_ = regularLegendrePolynomialFunction; + } + + resetMaximumDegreeAndOrder( 0, 0 ); + computeSecondDerivatives_ = 0; + +} + +//! Constructor +LegendreCache::LegendreCache( const int maximumDegree, const int maximumOrder, const bool useGeodesyNormalization ) +{ + useGeodesyNormalization_ = useGeodesyNormalization; + + if( useGeodesyNormalization_ ) + { + legendrePolynomialFunction_ = geodesyNormalizedLegendrePolynomialFunction; + } + else + { + legendrePolynomialFunction_ = regularLegendrePolynomialFunction; + } + + resetMaximumDegreeAndOrder( maximumDegree, maximumOrder ); + computeSecondDerivatives_ = 0; +} + +//! Get Legendre polynomial from cache when possible, and from direct computation otherwise. +void LegendreCache::update( const double polynomialParameter ) +{ + // Check if cache needs update + if( !( polynomialParameter == currentPolynomialParameter_ ) ) + { + currentPolynomialParameter_ = polynomialParameter; + + // Set complement of argument (assuming it to be sine of latitude) cosine of latitude is always positive. + currentPolynomialParameterComplement_ = std::sqrt( 1.0 - polynomialParameter * polynomialParameter ); + + LegendreCache& thisReference = *this; + + int jMax = -1; + for( int i = 0; i <= maximumDegree_; i++ ) + { + jMax = std::min( i, maximumOrder_ ); + for( int j = 0; j <= jMax ; j++ ) + { + // Compute legendre polynomial + legendreValues_[ i * ( maximumOrder_ + 1 ) + j ] = legendrePolynomialFunction_( i, j, thisReference ); + + if( j != 0 ) + { + // Compute legendre polynomial derivative + if( useGeodesyNormalization_ ) + { + legendreDerivatives_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ] = + computeGeodesyLegendrePolynomialDerivative( + i, j - 1, currentPolynomialParameter_, + legendreValues_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ], + legendreValues_[ i * ( maximumOrder_ + 1 ) + j ], + derivativeNormalizations_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ] ); + } + else + { + legendreDerivatives_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ] = + computeLegendrePolynomialDerivative( + j - 1, currentPolynomialParameter_, + legendreValues_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ], + legendreValues_[ i * ( maximumOrder_ + 1 ) + j ] ); + } + + } + } + + // Compute legendre polynomial derivative for i = j (if needed) + if( jMax == i ) + { + if( useGeodesyNormalization_ ) + { + legendreDerivatives_[ i * ( maximumOrder_ + 1 ) + jMax ] = + computeGeodesyLegendrePolynomialDerivative( + i, jMax, currentPolynomialParameter_, + legendreValues_[ i * ( maximumOrder_ + 1 ) + jMax ], 0.0, + derivativeNormalizations_[ i * ( maximumOrder_ + 1 ) + jMax ] ); + } + else + { + legendreDerivatives_[ i * ( maximumOrder_ + 1 ) + jMax ] = + computeLegendrePolynomialDerivative( + jMax, currentPolynomialParameter_, + legendreValues_[ i * ( maximumOrder_ + 1 ) + jMax ], 0.0 ); + } + } + } + + // Compute second derivatives of Legendre polynomials if needed + if( computeSecondDerivatives_ ) + { + for( int i = 0; i <= maximumDegree_; i++ ) + { + jMax = std::min( i, maximumOrder_ ); + for( int j = 0; j <= jMax ; j++ ) + { + if( j != 0 ) + { + // Compute legendre polynomial second derivatives + if( useGeodesyNormalization_ ) + { + legendreSecondDerivatives_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ] = + computeGeodesyLegendrePolynomialSecondDerivative( + i, j - 1, currentPolynomialParameter_, + legendreValues_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ], + legendreValues_[ i * ( maximumOrder_ + 1 ) + j ], + legendreDerivatives_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ], + legendreDerivatives_[ i * ( maximumOrder_ + 1 ) + j ], + derivativeNormalizations_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ] ); + } + else + { + legendreSecondDerivatives_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ] = + computeGeodesyLegendrePolynomialSecondDerivative( + i, j - 1, currentPolynomialParameter_, + legendreValues_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ], + legendreValues_[ i * ( maximumOrder_ + 1 ) + j ], + legendreDerivatives_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ], + legendreDerivatives_[ i * ( maximumOrder_ + 1 ) + j ], 1.0 ); + } + + } + } + // Compute legendre polynomial second derivative for i = j (if needed) + if( jMax == i ) + { + if( useGeodesyNormalization_ ) + { + legendreSecondDerivatives_[ i * ( maximumOrder_ + 1 ) + jMax ] = + computeGeodesyLegendrePolynomialSecondDerivative( + i, jMax, currentPolynomialParameter_, + legendreValues_[ i * ( maximumOrder_ + 1 ) + jMax ], 0.0, + legendreDerivatives_[ i * ( maximumOrder_ + 1 ) + jMax ], 0.0, + derivativeNormalizations_[ i * ( maximumOrder_ + 1 ) + jMax ] ); + } + else + { + legendreSecondDerivatives_[ i * ( maximumOrder_ + 1 ) + jMax ] = + computeGeodesyLegendrePolynomialSecondDerivative( + i, jMax, currentPolynomialParameter_, + legendreValues_[ i * ( maximumOrder_ + 1 ) + jMax ], 0.0, + legendreDerivatives_[ i * ( maximumOrder_ + 1 ) + jMax ], 0.0, + 1.0 ); + } + } + + } + } + } +} + +//! Update maximum degree and order of cache +void LegendreCache::resetMaximumDegreeAndOrder( const int maximumDegree, const int maximumOrder ) +{ + maximumDegree_ = maximumDegree; + maximumOrder_ = maximumOrder; + + if( maximumOrder_ > maximumDegree_ ) + { + maximumOrder_ = maximumDegree_; + } + + legendreValues_.resize( ( maximumDegree_ + 1 ) * ( maximumOrder_ + 1 ) ); + legendreDerivatives_.resize( ( maximumDegree_ + 1 ) * ( maximumOrder_ + 1 ) ); + legendreSecondDerivatives_.resize( ( maximumDegree_ + 1 ) * ( maximumOrder_ + 1 ) ); + + derivativeNormalizations_.resize( ( maximumDegree_ + 1 ) * ( maximumOrder_ + 1 ) ); + + for( int i = 0; i <= maximumDegree_; i++ ) + { + for( int j = 0; ( ( j <= i ) && ( j <= maximumOrder_ ) ) ; j++ ) + { + // Compute normalization correction factor. + derivativeNormalizations_[ i * ( maximumOrder_ + 1 ) + j ] = std::sqrt( + ( static_cast< double >( i + j + 1 ) ) + * ( static_cast< double >( i - j ) ) ); + + // If order is zero apply multiplication factor. + if ( j == 0 ) + { + derivativeNormalizations_[ i * ( maximumOrder_ + 1 ) + j ] *= std::sqrt( 0.5 ); + } + } + } + + currentPolynomialParameter_ = TUDAT_NAN; + currentPolynomialParameterComplement_ = TUDAT_NAN; +} + + +//! Get Legendre polynomial value from the cache. +double LegendreCache::getLegendrePolynomial( + const int degree, const int order ) +{ + if( degree > maximumDegree_ || order > maximumOrder_ ) + { + std::string errorMessage = "Error when requesting legendre cache, maximum degree or order exceeded " + + boost::lexical_cast< std::string >( degree ) + " " + + boost::lexical_cast< std::string >( maximumDegree_ ) + " " + + boost::lexical_cast< std::string >( order ) + " " + + boost::lexical_cast< std::string >( maximumOrder_ ); + throw std::runtime_error( errorMessage ); + return TUDAT_NAN; + } + else if( order > degree ) + { + return 0.0; + } + else + { + return legendreValues_[ degree * ( maximumOrder_ + 1 ) + order ]; + }; +} + +//! Get first derivative of Legendre polynomial value from the cache. +double LegendreCache::getLegendrePolynomialDerivative( + const int degree, const int order ) +{ + if( degree > ( maximumDegree_ ) || order > maximumOrder_ ) + { + std::string errorMessage = "Error when requesting legendre cache first derivatives, maximum degree or order exceeded " + + boost::lexical_cast< std::string >( degree ) + " " + + boost::lexical_cast< std::string >( maximumDegree_ ) + " " + + boost::lexical_cast< std::string >( order ) + " " + + boost::lexical_cast< std::string >( maximumOrder_ ); + throw std::runtime_error( errorMessage ); + return TUDAT_NAN; + } + else if( order > degree ) + { + return 0.0; + } + else + { + return legendreDerivatives_[ degree * ( maximumOrder_ + 1 ) + order ]; + }; +} + +//! Get second derivative of Legendre polynomial value from the cache. +double LegendreCache::getLegendrePolynomialSecondDerivative( + const int degree, const int order ) +{ + if( degree > ( maximumDegree_ ) || order > maximumOrder_ ) + { + std::string errorMessage = "Error when requesting legendre cache second derivatives, maximum degree or order exceeded " + + boost::lexical_cast< std::string >( degree ) + " " + + boost::lexical_cast< std::string >( maximumDegree_ ) + " " + + boost::lexical_cast< std::string >( order ) + " " + + boost::lexical_cast< std::string >( maximumOrder_ ); + throw std::runtime_error( errorMessage ); + return TUDAT_NAN; + } + else if( computeSecondDerivatives_ == 0 ) + { + throw std::runtime_error( "Error when requesting legendre cache second derivatives, no computations performed" ); + } + else if( order > degree ) + { + return 0.0; + } + else + { + return legendreSecondDerivatives_[ degree * ( maximumOrder_ + 1 ) + order ]; + }; +} //! Compute unnormalized associated Legendre polynomial. double computeLegendrePolynomial( const int degree, const int order, - const double polynomialParameter ) + LegendreCache& legendreCache ) { + if( legendreCache.getUseGeodesyNormalization( ) ) + { + throw std::runtime_error( "Error when computing Legendre polynomial, input uses normalization" ); + } + // If degree or order is negative... if ( degree < 0 || order < 0 ) { @@ -82,19 +366,19 @@ double computeLegendrePolynomial( const int degree, else if ( degree <= 1 && order <= 1 ) { // Compute polynomial explicitly. - return computeLegendrePolynomialExplicit( degree, order, polynomialParameter ); + return computeLegendrePolynomialExplicit( degree, order, legendreCache.getCurrentPolynomialParameter( ) ); } // Else if degree and order are sectoral... else if ( degree == order ) { // Obtain polynomial of degree one and order one. - const double degreeOneOrderOnePolynomial = legendreCache.getOrElseUpdate( - 1, 1, polynomialParameter, &computeLegendrePolynomial ); + const double degreeOneOrderOnePolynomial = legendreCache.getLegendrePolynomial( + 1, 1 ); // Obtain prior sectoral polynomial. - const double priorSectoralPolynomial = legendreCache.getOrElseUpdate( - degree - 1, order - 1, polynomialParameter, &computeLegendrePolynomial ); + const double priorSectoralPolynomial = legendreCache.getLegendrePolynomial( + degree - 1, order - 1 ); // Compute polynomial. return computeLegendrePolynomialDiagonal( @@ -105,27 +389,44 @@ double computeLegendrePolynomial( const int degree, else { // Obtain prior degree polynomial. - const double oneDegreePriorPolynomial = legendreCache.getOrElseUpdate( - degree - 1, order, polynomialParameter, &computeLegendrePolynomial ); + const double oneDegreePriorPolynomial = legendreCache.getLegendrePolynomial( + degree - 1, order ); // Obtain two degrees prior polynomial. - const double twoDegreesPriorPolynomial = legendreCache.getOrElseUpdate( - degree - 2, order, polynomialParameter, &computeLegendrePolynomial ); + const double twoDegreesPriorPolynomial = legendreCache.getLegendrePolynomial( + degree - 2, order ); // Compute polynomial. return computeLegendrePolynomialVertical( degree, order, - polynomialParameter, + legendreCache.getCurrentPolynomialParameter( ), oneDegreePriorPolynomial, twoDegreesPriorPolynomial ); } } + +double computeLegendrePolynomial( const int degree, + const int order, + const double legendreParameter ) +{ + LegendreCache legendreCache( degree, order, 0 ); + legendreCache.update( legendreParameter ); + return computeLegendrePolynomial( degree, order, legendreCache ); +} + + //! Compute geodesy-normalized associated Legendre polynomial. double computeGeodesyLegendrePolynomial( const int degree, const int order, - const double polynomialParameter ) + LegendreCache& geodesyLegendreCache ) { + + if( !geodesyLegendreCache.getUseGeodesyNormalization( ) ) + { + throw std::runtime_error( "Error when computing Legendre polynomial, input uses no normalization" ); + } + // If degree or order is negative... if ( degree < 0 || order < 0 ) { @@ -150,19 +451,19 @@ double computeGeodesyLegendrePolynomial( const int degree, else if ( degree <= 1 && order <= 1 ) { // Compute polynomial explicitly. - return computeGeodesyLegendrePolynomialExplicit( degree, order, polynomialParameter ); + return computeGeodesyLegendrePolynomialExplicit( degree, order, geodesyLegendreCache.getCurrentPolynomialParameter( ) ); } // Else if degree and order are sectoral... else if ( degree == order ) { // Obtain polynomial of degree one and order one. - double degreeOneOrderOnePolynomial = geodesyLegendreCache.getOrElseUpdate( - 1, 1, polynomialParameter, &computeGeodesyLegendrePolynomial ); + double degreeOneOrderOnePolynomial = geodesyLegendreCache.getLegendrePolynomial( + 1, 1 ); // Obtain prior sectoral polynomial. - double priorSectoralPolynomial = geodesyLegendreCache.getOrElseUpdate( - degree - 1, order - 1, polynomialParameter, &computeGeodesyLegendrePolynomial ); + double priorSectoralPolynomial = geodesyLegendreCache.getLegendrePolynomial( + degree - 1, order - 1 ); // Compute polynomial. return computeGeodesyLegendrePolynomialDiagonal( @@ -173,22 +474,31 @@ double computeGeodesyLegendrePolynomial( const int degree, else { // Obtain prior degree polynomial. - double oneDegreePriorPolynomial = geodesyLegendreCache.getOrElseUpdate( - degree - 1, order, polynomialParameter, &computeGeodesyLegendrePolynomial ); + double oneDegreePriorPolynomial = geodesyLegendreCache.getLegendrePolynomial( + degree - 1, order ); // Obtain two degrees prior polynomial. - double twoDegreesPriorPolynomial = geodesyLegendreCache.getOrElseUpdate( - degree - 2, order, polynomialParameter, &computeGeodesyLegendrePolynomial ); + double twoDegreesPriorPolynomial = geodesyLegendreCache.getLegendrePolynomial( + degree - 2, order ); // Compute polynomial. return computeGeodesyLegendrePolynomialVertical( degree, order, - polynomialParameter, + geodesyLegendreCache.getCurrentPolynomialParameter( ), oneDegreePriorPolynomial, twoDegreesPriorPolynomial ); } } +double computeGeodesyLegendrePolynomial( const int degree, + const int order, + const double legendreParameter ) +{ + LegendreCache legendreCache( degree, order, 1 ); + legendreCache.update( legendreParameter ); + return computeGeodesyLegendrePolynomial( degree, order, legendreCache ); +} + //! Compute derivative of unnormalized Legendre polynomial. double computeLegendrePolynomialDerivative( const int order, const double polynomialParameter, @@ -203,6 +513,22 @@ double computeLegendrePolynomialDerivative( const int order, * currentLegendrePolynomial; } +//! Compute derivative of geodesy-normalized Legendre polynomial. +double computeGeodesyLegendrePolynomialDerivative( const int degree, + const int order, + const double polynomialParameter, + const double currentLegendrePolynomial, + const double incrementedLegendrePolynomial, + const double normalizationCorrection ) +{ + // Return polynomial derivative. + return normalizationCorrection * incrementedLegendrePolynomial + / std::sqrt( 1.0 - polynomialParameter * polynomialParameter ) + - static_cast< double >( order ) * polynomialParameter + / ( 1.0 - polynomialParameter * polynomialParameter ) + * currentLegendrePolynomial; +} + //! Compute derivative of geodesy-normalized Legendre polynomial. double computeGeodesyLegendrePolynomialDerivative( const int degree, const int order, @@ -211,9 +537,9 @@ double computeGeodesyLegendrePolynomialDerivative( const int degree, const double incrementedLegendrePolynomial ) { // Compute normalization correction factor. - double normalizationCorrection = std::sqrt( ( static_cast< double >( degree ) - + static_cast< double >( order ) + 1.0 ) - * ( static_cast< double >( degree - order ) ) ); + double normalizationCorrection = std::sqrt( + ( static_cast< double >( degree + order + 1 ) ) + * ( static_cast< double >( degree - order ) ) ); // If order is zero apply multiplication factor. if ( order == 0 ) @@ -222,13 +548,33 @@ double computeGeodesyLegendrePolynomialDerivative( const int degree, } // Return polynomial derivative. - return normalizationCorrection * incrementedLegendrePolynomial - / std::sqrt( 1.0 - polynomialParameter * polynomialParameter ) - - static_cast< double >( order ) * polynomialParameter - / ( 1.0 - polynomialParameter * polynomialParameter ) - * currentLegendrePolynomial; + return computeGeodesyLegendrePolynomialDerivative( + degree, order, polynomialParameter, currentLegendrePolynomial, + incrementedLegendrePolynomial, normalizationCorrection ); +} + +//! Compute second derivative of geodesy-normalized associated Legendre polynomial. +double computeGeodesyLegendrePolynomialSecondDerivative( const int degree, + const int order, + const double polynomialParameter, + const double currentLegendrePolynomial, + const double incrementedLegendrePolynomial, + const double currentLegendrePolynomialDerivative, + const double incrementedLegendrePolynomialDerivative, + const double normalizationCorrection ) +{ + double polynomialParameterSquare = polynomialParameter * polynomialParameter; + + // Return polynomial derivative. + return normalizationCorrection * ( + incrementedLegendrePolynomialDerivative / std::sqrt( 1.0 - polynomialParameterSquare ) + + polynomialParameter * std::pow( 1.0 - polynomialParameterSquare, -1.5 ) * incrementedLegendrePolynomial ) - + static_cast< double >( order ) * + ( polynomialParameter / ( 1.0 - polynomialParameter * polynomialParameter ) * currentLegendrePolynomialDerivative + + ( 1.0 + polynomialParameterSquare ) / ( ( 1.0 - polynomialParameterSquare ) * ( 1.0 - polynomialParameterSquare ) ) * currentLegendrePolynomial ); } + //! Compute low degree/order unnormalized Legendre polynomial explicitly. double computeLegendrePolynomialExplicit( const int degree, const int order, @@ -243,8 +589,12 @@ double computeLegendrePolynomialExplicit( const int degree, case 0: return 1.0; default: - std::cerr << "Error, explicit legendre polynomial not possible for " - << degree << " " << order << std::endl; + { + std::string errorMessage = "Error, explicit legendre polynomial not possible for " + + boost::lexical_cast< std::string >( degree ) + ", " + + boost::lexical_cast< std::string >( order ); + throw std::runtime_error( errorMessage ); + } } break; case 1: @@ -255,8 +605,12 @@ double computeLegendrePolynomialExplicit( const int degree, case 1: return std::sqrt( 1 - polynomialParameter * polynomialParameter ); default: - std::cerr << "Error, explicit legendre polynomial not possible for " - << degree << " " << order << std::endl; + { + std::string errorMessage = "Error, explicit legendre polynomial not possible for " + + boost::lexical_cast< std::string >( degree ) + ", " + + boost::lexical_cast< std::string >( order ); + throw std::runtime_error( errorMessage ); + } } break; case 2: @@ -270,8 +624,12 @@ double computeLegendrePolynomialExplicit( const int degree, case 2: return 3.0 * ( 1.0 - polynomialParameter * polynomialParameter ); default: - std::cerr << "Error, explicit legendre polynomial not possible for " - << degree << " " << order << std::endl; + { + std::string errorMessage = "Error, explicit legendre polynomial not possible for " + + boost::lexical_cast< std::string >( degree ) + ", " + + boost::lexical_cast< std::string >( order ); + throw std::runtime_error( errorMessage ); + } } break; case 3: @@ -289,8 +647,12 @@ double computeLegendrePolynomialExplicit( const int degree, return 15.0 * ( 1.0 - polynomialParameter * polynomialParameter ) * std::sqrt( 1.0 - polynomialParameter * polynomialParameter ); default: - std::cerr << "Error, a explicit legendre polynomial not possible for " - << degree << " " << order << std::endl; + { + std::string errorMessage = "Error, explicit legendre polynomial not possible for " + + boost::lexical_cast< std::string >( degree ) + ", " + + boost::lexical_cast< std::string >( order ); + throw std::runtime_error( errorMessage ); + } } break; case 4: @@ -315,13 +677,21 @@ double computeLegendrePolynomialExplicit( const int degree, * ( 1.0 - polynomialParameter * polynomialParameter ); default: - std::cerr << "Error, a explicit legendre polynomial not possible for " - << degree << " " << order << std::endl; + { + std::string errorMessage = "Error, explicit legendre polynomial not possible for " + + boost::lexical_cast< std::string >( degree ) + ", " + + boost::lexical_cast< std::string >( order ); + throw std::runtime_error( errorMessage ); + } } break; default: - std::cerr << "Error, explicit legendre polynomial not possible for " - << degree << " " << order << std::endl; + { + std::string errorMessage = "Error, explicit legendre polynomial not possible for " + + boost::lexical_cast< std::string >( degree ) + ", " + + boost::lexical_cast< std::string >( order ); + throw std::runtime_error( errorMessage ); + } } return TUDAT_NAN; } @@ -355,7 +725,7 @@ double computeGeodesyLegendrePolynomialExplicit( const int degree, // Set error message. std::stringstream errorMessage; errorMessage << "Error: computation of Legendre polynomial of = " << degree - << " and order = " << order << " is not supported." << std::endl; + << " and order = " << order << " is not supported." << std::endl; // Throw a run-time error. boost::throw_exception( boost::enable_error_info( std::runtime_error( @@ -407,120 +777,14 @@ double computeGeodesyLegendrePolynomialVertical( const int degree, { // Return polynomial. return std::sqrt( ( 2.0 * static_cast< double >( degree ) + 1.0 ) - / ( static_cast< double >( degree + order ) ) - / ( static_cast< double >( degree - order ) ) ) - * ( std::sqrt( 2.0 * static_cast< double >( degree ) - 1.0 ) * polynomialParameter - * oneDegreePriorPolynomial + / ( ( static_cast< double >( degree + order ) ) * ( static_cast< double >( degree - order ) ) ) ) + * ( std::sqrt( 2.0 * static_cast< double >( degree ) - 1.0 ) * polynomialParameter * oneDegreePriorPolynomial - std::sqrt( ( static_cast< double >( degree + order ) - 1.0 ) * ( static_cast< double >( degree - order ) - 1.0 ) / ( 2.0 * static_cast< double >( degree ) - 3.0 ) ) * twoDegreesPriorPolynomial ); } -//! Define overloaded 'equals' operator for use with 'Point' structure. -bool operator==( const Point& polynomialArguments1, const Point& polynomialArguments2 ) -{ - bool equal = polynomialArguments1.degree == polynomialArguments2.degree - && polynomialArguments1.order == polynomialArguments2.order - && polynomialArguments1.polynomialParameter - == polynomialArguments2.polynomialParameter; - - return equal; -} - -//! Set hash value. -std::size_t hash_value( Point const& polynomialArguments ) -{ - std::size_t seed = 0; - boost::hash_combine( seed, polynomialArguments.degree ); - boost::hash_combine( seed, polynomialArguments.order ); - boost::hash_combine( seed, polynomialArguments.polynomialParameter ); - - return seed; -} - -//! Get Legendre polynomial from cache when possible, and from direct computation otherwise. -double LegendreCache::getOrElseUpdate( - const int degree, const int order, const double polynomialParameter, - const LegendrePolynomialFunction legendrePolynomialFunction ) -{ - // Initialize structure with polynomial arguments. - Point polynomialArguments( degree, order, polynomialParameter ); - - // Initialize cache iterator. - CacheTable::iterator cachedEntry = backendCache.find( polynomialArguments ); - - // If the requested polynomial was not found in cache, compute polynomial. - if ( cachedEntry == backendCache.end( ) ) - { - double legendrePolynomial = legendrePolynomialFunction( degree, order, - polynomialParameter ); - // If cache is full, remove the oldest element. - if ( history.full( ) ) - { - backendCache.erase( backendCache.find( history[ 0 ] ) ); - history.pop_front( ); - } - - // Insert computed polynomial into cache. - backendCache.insert( std::pair< Point, double >( polynomialArguments, - legendrePolynomial ) ); - history.push_back( polynomialArguments ); - - // Return polynomial value from computation. - return legendrePolynomial; - } - - // Else the requested polynomial was found in cache; return polynomial value from cache entry. - else - { - return cachedEntry->second; - } -} - -//! Initialize LegendreCache objects. -LegendreCache::LegendreCache( ) : history( MAXIMUM_CACHE_ENTRIES ) { } - -//! Write contents of Legendre polynomial structure to string. -std::string writeLegendrePolynomialStructureToString( const Point legendrePolynomialStructure ) -{ - std::stringstream buffer; - - buffer << "(" << legendrePolynomialStructure.degree << ", " - << legendrePolynomialStructure.order << ", " - << legendrePolynomialStructure.polynomialParameter << ")"; - - return buffer.str( ); -} - -//! Dump Legendre polynomial cache data to stream (table and history). -void dumpLegendrePolynomialCacheData( std::ostream& outputStream, - boost::unordered_map< Point, double > cacheTable, - boost::circular_buffer< Point > cacheHistory ) -{ - outputStream << "Table:\n"; - - for ( boost::unordered_map< Point, double >::iterator iteratorCacheTable = cacheTable.begin( ); - iteratorCacheTable != cacheTable.end( ); iteratorCacheTable++ ) - { - outputStream << "\t" << writeLegendrePolynomialStructureToString( - iteratorCacheTable->first ).c_str( ) << " => " - << iteratorCacheTable->second << std::endl; - } - - outputStream << "History:\n"; - - for ( boost::circular_buffer< Point >::iterator iteratorCacheHistory = cacheHistory.begin( ); - iteratorCacheHistory != cacheHistory.end( ); iteratorCacheHistory++ ) - { - outputStream << "\t" - << writeLegendrePolynomialStructureToString( *iteratorCacheHistory ).c_str( ) - << ", "; - } - - outputStream << std::endl; -} - //! Function to calculate the normalization factor for Legendre polynomials to geodesy-normalized. double calculateLegendreGeodesyNormalizationFactor( const int degree, const int order ) { @@ -532,9 +796,9 @@ double calculateLegendreGeodesyNormalizationFactor( const int degree, const int } double factor = std::sqrt( - boost::math::factorial< double >( static_cast< double >( degree + order ) ) - / ( ( 2.0 - deltaFunction ) * ( 2.0 * static_cast< double >( degree ) + 1.0 ) - * boost::math::factorial< double >( static_cast< double >( degree - order ) ) ) ); + boost::math::factorial< double >( static_cast< double >( degree + order ) ) + / ( ( 2.0 - deltaFunction ) * ( 2.0 * static_cast< double >( degree ) + 1.0 ) + * boost::math::factorial< double >( static_cast< double >( degree - order ) ) ) ); return 1.0 / factor; } diff --git a/Tudat/Mathematics/BasicMathematics/legendrePolynomials.h b/Tudat/Mathematics/BasicMathematics/legendrePolynomials.h index 075897bd23..937c67cbfc 100644 --- a/Tudat/Mathematics/BasicMathematics/legendrePolynomials.h +++ b/Tudat/Mathematics/BasicMathematics/legendrePolynomials.h @@ -55,16 +55,242 @@ #include #include +#include + #include #include #include #include +#include + +#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h" namespace tudat { namespace basic_mathematics { +//! Class for creating and accessing a back-end cache of Legendre polynomials. +class LegendreCache +{ + +public: + + //! Define Legendre polynomial function pointer. + typedef boost::function< double ( int, int, LegendreCache& ) > LegendrePolynomialFunction; + + //! Default constructor, initializes cache object with 0 maximum degree and order. + /*! + * Default constructor, initializes cache object with 0 mazimum degree and order. + * \param useGeodesyNormalization Parameter defining whether the cache is used for a normalized or unnormalized + * gravity field. + */ + LegendreCache( const bool useGeodesyNormalization = 1 ); + + //! Constructor + /*! + * Constructor + * \param maximumDegree Maximum degree to which to update cache + * \param maximumOrder Maximum order to which to update cache + * \param useGeodesyNormalization Parameter defining whether the cache is used for a normalized or unnormalized + * gravity field. + */ + LegendreCache( const int maximumDegree, const int maximumOrder, const bool useGeodesyNormalization = 1 ); + + //! Update maximum degree and order of cache + /*! + * Update maximum degree and order of cache + * \param maximumDegree Maximum degree to which to update cache + * \param maximumOrder Maximum order to which to update cache + */ + void resetMaximumDegreeAndOrder( const int maximumDegree, const int maximumOrder ); + + //! Update cache with new polynomial parameter (sine of latitude) + /*! + * Update cache with new polynomial parameter (sine of latitude). + * \param polynomialParameter Parameter used as input argument for Legendre polynomials, in astrodynamics + * applications, this is typically the sine of the body-fixed latitude. + */ + void update( const double polynomialParameter ); + + //! Function to return the current polynomial parameter (typically sine of latitude) + /*! + * Function to return the current polynomial parameter (typically sine of latitude) + * \return Current polynomial parameter + */ + double getCurrentPolynomialParameter( ) + { + return currentPolynomialParameter_; + } + + //! Function to return the complement to the current polynomial parameter (typically cosine of latitude) + /*! + * Function to return the complement to the current polynomial parameter (typically cosine of latitude) + * \return Complement to the current polynomial parameter + */ + double getCurrentPolynomialParameterComplement( ) + { + return currentPolynomialParameterComplement_; + } + + //! Get Legendre polynomial value from the cache. + /*! + * Get Legendre polynomial value from the cache, as computed by last call to update function. + * \param degree Degree of requested Legendre polynomial. + * \param order Order of requested Legendre polynomial. + * \return Legendre polynomial value. + */ + double getLegendrePolynomial( const int degree, const int order ); + + //! Get first derivative of Legendre polynomial value from the cache. + /*! + * Get first derivative of Legendre polynomial value from the cache, as computed by last call to update function. + * \param degree Degree of requested Legendre polynomial. + * \param order Order of requested Legendre polynomial. + * \return First derivative of Legendre polynomial value. + */ + double getLegendrePolynomialDerivative( const int degree, const int order ); + + //! Get second derivative of Legendre polynomial value from the cache. + /*! + * Get second derivative of Legendre polynomial value from the cache, as computed by last call to update function. + * \param degree Degree of requested Legendre polynomial. + * \param order Order of requested Legendre polynomial. + * \return Second derivative of Legendre polynomial value. + */ + double getLegendrePolynomialSecondDerivative( const int degree, const int order ); + + //! Function to get the maximum degree of cache. + /*! + * Function to get the maximum degree of cache + * \return Maximum degree of cache. + */ + int getMaximumDegree( ) + { + return maximumDegree_; + } + + //! Function to get the maximum order of cache. + /*! + * Function to get the maximum order of cache. + * \return Maximum order of cache. + */ + int getMaximumOrder( ) + { + return maximumOrder_; + } + + //! Function to get whether the Legendre polynomials are geodesy-normalized or unnormalized + /*! + * Function to get whether the Legendre polynomials are geodesy-normalized or unnormalized + * \return Boolean denoting to get whether the Legendre polynomials are geodesy-normalized or unnormalized + */ + bool getUseGeodesyNormalization( ) + { + return useGeodesyNormalization_; + } + + //! Function to reset whether the second derivatives are to be computed when calling update function + /*! + * Function to reset whether the second derivatives are to be computed when calling update function + * \param computeSecondDerivatives Boolean denoting whether the second derivatives of the Legendre polynomials are + * to be computed when calling update function. + */ + void setComputeSecondDerivatives( const bool computeSecondDerivatives ) + { + computeSecondDerivatives_ = computeSecondDerivatives; + currentPolynomialParameter_ = TUDAT_NAN; + } + + + +private: + + //! Maximum degree of cache. + int maximumDegree_; + + //! Maximum order of cache. + int maximumOrder_; + + //! Current polynomial parameter (sine of latitude). + double currentPolynomialParameter_; + + //! Current 'complement' to polynomial parameter (cosine of latitude). + double currentPolynomialParameterComplement_; + + //! List of current values of Legendre polynomials at degree and order (n,m) + /*! + * List of current values of Legendre polynomials at degree and order (n,m). The corresponding polynomial is at entry + * n * ( maximumOrder_ + 1 ) + m. + */ + std::vector< double > legendreValues_; + + //! List of current values of first derivatives of Legendre polynomials at degree and order (n,m) + /*! + * List of current values of first derivatives of Legendre polynomials at degree and order (n,m). + * The corresponding polynomial is at entry n * ( maximumOrder_ + 1 ) + m. + */ + std::vector< double > legendreDerivatives_; + + //! List of current values of second derivatives of Legendre polynomials at degree and order (n,m) + /*! + * List of current values of second derivatives of Legendre polynomials at degree and order (n,m). + * The corresponding polynomial is at entry n * ( maximumOrder_ + 1 ) + m. + */ + std::vector< double > legendreSecondDerivatives_; + + //! Function from which to compute the Legendre polynomials. + LegendrePolynomialFunction legendrePolynomialFunction_; + + //! Boolean denoting whether the Legendre polynomials are geodesy-normalized or unnormalized + bool useGeodesyNormalization_; + + //! Vector of ratio of reference radius over current radius to power i, with i the entry in the vector. + std::vector< double > referenceRadiusRatioPowers_; + + //! Prec-computed normalization factors that are to be used for computation fo Legendre polynomial derivative + std::vector< double > derivativeNormalizations_; + + //! Boolean denoting whether the second derivatives of the Legendre polynomials are to be computed when calling + //! update function. + bool computeSecondDerivatives_; + + +}; + + + +//! Compute unnormalized associated Legendre polynomial. +/*! + * This function returns an unnormalized associated Legendre polynomial \f$ P _{ n, m }( u ) \f$ + * with degree \f$ n \f$, order \f$ m \f$ and polynomial parameter \f$ u \f$. + * \f$ P _{ n, m }( u ) \f$ obeys the definition given by Vallado [2001]: + * \f[ + * P _{ n, m } ( u ) = ( 1 - u ^ 2 ) ^ { m / 2 } \frac{ d ^ m }{ du ^ m } \left[ P_n ( u ) + * \right] \textrm{ for }n \geq 0, 0 \leq m \leq n + * \f] + * in which \f$ P_{ n, m }( u ) \f$ is the ordinary Legendre polynomial with degree \f$ n \f$, + * order \f$ m \f$ and polynomial parameter \f$ u \f$. + * + * For \f$ n \geq 0, m \geq n \f$ the \f$ P _{ n, m }( u ) \f$ has been defined here as + * follows: + * \f[ + * P _{ n, m } ( u ) = 0 \textrm{ for }n \geq 0, m \geq n + * \f] + * + * This function has been optimized for repeated calls with varying 'degree' and 'order' arguments + * (but with identical 'polynomialParameter' argument). To this end the function maintains a + * back-end cache with intermediate results which is automatically carried over between calls. + * \param degree Degree of requested Legendre polynomial. + * \param order Order of requested Legendre polynomial. + * \param legendreCache Legendre cache from which to retrieve Legendre polynomial. + * \return Unnormalized Legendre polynomial. +*/ +double computeLegendrePolynomial( const int degree, + const int order, + LegendreCache& legendreCache ); + + //! Compute unnormalized associated Legendre polynomial. /*! * This function returns an unnormalized associated Legendre polynomial \f$ P _{ n, m }( u ) \f$ @@ -88,12 +314,13 @@ namespace basic_mathematics * back-end cache with intermediate results which is automatically carried over between calls. * \param degree Degree of requested Legendre polynomial. * \param order Order of requested Legendre polynomial. - * \param polynomialParameter Free variable of requested Legendre polynomial. + * \param legendreParameter Free variable of requested Legendre polynomial. * \return Unnormalized Legendre polynomial. */ double computeLegendrePolynomial( const int degree, const int order, - const double polynomialParameter ); + const double legendreParameter ); + //! Compute geodesy-normalized associated Legendre polynomial. /*! @@ -131,12 +358,55 @@ double computeLegendrePolynomial( const int degree, * back-end cache with intermediate results which is automatically carried over between calls. * \param degree Degree of requested Legendre polynomial. * \param order Order of requested Legendre polynomial. - * \param polynomialParameter Free variable of requested Legendre polynomial. + * \param geodesyLegendreCache Legendre cache from which to retrieve Legendre polynomial. + * \return Geodesy-normalized Legendre polynomial. +*/ +double computeGeodesyLegendrePolynomial( const int degree, + const int order, + LegendreCache& geodesyLegendreCache ); + +//! Compute geodesy-normalized associated Legendre polynomial. +/*! + * This function returns a normalized associated Legendre polynomial + * \f$ \bar{ P }_{ n, m }( u ) \f$ with degree \f$ n \f$, order \f$ m \f$ and polynomial + * parameter \f$ u \f$. The normalization obeys the definition: + * \f[ + * \bar{ P }_{ n, m } ( u ) = \Pi_{ n, m } P_{ n, m } ( u ) + * \f] + * in which \f$ \Pi_{ n, m } \f$ is the normalization factor which is commonly used in geodesy and + * is given by Heiskanen & Moritz [1967] as: + * \f[ + * \Pi_{ n, m } = \sqrt{ \frac{ ( 2 - \delta_{ 0, m } ) ( 2 n + 1 ) ( n - m )! } + * { ( n + m )! } } + * \f] + * in which \f$ n \f$ is the degree, \f$ m \f$ is the order and \f$ \delta_{ 0, m } \f$ is the + * Kronecker delta. + + * \f$ P _{ n, m }( u ) \f$ obeys the definition given by Vallado [2001]: + * \f[ + * P _{ n, m } ( u ) = ( 1 - u ^ 2 ) ^ { m / 2 } \frac{ d ^ m }{ du ^ m } \left[ P_n ( u ) + * \right] + * \f] + * in which \f$ P_{ n, m }( u ) \f$ is the ordinary Legendre polynomial with degree \f$ n \f$, + * order \f$ m \f$ and polynomial parameter \f$ u \f$. + * + * For \f$ n \geq 0, m \geq n \f$ the \f$ P _{ n, m }( u ) \f$ has been defined here as + * follows: + * \f[ + * P _{ n, m } ( u ) = 0 \textrm{ for }n \geq 0, m \geq n + * \f] + * + * This function has been optimized for repeated calls with varying 'degree' and 'order' arguments + * (but with identical 'polynomialParameter' argument). To this end the function maintains a + * back-end cache with intermediate results which is automatically carried over between calls. + * \param degree Degree of requested Legendre polynomial. + * \param order Order of requested Legendre polynomial. + * \param legendreParameter Free variable of requested Legendre polynomial. * \return Geodesy-normalized Legendre polynomial. */ double computeGeodesyLegendrePolynomial( const int degree, const int order, - const double polynomialParameter ); + const double legendreParameter ); //! Compute derivative of unnormalized Legendre polynomial. /*! @@ -162,6 +432,28 @@ double computeLegendrePolynomialDerivative( const int order, const double currentLegendrePolynomial, const double incrementedLegendrePolynomial ); +//! Compute derivative of geodesy-normalized associated Legendre polynomial. +/*! + * Compute derivative of geodesy-normalized associated Legendre polynomial. + * \param degree Degree of requested Legendre polynomial derivative. + * \param order Order of requested Legendre polynomial derivative. + * \param polynomialParameter Free variable of requested Legendre polynomial derivative. + * \param currentLegendrePolynomial Geodesy-normalized Legendre polynomial with the same degree, order + * and polynomial parameter as the requested Legendre polynomial derivative. + * \param incrementedLegendrePolynomial Geodesy-normalized Legendre polynomial with the same degree and + * polynomial parameter as the requested Legendre polynomial derivative, but with an order + * of one more. + * \param normalizationCorrection Pre-computed scaling term used for part of computations. + * \return Geodesy-normalized Legendre polynomial derivative with respect to the polynomial parameter. +*/ +double computeGeodesyLegendrePolynomialDerivative( const int degree, + const int order, + const double polynomialParameter, + const double currentLegendrePolynomial, + const double incrementedLegendrePolynomial, + const double normalizationCorrection ); + + //! Compute derivative of geodesy-normalized Legendre polynomial. /*! * The derivative is computed as: @@ -189,6 +481,34 @@ double computeGeodesyLegendrePolynomialDerivative( const int degree, const double currentLegendrePolynomial, const double incrementedLegendrePolynomial ); +//! Compute second derivative of geodesy-normalized associated Legendre polynomial. +/*! + * Compute second derivative of geodesy-normalized associated Legendre polynomial. + * \param degree Degree of requested Legendre polynomial derivative. + * \param order Order of requested Legendre polynomial derivative. + * \param polynomialParameter Free variable of requested Legendre polynomial derivative. + * \param currentLegendrePolynomial Geodesy-normalized Legendre polynomial with the same degree, order + * and polynomial parameter as the requested Legendre polynomial derivative. + * \param incrementedLegendrePolynomial Geodesy-normalized Legendre polynomial with the same degree and + * polynomial parameter as the requested Legendre polynomial derivative, but with an order + * of one more. + * \param currentLegendrePolynomialDerivative Geodesy-normalized derivative of Legendre polynomial with the same degree, + * order and polynomial parameter as the requested Legendre polynomial derivative. + * \param incrementedLegendrePolynomialDerivative Geodesy-normalized derivative of Legendre polynomial with the same degree + * and polynomial parameter as the requested Legendre polynomial derivative, but with an order + * of one more. + * \param normalizationCorrection Pre-computed scaling term used for part of computations. + * \return Geodesy-normalized Legendre polynomial derivative with respect to the polynomial parameter. +*/ +double computeGeodesyLegendrePolynomialSecondDerivative( const int degree, + const int order, + const double polynomialParameter, + const double currentLegendrePolynomial, + const double incrementedLegendrePolynomial, + const double currentLegendrePolynomialDerivative, + const double incrementedLegendrePolynomialDerivative, + const double normalizationCorrection ); + //! Compute low degree/order unnormalized Legendre polynomial explicitly. /*! * The associated Legendre polynomial \f$ P_{ n, m }(u) \f$ with degree \f$ n \f$, order @@ -320,111 +640,11 @@ double computeGeodesyLegendrePolynomialVertical( const int degree, const double oneDegreePriorPolynomial, const double twoDegreesPriorPolynomial ); -//! Declare structure for arguments of Legendre polynomial (for use in back-end cache). -struct Point -{ -public: - - //! Self-referential structure. - Point( const int aDegree, const int anOrder, const double aPolynomialParameter ) - : degree( aDegree ), - order( anOrder ), - polynomialParameter( aPolynomialParameter ) - { } - - //! Degree of Legendre polynomial. - int degree; - - //! Order of Legendre polynomial. - int order; - - //! Polynomial parameter of Legendre polynomial. - double polynomialParameter; - -protected: - -private: -}; - -//! Typedef for shared-pointer to Point object. -typedef boost::shared_ptr< Point > PointPointer; - -//! Define overloaded 'equals' operator for use with 'Point' structure. -bool operator==( const Point& polynomialArguments1, const Point& polynomialArguments2 ); - -//! Set hash value. -std::size_t hash_value( Point const& polynomialArguments ); - -//! Class for creating and accessing a back-end cache of Legendre polynomials. -class LegendreCache -{ -private: - - //! Define Legendre polynomial function pointer. - typedef boost::function< double ( int, int, double ) > LegendrePolynomialFunction; - - //! Define map variables type. - typedef boost::unordered_map< Point, double > CacheTable; - - //! Define buffer variables type. - typedef boost::circular_buffer< Point > CacheHistory; - -public: - - //! Initialize LegendreCache instance. - LegendreCache( ); - - //! Get Legendre polynomial value from either cache or from computation. - /*! - * \param degree Degree of requested Legendre polynomial. - * \param order Order of requested Legendre polynomial. - * \param polynomialParameter Free variable of requested Legendre polynomial. - * \param legendrePolynomialFunction Function which takes degree, order and - * polynomialParameter as arguments. The function must return the corresponding - * Legendre polynomial value. - * \return Legendre polynomial value. - */ - double getOrElseUpdate( const int degree, const int order, const double polynomialParameter, - const LegendrePolynomialFunction legendrePolynomialFunction ); - -private: - - //! Hashmap which links a specific degree, order and polynomial parameter to its - //! corresponding Legendre polynomial value. - CacheTable backendCache; - - //! History buffer. - CacheHistory history; -}; - -//! Typedef shared-pointer to LegendreCache object. -typedef boost::shared_ptr< LegendreCache > LegendreCachePointer; - -//! Global instances of LegendreCache class for unnormalized polynomials. -static LegendreCache legendreCache; -//! Global instances of LegendreCache class for geodesy-normalized polynomials. -static LegendreCache geodesyLegendreCache; - -//! Write contents of Legendre polynomial structure to string. -/*! - * Writes contents of Legendre polynomial structure, containing degree, order, and value of - * polynomial parameter, to string. - * \param legendrePolynomialStructure Structure containing legendre polynomial data. - * \return String containing degree, order, and value of polynomial paramter stored in structure. - */ -std::string writeLegendrePolynomialStructureToString( const Point legendrePolynomialStructure ); - -//! Dump Legendre polynomial cache data to stream (table and history). -/*! - * Dumps cached table and history data for Legendre polynomials to given output stream. - * \param outputStream Output stream. - * \param cacheTable Cache table containing current values of Legendre polynomials. - * \param cacheHistory Cached history of Legendre polynomials. - */ -void dumpLegendrePolynomialCacheData( std::ostream& outputStream, - boost::unordered_map< Point, double > cacheTable, - boost::circular_buffer< Point > cacheHistory ); +//! Predefine boost function for geodesy-normalized Legendre polynomial. +static const LegendreCache::LegendrePolynomialFunction geodesyNormalizedLegendrePolynomialFunction = + boost::bind( static_cast< double(&)( const int, const int, LegendreCache& )>( + &computeGeodesyLegendrePolynomial ), _1, _2, _3 ); //! Function to calculate the normalization factor for Legendre polynomials to geodesy-normalized. /*! @@ -438,6 +658,10 @@ void dumpLegendrePolynomialCacheData( std::ostream& outputStream, */ double calculateLegendreGeodesyNormalizationFactor( const int degree, const int order ); +//! Predefine boost function for unnormalized Legendre polynomial. +const LegendreCache::LegendrePolynomialFunction regularLegendrePolynomialFunction = + boost::bind( static_cast< double(&)( const int, const int, LegendreCache& )>( + &computeLegendrePolynomial ), _1, _2, _3 ); } // namespace basic_mathematics } // namespace tudat diff --git a/Tudat/Mathematics/BasicMathematics/linearAlgebra.cpp b/Tudat/Mathematics/BasicMathematics/linearAlgebra.cpp index e15eaed752..82115ae9d3 100644 --- a/Tudat/Mathematics/BasicMathematics/linearAlgebra.cpp +++ b/Tudat/Mathematics/BasicMathematics/linearAlgebra.cpp @@ -103,6 +103,34 @@ double computeAngleBetweenVectors( const Eigen::VectorXd& vector0, const Eigen:: return std::acos( dotProductOfNormalizedVectors ); } +//! Computes the difference between two 3d vectors. +Eigen::Vector3d computeVectorDifference( const Eigen::Vector3d& vector0, + const Eigen::Vector3d& vector1 ) +{ + return ( vector0 - vector1 ); +} + +//! Computes norm of the the difference between two 3d vectors. +double computeNormOfVectorDifference( const Eigen::Vector3d& vector0, + const Eigen::Vector3d& vector1 ) +{ + return ( vector0 - vector1 ).norm( ); +} + +//! Computes the norm of a 3d vector +double getVectorNorm( const Eigen::Vector3d& vector ) +{ + return vector.norm( ); +} + +//! Computes the norm of a 3d vector from a vector-returning function. +double getVectorNormFromFunction( const boost::function< Eigen::Vector3d( ) > vectorFunction ) +{ + return getVectorNorm( vectorFunction( ) ); +} + + + } // namespace linear_algebra } // namespace tudat diff --git a/Tudat/Mathematics/BasicMathematics/linearAlgebra.h b/Tudat/Mathematics/BasicMathematics/linearAlgebra.h index c7b5e762cd..eb09ed69d2 100644 --- a/Tudat/Mathematics/BasicMathematics/linearAlgebra.h +++ b/Tudat/Mathematics/BasicMathematics/linearAlgebra.h @@ -35,6 +35,8 @@ #ifndef TUDAT_LINEAR_ALGEBRA_H #define TUDAT_LINEAR_ALGEBRA_H +#include + #include namespace tudat @@ -72,6 +74,43 @@ double computeCosineOfAngleBetweenVectors( const Eigen::VectorXd& vector0, double computeAngleBetweenVectors( const Eigen::VectorXd& vector0, const Eigen::VectorXd& vector1 ); +//! Computes the difference between two 3d vectors. +/*! + * Computes the difference between two 3d vectors (first input minus second input, i.e vector from second input to + * first input). + * \param vector0 First vector. + * \param vector1 Second vector. + * \return Difference between vectors + */ +Eigen::Vector3d computeVectorDifference( const Eigen::Vector3d& vector0, + const Eigen::Vector3d& vector1 ); + +//! Computes norm of the the difference between two 3d vectors. +/*! + * Computes the norm of the difference between two 3d vectors (i.e. distance between vectors) + * \param vector0 First vector. + * \param vector1 Second vector. + * \return Norm of difference between vectors + */ +double computeNormOfVectorDifference( const Eigen::Vector3d& vector0, + const Eigen::Vector3d& vector1 ); + +//! Computes the norm of a 3d vector +/*! + * Computes the norm of a 3d vector + * \param vector Vector for which the norm is to be computed + * \return Vector norm + */ +double getVectorNorm( const Eigen::Vector3d& vector ); + +//! Computes the norm of a 3d vector from a vector-returning function. +/*! + * Computes the norm of a 3d vector from a vector-returning function. + * \param vectorFunction Function returning the vector for which the norm is to be computed + * \return Vector norm + */ +double getVectorNormFromFunction( const boost::function< Eigen::Vector3d( ) > vectorFunction ); + //! Flip matrix rows. /*! * Flips all rows of an Eigen-matrix, i.e., order of rows is reversed. @@ -94,6 +133,8 @@ static inline void flipMatrixRows( Eigen::MatrixXd& matrixToFlip ) } } +double computeNormOfVectorDifference( const Eigen::Vector3d& vector0, + const Eigen::Vector3d& vector1 ); } // namespace linear_algebra } // namespace tudat diff --git a/Tudat/Mathematics/BasicMathematics/linearAlgebraTypes.h b/Tudat/Mathematics/BasicMathematics/linearAlgebraTypes.h index 4c38f1d1b9..74f32ebef2 100644 --- a/Tudat/Mathematics/BasicMathematics/linearAlgebraTypes.h +++ b/Tudat/Mathematics/BasicMathematics/linearAlgebraTypes.h @@ -29,7 +29,6 @@ * References * * Notes - * Code in this file, if used often, should be migrated to Tudat Core. * */ diff --git a/Tudat/Mathematics/BasicMathematics/sphericalHarmonics.cpp b/Tudat/Mathematics/BasicMathematics/sphericalHarmonics.cpp index f9e02e6628..559bb6a4fd 100644 --- a/Tudat/Mathematics/BasicMathematics/sphericalHarmonics.cpp +++ b/Tudat/Mathematics/BasicMathematics/sphericalHarmonics.cpp @@ -37,13 +37,64 @@ #include #include "Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h" +#include "Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h" namespace tudat { namespace basic_mathematics { -// Compute the gradient of a single term of a spherical harmonics potential field. +//! Update maximum degree and order of cache +void SphericalHarmonicsCache::resetMaximumDegreeAndOrder( const int maximumDegree, const int maximumOrder ) +{ + maximumDegree_ = maximumDegree; + maximumOrder_ = maximumOrder; + + if( maximumOrder_ > maximumDegree_ ) + { + maximumOrder_ = maximumDegree_; + } + legendreCache_->resetMaximumDegreeAndOrder( maximumDegree_, maximumOrder_ ); + + sinesOfLongitude_.resize( maximumOrder_ + 1 ); + cosinesOfLongitude_.resize( maximumOrder_ + 1 ); + referenceRadiusRatioPowers_.resize( maximumDegree_ + 2 ); +} + + +//! Compute the gradient of a single term of a spherical harmonics potential field. +Eigen::Vector3d computePotentialGradient( + const double distance, + const double radiusPowerTerm, + const double cosineOfOrderLongitude, + const double sineOfOrderLongitude, + const double cosineOfLatitude, + const double preMultiplier, + const int degree, + const int order, + const double cosineHarmonicCoefficient, + const double sineHarmonicCoefficient, + const double legendrePolynomial, + const double legendrePolynomialDerivative ) +{ + // Return result. + return ( Eigen::Vector3d( ) << + - preMultiplier / distance + * radiusPowerTerm + * ( static_cast< double >( degree ) + 1.0 ) * legendrePolynomial + * ( cosineHarmonicCoefficient * cosineOfOrderLongitude + + sineHarmonicCoefficient * sineOfOrderLongitude ), + preMultiplier * radiusPowerTerm + * legendrePolynomialDerivative * cosineOfLatitude * ( + cosineHarmonicCoefficient * cosineOfOrderLongitude + + sineHarmonicCoefficient * sineOfOrderLongitude ), + preMultiplier * radiusPowerTerm + * static_cast< double >( order ) * legendrePolynomial + * ( sineHarmonicCoefficient * cosineOfOrderLongitude + - cosineHarmonicCoefficient * sineOfOrderLongitude ) ).finished( ); +} + +//! Compute the gradient of a single term of a spherical harmonics potential field. Eigen::Vector3d computePotentialGradient( const Eigen::Vector3d& sphericalPosition, const double referenceRadius, @@ -55,41 +106,35 @@ Eigen::Vector3d computePotentialGradient( const double legendrePolynomial, const double legendrePolynomialDerivative ) { - // Initialize return variable. - Eigen::Vector3d potentialGradient; - - // Compute radius power term. - const double radiusPowerTerm = std::pow( referenceRadius / sphericalPosition( radiusIndex ), - static_cast< double >( degree ) + 1.0 ); - - // Calculate derivative with respect to radius. - potentialGradient( radiusIndex ) = - preMultiplier / sphericalPosition( radiusIndex ) - * radiusPowerTerm - * ( static_cast< double >( degree ) + 1.0 ) * legendrePolynomial - * ( cosineHarmonicCoefficient - * std::cos( static_cast< double >( order ) * sphericalPosition( longitudeIndex ) ) - + sineHarmonicCoefficient - * std::sin( static_cast< double >( order ) - * sphericalPosition( longitudeIndex ) ) ); - - // Calculate derivative with respect to latitude. - potentialGradient( latitudeIndex ) = preMultiplier * radiusPowerTerm - * legendrePolynomialDerivative - * std::cos( sphericalPosition( latitudeIndex ) ) * ( cosineHarmonicCoefficient - * std::cos( static_cast< double >( order ) * sphericalPosition ( longitudeIndex ) ) - + sineHarmonicCoefficient * std::sin( static_cast< double >( order ) - * sphericalPosition( longitudeIndex ) ) ); - - // Calculate derivative with respect to longitude. - potentialGradient( longitudeIndex ) = preMultiplier * radiusPowerTerm - * static_cast< double >( order ) * legendrePolynomial - * ( sineHarmonicCoefficient * std::cos( static_cast< double >( order ) - * sphericalPosition( longitudeIndex ) ) - - cosineHarmonicCoefficient * std::sin( static_cast< double >( order ) - * sphericalPosition( longitudeIndex ) ) ); + return computePotentialGradient( + sphericalPosition( radiusIndex ), + basic_mathematics::raiseToIntegerPower + ( referenceRadius / sphericalPosition( radiusIndex ), static_cast< double >( degree ) + 1.0 ), + std::cos( static_cast< double >( order ) * sphericalPosition( longitudeIndex ) ), + std::sin( static_cast< double >( order ) * sphericalPosition( longitudeIndex ) ), + std::cos( sphericalPosition( latitudeIndex ) ), preMultiplier, degree, order, + cosineHarmonicCoefficient, sineHarmonicCoefficient, legendrePolynomial,legendrePolynomialDerivative ); +} - // Return result. - return potentialGradient; +//! Compute the gradient of a single term of a spherical harmonics potential field. +Eigen::Vector3d computePotentialGradient( const Eigen::Vector3d& sphericalPosition, + const double preMultiplier, + const int degree, + const int order, + const double cosineHarmonicCoefficient, + const double sineHarmonicCoefficient, + const double legendrePolynomial, + const double legendrePolynomialDerivative, + const boost::shared_ptr< SphericalHarmonicsCache > sphericalHarmonicsCache ) +{ + return computePotentialGradient( + sphericalPosition( radiusIndex ), + sphericalHarmonicsCache->getReferenceRadiusRatioPowers( degree + 1 ), + sphericalHarmonicsCache->getCosineOfMultipleLongitude( order ), + sphericalHarmonicsCache->getSineOfMultipleLongitude( order ), + sphericalHarmonicsCache->getLegendreCache( )->getCurrentPolynomialParameterComplement( ), + preMultiplier, degree, order, + cosineHarmonicCoefficient, sineHarmonicCoefficient, legendrePolynomial,legendrePolynomialDerivative ); } } // namespace basic_mathematics diff --git a/Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h b/Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h index 67ea431806..5c7b49de1d 100644 --- a/Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h +++ b/Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h @@ -37,14 +37,324 @@ #include +#include + +#include "Tudat/Mathematics/BasicMathematics/legendrePolynomials.h" + namespace tudat { namespace basic_mathematics { +//! Cache object in which variables that are required for the computation of spherical harmonic potential are stored. +/*! + * Cache object in which variables that are required for the computation of spherical harmonic potential are stored. + * The variables are the Legendre polynomials at the required degree and order, the cosine of teh latitude, the + * sine and cosine of the order times the longitude, and the ratio of the distance and the reference radius to the + * power degree + 1. + */ +class SphericalHarmonicsCache +{ +public: + + + //! Default constructor, initializes cache object with 0 maximum degree and order. + /*! + * Default constructor, initializes cache object with 0 mazimum degree and order. + * \param useGeodesyNormalization Parameter defining whether the cache is used for a normalized or unnormalized + * gravity field. + */ + SphericalHarmonicsCache( const bool useGeodesyNormalization = 1 ) + { + legendreCache_ = boost::make_shared< LegendreCache >( useGeodesyNormalization ); + + currentLongitude_ = TUDAT_NAN; + referenceRadiusRatio_ = TUDAT_NAN; + + resetMaximumDegreeAndOrder( 0, 0 ); + } + + //! Constructor + /*! + * Constructor + * \param maximumDegree Maximum degree to which to update cache + * \param maximumOrder Maximum order to which to update cache + * \param useGeodesyNormalization Parameter defining whether the cache is used for a normalized or unnormalized + * gravity field. + */ + SphericalHarmonicsCache( const int maximumDegree, const int maximumOrder, const bool useGeodesyNormalization = 1 ) + { + legendreCache_ = boost::make_shared< LegendreCache >( maximumDegree, maximumOrder, useGeodesyNormalization ); + + currentLongitude_ = TUDAT_NAN; + referenceRadiusRatio_ = TUDAT_NAN; + + resetMaximumDegreeAndOrder( maximumDegree, maximumOrder ); + } + + //! Update maximum degree and order of cache + /*! + * Update maximum degree and order of cache + * \param maximumDegree Maximum degree to which to update cache + * \param maximumOrder Maximum order to which to update cache + */ + void resetMaximumDegreeAndOrder( const int maximumDegree, const int maximumOrder ); + + + //! Update cached variables to current state. + /*! + * Update cached variables to current state. + * \param radius Distance from origin + * \param polynomialParameter Input parameter to Legendre polynomials (sine of latitude) + * \param longitude Current latitude + * \param referenceRadius Reference (typically equatorial) radius of gravity field. + */ + void update( const double radius, const double polynomialParameter, + const double longitude, const double referenceRadius ) + { + legendreCache_->update( polynomialParameter ); + updateSines( longitude ); + updateRadiusPowers( referenceRadius / radius ); + } + + //! Function to retrieve the current sine of m times the longitude. + /*! + * Function to retrieve the current sine of order times the longitude. + * \param order Order as input to sine( order * longitude ) + * \return Sine( order * longitude ) + */ + double getSineOfMultipleLongitude( const int order ) + { + return sinesOfLongitude_[ order ]; + } + + //! Function to retrieve the current cosine of m times the longitude. + /*! + * Function to retrieve the current cosine of order times the longitude. + * \param order Order as input to cosine( order * longitude ) + * \return Cosine( order * longitude ) + */ + double getCosineOfMultipleLongitude( const int order ) + { + return cosinesOfLongitude_[ order ]; + } + + //! Function to get an integer power of the distance divided by the reference radius. + /*! + * Function to get an integer power of the distance divided by the reference radius. + * \param degreePlusOne Power to which the ratio of distance and reference radius is to be computed (typically + * degree + 1). + * \return Ratio of distance and reference radius to power of input argument. + */ + double getReferenceRadiusRatioPowers( const int degreePlusOne ) + { + return referenceRadiusRatioPowers_[ degreePlusOne ]; + } + + //! Function to get the maximum degree of cache. + /*! + * Function to get the maximum degree of cache + * \return Maximum degree of cache. + */ + int getMaximumDegree( ) + { + return maximumDegree_; + } + + //! Function to get the maximum order of cache. + /*! + * Function to get the maximum order of cache. + * \return Maximum order of cache. + */ + int getMaximumOrder( ) + { + return maximumOrder_; + } + + //! Function to get current longitude + /*! + * Function to get current longitude + * \return Current longitude + */ + double getCurrentLongitude( ) + { + return currentLongitude_; + } + + //! Function to get object for caching and computing Legendre polynomials. + /*! + * Function to get object for caching and computing Legendre polynomials. + * \return Object for caching and computing Legendre polynomials. + */ + boost::shared_ptr< LegendreCache > getLegendreCache( ) + { + return legendreCache_; + } + +private: + + //! Update cached values of sines and cosines of longitude/ + /*! + * Update cached values of sines and cosines of longitude/ + * \param longitude Current longitude. + */ + void updateSines( const double longitude ) + { + //! Check if update is needed. + if( !( currentLongitude_ == longitude ) ) + { + currentLongitude_ = longitude; + for( unsigned int i = 0; i < sinesOfLongitude_.size( ); i++ ) + { + sinesOfLongitude_[ i ] = std::sin( static_cast< double >( i ) * longitude ); + cosinesOfLongitude_[ i ] = std::cos( static_cast< double >( i ) * longitude ); + } + } + } + + //! Update cached values of powers of distance over reference radius. + /*! + * Update cached values of powers of distance over reference radius. + * \param referenceRadiusRatio Distance divided by reference radius. + */ + void updateRadiusPowers( const double referenceRadiusRatio ) + { + //! Check if update is needed. + if( !( referenceRadiusRatio_ == referenceRadiusRatio ) ) + { + referenceRadiusRatio_ = referenceRadiusRatio; + double currentRatioPower = 1.0; + for( int i = 0; i <= maximumDegree_ + 1; i++ ) + { + referenceRadiusRatioPowers_[ i ] = currentRatioPower; + currentRatioPower *= referenceRadiusRatio_; + } + } + } + + //! Maximum degree of cache. + int maximumDegree_; + + //! Maximum order of cache. + int maximumOrder_; + + //! Current longitude. + double currentLongitude_; + + //! Current ratio of distance to reference radius + double referenceRadiusRatio_; + + //! List of sines of order times longitude. + /*! + * List of sines of order times longitude. Entry i denotes sin(i times longitude). + */ + std::vector< double > sinesOfLongitude_; + + //! List of cosines of order times longitude. + /*! + * List of cosines of order times longitude. Entry i denotes cos(i times longitude). + */ + std::vector< double > cosinesOfLongitude_; + + //! List of powers of distance divided by reference radius. + /*! + * List of powers of distance divided by reference radius. Entry i denoted (distance/reference radius) to the power i. + */ + std::vector< double > referenceRadiusRatioPowers_; + + //! Object for caching and computing Legendre polynomials. + boost::shared_ptr< LegendreCache > legendreCache_; + + + +}; + //! Spherical coordinate indices. enum SphericalCoordinatesIndices{ radiusIndex, latitudeIndex, longitudeIndex }; +//! Compute the gradient of a single term of a spherical harmonics potential field. +/*! + * This function returns a vector with the derivatives of a generic potential field (defined by + * spherical harmonics) from pre-computed quatities. + * \param distance Distance to center of body with gravity field at which the potential gradient is to be calculated + * \param radiusPowerTerm Distance divided by the reference radius of the gravity field, to the power (degree + 1) + * \param cosineOfOrderLongitude Cosine of order times the longitude at which the potential is to be calculated + * \param sineOfOrderLongitude Sine of order times the longitude at which the potential is to be calculated + * \param cosineOfLatitude Cosine of the latitude at which the potential is to be calculated + * \param preMultiplier Generic multiplication factor. + * \param degree Degree of the harmonic for which the gradient is to be computed. + * \param order Order of the harmonic for which the gradient is to be computed. + * \param cosineHarmonicCoefficient Coefficient which characterizes relative strengh of a harmonic + * term. + * \param sineHarmonicCoefficient Coefficient which characterizes relative strengh of a harmonic + * term. + * \param legendrePolynomial Value of associated Legendre polynomial with the same degree and order + * as the to be computed harmonic, and with the sine of the latitude coordinate as + * polynomial parameter. Make sure that the Legendre polynomial has the same + * normalization as the harmonic coefficients. + * \param legendrePolynomialDerivative Value of the derivative of parameter 'legendrePolynomial' + * with respect to the sine of the latitude angle. + * \return Vector with derivatives of potential field. + * The order is important! + * gradient( 0 ) = derivative with respect to radial distance, + * gradient( 1 ) = derivative with respect to latitude angle, + * gradient( 2 ) = derivative with respect to longitude angle. + */ +Eigen::Vector3d computePotentialGradient( + const double distance, + const double radiusPowerTerm, + const double cosineOfOrderLongitude, + const double sineOfOrderLongitude, + const double cosineOfLatitude, + const double preMultiplier, + const int degree, + const int order, + const double cosineHarmonicCoefficient, + const double sineHarmonicCoefficient, + const double legendrePolynomial, + const double legendrePolynomialDerivative ); + + + +//! Compute the gradient of a single term of a spherical harmonics potential field. +/*! + * This function returns a vector with the derivatives of a generic potential field (defined by + * spherical harmonics). * + * \param sphericalPosition Vector with spherical coordinates. + * The order is important! + * sphericalPosition( 0 ) = radial coordinate, + * sphericalPosition( 1 ) = latitude coordinate, + * sphericalPosition( 2 ) = longitude coordinate. + * \param referenceRadius Radius of harmonics reference sphere. + * \param preMultiplier Generic multiplication factor. + * \param degree Degree of the harmonic for which the gradient is to be computed. + * \param order Order of the harmonic for which the gradient is to be computed. + * \param cosineHarmonicCoefficient Coefficient which characterizes relative strengh of a harmonic + * term. + * \param sineHarmonicCoefficient Coefficient which characterizes relative strengh of a harmonic + * term. + * \param legendrePolynomial Value of associated Legendre polynomial with the same degree and order + * as the to be computed harmonic, and with the sine of the latitude coordinate as + * polynomial parameter. Make sure that the Legendre polynomial has the same + * normalization as the harmonic coefficients. + * \param legendrePolynomialDerivative Value of the derivative of parameter 'legendrePolynomial' + * with respect to the sine of the latitude angle. + * \return Vector with derivatives of potential field. + * The order is important! + * gradient( 0 ) = derivative with respect to radial distance, + * gradient( 1 ) = derivative with respect to latitude angle, + * gradient( 2 ) = derivative with respect to longitude angle. + */ +Eigen::Vector3d computePotentialGradient( const Eigen::Vector3d& sphericalPosition, + const double referenceRadius, + const double preMultiplier, + const int degree, + const int order, + const double cosineHarmonicCoefficient, + const double sineHarmonicCoefficient, + const double legendrePolynomial, + const double legendrePolynomialDerivative ); + //! Compute the gradient of a single term of a spherical harmonics potential field. /*! * This function returns a vector with the derivatives of a generic potential field (defined by @@ -80,7 +390,6 @@ enum SphericalCoordinatesIndices{ radiusIndex, latitudeIndex, longitudeIndex }; * sphericalPosition( 0 ) = radial coordinate, * sphericalPosition( 1 ) = latitude coordinate, * sphericalPosition( 2 ) = longitude coordinate. - * \param referenceRadius Radius of harmonics reference sphere. * \param preMultiplier Generic multiplication factor. * \param degree Degree of the harmonic for which the gradient is to be computed. * \param order Order of the harmonic for which the gradient is to be computed. @@ -94,6 +403,8 @@ enum SphericalCoordinatesIndices{ radiusIndex, latitudeIndex, longitudeIndex }; * normalization as the harmonic coefficients. * \param legendrePolynomialDerivative Value of the derivative of parameter 'legendrePolynomial' * with respect to the sine of the latitude angle. + * \param sphericalHarmonicsCache Cache object containing current values of trigonometric funtions of latitude anf longitude, + * as well as legendre polynomials at current state. * \return Vector with derivatives of potential field. * The order is important! * gradient( 0 ) = derivative with respect to radial distance, @@ -101,14 +412,14 @@ enum SphericalCoordinatesIndices{ radiusIndex, latitudeIndex, longitudeIndex }; * gradient( 2 ) = derivative with respect to longitude angle. */ Eigen::Vector3d computePotentialGradient( const Eigen::Vector3d& sphericalPosition, - const double referenceRadius, const double preMultiplier, const int degree, const int order, const double cosineHarmonicCoefficient, const double sineHarmonicCoefficient, const double legendrePolynomial, - const double legendrePolynomialDerivative ); + const double legendrePolynomialDerivative, + const boost::shared_ptr< SphericalHarmonicsCache > sphericalHarmonicsCache ); } // namespace basic_mathematics } // namespace tudat diff --git a/Tudat/Mathematics/GeometricShapes/capsule.cpp b/Tudat/Mathematics/GeometricShapes/capsule.cpp index 883dc5a1bf..2226074408 100644 --- a/Tudat/Mathematics/GeometricShapes/capsule.cpp +++ b/Tudat/Mathematics/GeometricShapes/capsule.cpp @@ -169,15 +169,13 @@ Capsule::Capsule( const double noseRadius, //! Overload ostream to print class information. std::ostream &operator<<( std::ostream &stream, Capsule& capsule ) { - using std::endl; - - stream << "This is a capsule." << endl; - stream << "The defining parameters are: "<< endl - << "Nose radius: " << capsule.getNoseRadius( ) << endl - << "Mid radius: " << capsule.getMiddleRadius( ) << endl - << "Rear length: " << capsule.getRearLength( ) << endl - << "Rear angle: " << capsule.getRearAngle( ) << endl - << "Side radius: " << capsule.getSideRadius( )<< endl; + stream << "This is a capsule." << std::endl; + stream << "The defining parameters are: "<< std::endl + << "Nose radius: " << capsule.getNoseRadius( ) << std::endl + << "Mid radius: " << capsule.getMiddleRadius( ) << std::endl + << "Rear length: " << capsule.getRearLength( ) << std::endl + << "Rear angle: " << capsule.getRearAngle( ) << std::endl + << "Side radius: " << capsule.getSideRadius( )<< std::endl; return stream; } diff --git a/Tudat/Mathematics/GeometricShapes/conicalFrustum.cpp b/Tudat/Mathematics/GeometricShapes/conicalFrustum.cpp index ab081aff09..30520f2f85 100644 --- a/Tudat/Mathematics/GeometricShapes/conicalFrustum.cpp +++ b/Tudat/Mathematics/GeometricShapes/conicalFrustum.cpp @@ -40,17 +40,18 @@ * */ +#include + #include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h" #include "Tudat/Mathematics/GeometricShapes/conicalFrustum.h" + namespace tudat { namespace geometric_shapes { using mathematical_constants::PI; -using std::cerr; -using std::endl; using std::sin; using std::cos; @@ -108,8 +109,7 @@ Eigen::VectorXd ConicalFrustum::getSurfaceDerivative( derivative_( 1 ) = 0.0; derivative_( 2 ) = 0.0; - cerr << " No negative derivatives allowed when retrieving cone " - << "derivative, returning 0,0,0" << endl; + throw std::runtime_error( " No negative derivatives allowed when retrieving cone derivative " ); } // When requesting the zeroth derivative with respect to the two @@ -155,8 +155,7 @@ Eigen::VectorXd ConicalFrustum::getSurfaceDerivative( // Since this derivative is "cyclical", as it is only dependant on sines // and cosines, only the "modulo 4"th derivative need be determined. // Derivatives are determined from the form of the cylindrical coordinates, - // see basic_mathematics::coordinateConversions::convertSphericalToCartesian from - // Tudat Core. + // see basic_mathematics::coordinateConversions::convertSphericalToCartesian switch( powerOfAzimuthAngleDerivative % 4 ) { case( 0 ): @@ -189,8 +188,7 @@ Eigen::VectorXd ConicalFrustum::getSurfaceDerivative( default: - cerr << " Bad value for powerOfAzimuthAngleDerivative " - << " ( mod 4 ) of value is not 0, 1, 2 or 3 " << endl; + throw std::runtime_error( " Bad value for powerOfAzimuthAngleDerivative ( mod 4 ) of value is not 0, 1, 2 or 3 "); } // Combine contributions to derivative. @@ -228,10 +226,8 @@ double ConicalFrustum::getParameter( const int index ) break; default: - - cerr << "Parameter " << index << " does not exist in ConicalFrustum, returning 0" << endl; - parameter_ = 0; - break; + std::string errorMessage = "Parameter "+ boost::lexical_cast< std::string >( index ) + "does not exist in ConicalFrustum."; + throw std::runtime_error( errorMessage ); } // Return parameter. @@ -241,14 +237,14 @@ double ConicalFrustum::getParameter( const int index ) //! Overload ostream to print class information. std::ostream &operator<<( std::ostream &stream, ConicalFrustum& conicalFrustum ) { - stream << "This is a conical frustum geometry." << endl; + stream << "This is a conical frustum geometry." << std::endl; stream << "The circumferential angle runs from: " << conicalFrustum.getMinimumAzimuthAngle( ) * 180.0 / PI << " degrees to " - << conicalFrustum.getMaximumAzimuthAngle( ) * 180.0 / PI << " degrees" << endl; - stream << "The start radius is: " << conicalFrustum.getStartRadius( ) << endl; - stream << "The length is: " << conicalFrustum.getLength( ) << endl; + << conicalFrustum.getMaximumAzimuthAngle( ) * 180.0 / PI << " degrees" << std::endl; + stream << "The start radius is: " << conicalFrustum.getStartRadius( ) << std::endl; + stream << "The length is: " << conicalFrustum.getLength( ) << std::endl; stream << "The cone half angle is: " << conicalFrustum.getConeHalfAngle( ) * 180.0 / PI - << " degrees" << endl; + << " degrees" << std::endl; // Return stream. return stream; diff --git a/Tudat/Mathematics/GeometricShapes/lawgsPartGeometry.cpp b/Tudat/Mathematics/GeometricShapes/lawgsPartGeometry.cpp index 21c9c16aa6..f1b6c7b941 100644 --- a/Tudat/Mathematics/GeometricShapes/lawgsPartGeometry.cpp +++ b/Tudat/Mathematics/GeometricShapes/lawgsPartGeometry.cpp @@ -41,6 +41,7 @@ */ #include +#include #include "Tudat/Mathematics/GeometricShapes/lawgsPartGeometry.h" @@ -156,10 +157,11 @@ Eigen::VectorXd LawgsPartGeometry::getSurfaceDerivative( const double u, const d const int uDerivative, const int vDerivative ) { - std::cerr << "Surface derivative function not implemented in " - << "LawgsPartGeometry class. Not able to return the " - << uDerivative << ", " << vDerivative << "th derivative at point," - << u << ", " << v << ". Returning zero vector." << std::endl; + std::string errorMessage = "Warning, surface derivative function not implemented in LawgsPartGeometry class. Not able to return the " + + boost::lexical_cast< std::string >( uDerivative ) + ", " + + boost::lexical_cast< std::string >( vDerivative ) + "the derivative at point," + + boost::lexical_cast< std::string >( u ) + ", " + boost::lexical_cast< std::string >( v ); + throw std::runtime_error( errorMessage ); return Eigen::Vector3d( 0.0, 0.0, 0.0 ); } @@ -167,9 +169,9 @@ Eigen::VectorXd LawgsPartGeometry::getSurfaceDerivative( const double u, const d //! Get parameter. double LawgsPartGeometry::getParameter( const int parameterIndex ) { - std::cerr << "Get parameter function not implemented in LawgsPartGeometry" - << "class, unable to retrieve parameter "<< parameterIndex - << ". Returning zero." << std::endl; + std::string errorMessage = "WWarning, get parameter function not implemented in LawgsPartGeometry class, unable to retrieve parameter" + + boost::lexical_cast< std::string >( parameterIndex ); + throw std::runtime_error( errorMessage ); return 0.0; } @@ -177,9 +179,9 @@ double LawgsPartGeometry::getParameter( const int parameterIndex ) //! Set parameter. void LawgsPartGeometry::setParameter( const int parameterIndex, const double value ) { - std::cerr << "Set parameter function not implemented in LawgsPartGeometry" - << "class. Unable to set value of " << value << " at parameter index " - << parameterIndex << std::endl; + std::string errorMessage = "WWarning, set parameter function not implemented in LawgsPartGeometry class, unable to set parameter" + + boost::lexical_cast< std::string >( parameterIndex ); + throw std::runtime_error( errorMessage ); } //! Overload ostream to print class information. diff --git a/Tudat/Mathematics/GeometricShapes/quadrilateralMeshedSurfaceGeometry.cpp b/Tudat/Mathematics/GeometricShapes/quadrilateralMeshedSurfaceGeometry.cpp index 241d32acb8..f1ccf4a58c 100644 --- a/Tudat/Mathematics/GeometricShapes/quadrilateralMeshedSurfaceGeometry.cpp +++ b/Tudat/Mathematics/GeometricShapes/quadrilateralMeshedSurfaceGeometry.cpp @@ -91,7 +91,7 @@ void QuadrilateralMeshedSurfaceGeometry::performPanelCalculations( ) panelAreas_[ i ][ j ] = panelSurfaceNormals_[ i ][ j ].norm( ); if ( panelAreas_[ i ][ j ] < std::numeric_limits< double >::epsilon( ) ) { - std::cerr << "WARNING panel area is zero in part at panel" << i + std::cerr << "Warning, panel area is zero in part at panel" << i << ", " << j << std::endl; } diff --git a/Tudat/Mathematics/GeometricShapes/singleSurfaceGeometry.cpp b/Tudat/Mathematics/GeometricShapes/singleSurfaceGeometry.cpp index 53561a5620..b87ad2c5ba 100644 --- a/Tudat/Mathematics/GeometricShapes/singleSurfaceGeometry.cpp +++ b/Tudat/Mathematics/GeometricShapes/singleSurfaceGeometry.cpp @@ -49,7 +49,7 @@ * */ -#include +#include #include "Tudat/Mathematics/GeometricShapes/singleSurfaceGeometry.h" @@ -58,9 +58,6 @@ namespace tudat namespace geometric_shapes { -using std::cerr; -using std::endl; - //! Set minimum value of independent variable. void SingleSurfaceGeometry::setMinimumIndependentVariable( const int parameterIndex, const double minimumValue ) @@ -86,9 +83,9 @@ void SingleSurfaceGeometry::setMinimumIndependentVariable( const int parameterIn default: - cerr << " Only 2 independent variables, variable " - << parameterIndex << " does not exist when" - << "setting minimum value"<< endl; + std::string errorMessage = "Only 2 independent variables, variable " + + boost::lexical_cast< std::string >( parameterIndex ) + " does not exist when setting minimum value"; + throw std::runtime_error( errorMessage ); } } @@ -118,9 +115,9 @@ void SingleSurfaceGeometry::setMaximumIndependentVariable( default: - cerr << " Only 2 independent variables, variable " - << parameterIndex << " does not exist when" - << "setting maximum value"<< endl; + std::string errorMessage = "Only 2 independent variables, variable " + + boost::lexical_cast< std::string >( parameterIndex ) + " does not exist when setting maximum value"; + throw std::runtime_error( errorMessage ); } } @@ -147,11 +144,9 @@ double SingleSurfaceGeometry::getMinimumIndependentVariable( const int parameter default: - minimumValue_ = -0.0; - - cerr << " Only 2 independent variables, variable " - << parameterIndex << " does not exist when " - << "getting minimum value, returning -0.0" << endl; + std::string errorMessage = "Only 2 independent variables, variable " + + boost::lexical_cast< std::string >( parameterIndex ) + " does not exist when getting minimum value"; + throw std::runtime_error( errorMessage ); } // Return minimum value. @@ -180,11 +175,9 @@ double SingleSurfaceGeometry::getMaximumIndependentVariable( const int parameter break; default: - - maximumValue_ = -0.0; - - cerr << "Only 2 independent variables, variable " << parameterIndex << " does not exist " - << "when getting maximum value, returning -0.0" << endl; + std::string errorMessage = "Only 2 independent variables, variable " + + boost::lexical_cast< std::string >( parameterIndex ) + " does not exist when getting maximum value"; + throw std::runtime_error( errorMessage ); } // Return maximum value. diff --git a/Tudat/Mathematics/GeometricShapes/sphereSegment.cpp b/Tudat/Mathematics/GeometricShapes/sphereSegment.cpp index 5f5a1f9226..3c0632e198 100644 --- a/Tudat/Mathematics/GeometricShapes/sphereSegment.cpp +++ b/Tudat/Mathematics/GeometricShapes/sphereSegment.cpp @@ -48,6 +48,8 @@ #include +#include + #include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h" #include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h" @@ -59,8 +61,6 @@ namespace geometric_shapes { // Using declarations. -using std::cerr; -using std::endl; using std::cos; using std::sin; using unit_conversions::convertRadiansToDegrees; @@ -113,7 +113,7 @@ Eigen::VectorXd SphereSegment::getSurfaceDerivative( const double azimuthAngle, derivative_( 1 ) = 0.0; derivative_( 2 ) = 0.0; - cerr << "No negative power of derivatives allowed, returning 0,0,0" << endl; + throw std::runtime_error( "No negative power of derivatives allowed in sphere segment." ); } // When requesting the zeroth derivative with respect to the two @@ -136,8 +136,7 @@ Eigen::VectorXd SphereSegment::getSurfaceDerivative( const double azimuthAngle, // only dependent on sines and cosines, only the "modulo 4"th // derivatives need to be determined. Derivatives are determined // from the form of the spherical coordinates, see - // basic_mathematics::coordinateConversions::convertSphericalToCartesian from - // Tudat Core. + // basic_mathematics::coordinateConversions::convertSphericalToCartesian switch( powerOfAzimuthAngleDerivative % 4 ) { case( 0 ): @@ -170,8 +169,8 @@ Eigen::VectorXd SphereSegment::getSurfaceDerivative( const double azimuthAngle, default: - cerr << " Bad value for powerOfAzimuthAngleDerivative" - << " ( mod 4 ) of value is not 0, 1, 2 or 3 " << endl; + throw std::runtime_error( " Bad value for powerOfAzimuthAngleDerivative ( mod 4 ) of value is not 0, 1, 2 or 3 in sphere segment." ); + } // This derivative is "cyclical" in the same manner as the derivative @@ -208,8 +207,7 @@ Eigen::VectorXd SphereSegment::getSurfaceDerivative( const double azimuthAngle, default: - cerr << " Bad value for powerOfZenithAngleDerivative" - << " ( mod 4 ) of value is not 0, 1, 2 or 3 " << endl; + throw std::runtime_error( " Bad value for powerOfZenithAngleDerivative ( mod 4 ) of value is not 0, 1, 2 or 3 in sphere segment." ); } // Construct the full derivative. @@ -239,10 +237,8 @@ double SphereSegment::getParameter( const int index ) // Else return cerr statement. else { - parameter_ = -0.0; - - // Cerr statement. - cerr << "Parameter does not exist" << endl; + std::string errorMessage = "Parameter "+ boost::lexical_cast< std::string >( index ) + "does not exist in sphere segment."; + throw std::runtime_error( errorMessage ); } // Return parameter. @@ -252,19 +248,19 @@ double SphereSegment::getParameter( const int index ) //! Overload ostream to print class information. std::ostream& operator<<( std::ostream& stream, SphereSegment& sphereSegment ) { - stream << "This is a sphere segment geometry." << endl; - stream << "The range of the independent variables are: " << endl; + stream << "This is a sphere segment geometry." << std::endl; + stream << "The range of the independent variables are: " << std::endl; stream << "Azimuth angle: " << convertRadiansToDegrees( sphereSegment.getMinimumIndependentVariable( 1 ) ) << " degrees to " << convertRadiansToDegrees( sphereSegment.getMaximumIndependentVariable( 1 ) ) - << " degrees" << endl; + << " degrees" << std::endl; stream << "Zenith angle: " << convertRadiansToDegrees( sphereSegment.getMinimumIndependentVariable( 2 ) ) << " degrees to " << convertRadiansToDegrees( sphereSegment.getMaximumIndependentVariable( 2 ) ) - << " degrees" << endl; - stream << "The radius is: " << sphereSegment.getRadius( ) << " meter." << endl; + << " degrees" << std::endl; + stream << "The radius is: " << sphereSegment.getRadius( ) << " meter." << std::endl; // Return stream. return stream; diff --git a/Tudat/Mathematics/GeometricShapes/sphereSegment.h b/Tudat/Mathematics/GeometricShapes/sphereSegment.h index 0a6388b12d..1c4b7db8a8 100644 --- a/Tudat/Mathematics/GeometricShapes/sphereSegment.h +++ b/Tudat/Mathematics/GeometricShapes/sphereSegment.h @@ -64,7 +64,7 @@ namespace geometric_shapes /*! * Class that defines the sphere ( segment ) shape. Parametrization is based * on spherical coordinates, with azimuth and zenith angles as 1st and 2nd - * variables (see basic_mathematics::convertSphericalToCartesian from Tudat Core). + * variables (see basic_mathematics::convertSphericalToCartesian). * In addition to the minimum and maximum of these two variables, the sphere radius is * required for defining the sphere. * Note that by using the scalingMatrix_ member variable, this class can also diff --git a/Tudat/Mathematics/GeometricShapes/surfaceGeometry.h b/Tudat/Mathematics/GeometricShapes/surfaceGeometry.h index f9c9bbe924..2cb1cbc95d 100644 --- a/Tudat/Mathematics/GeometricShapes/surfaceGeometry.h +++ b/Tudat/Mathematics/GeometricShapes/surfaceGeometry.h @@ -1,13 +1,11 @@ -/*! Copyright (c) 2010-2012 Delft University of Technology. +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved * - * This software is protected by national and international copyright. - * Any unauthorized use, reproduction or modification is unlawful and - * will be prosecuted. Commercial and non-private application of the - * software in any form is strictly prohibited unless otherwise granted - * by the authors. - * - * The code is provided without any warranty; without even the implied - * warranty of merchantibility or fitness for a particular purpose. + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. * * Changelog * YYMMDD Author Comment @@ -23,15 +21,8 @@ * */ -// Temporary notes (move to class/function doxygen): -// -// ` comments. -// 110204 K. Kumar Minor comment and layout modifications. -// 110905 S. Billemont Reorganized includes. -// Moved (con/de)structors and getter/setters to header. - -#ifndef SURFACEGEOMETRY_H -#define SURFACEGEOMETRY_H +#ifndef TUDAT_SURFACEGEOMETRY_H +#define TUDAT_SURFACEGEOMETRY_H #include @@ -62,8 +53,6 @@ class SurfaceGeometry private: }; -} - -#endif // SURFACEGEOMETRY_H +} // namespace tudat -// End of file. +#endif // TUDAT_SURFACEGEOMETRY_H diff --git a/Tudat/Mathematics/GeometricShapes/torus.cpp b/Tudat/Mathematics/GeometricShapes/torus.cpp index 0bf5ee9248..5626b8f131 100644 --- a/Tudat/Mathematics/GeometricShapes/torus.cpp +++ b/Tudat/Mathematics/GeometricShapes/torus.cpp @@ -42,6 +42,8 @@ #include +#include + #include "Tudat/Mathematics/GeometricShapes/torus.h" namespace tudat @@ -49,8 +51,6 @@ namespace tudat namespace geometric_shapes { -using std::cerr; -using std::endl; using std::sin; using std::cos; using mathematical_constants::PI; @@ -75,12 +75,12 @@ Eigen::VectorXd Torus::getSurfacePoint( const double majorCircumferentialAngle, { // Form cartesian position vector from paranmetrization. cartesianPositionVector_( 0 ) - = ( majorRadius_ + ( minorRadius_ * cos( minorCircumferentialAngle ) ) ) - * cos( majorCircumferentialAngle ); + = ( majorRadius_ + ( minorRadius_ * std::cos( minorCircumferentialAngle ) ) ) + * std::cos( majorCircumferentialAngle ); cartesianPositionVector_( 1 ) - = ( majorRadius_ + ( minorRadius_ * cos( minorCircumferentialAngle ) ) ) - * sin( majorCircumferentialAngle ); - cartesianPositionVector_( 2 ) = minorRadius_ * sin( minorCircumferentialAngle ); + = ( majorRadius_ + ( minorRadius_ * std::cos( minorCircumferentialAngle ) ) ) + * std::sin( majorCircumferentialAngle ); + cartesianPositionVector_( 2 ) = minorRadius_ * std::sin( minorCircumferentialAngle ); // Translate vector. transformPoint( cartesianPositionVector_ ); @@ -131,41 +131,39 @@ Eigen::VectorXd Torus::getSurfaceDerivative( const double majorCircumferentialAn // derivatives need to be determined. Derivatives are determined // from the form of the spherical coordinates, see // basic_mathematics::coordinateConversions::convertSphericalToCartesian - // from Tudat Core. switch( powerOfMajorCircumferentialAngleDerivative % 4 ) { case( 0 ): - derivative1Contribution_( 0 ) = cos( majorCircumferentialAngle ); - derivative1Contribution_( 1 ) = sin( majorCircumferentialAngle ); + derivative1Contribution_( 0 ) = std::cos( majorCircumferentialAngle ); + derivative1Contribution_( 1 ) = std::sin( majorCircumferentialAngle ); derivative1Contribution_( 2 ) = 1.0; break; case( 1 ): - derivative1Contribution_( 0 ) = -sin( majorCircumferentialAngle ); - derivative1Contribution_( 1 ) = cos( majorCircumferentialAngle ); + derivative1Contribution_( 0 ) = -std::sin( majorCircumferentialAngle ); + derivative1Contribution_( 1 ) = std::cos( majorCircumferentialAngle ); derivative1Contribution_( 2 ) = 0.0; break; case( 2 ): - derivative1Contribution_( 0 ) = -cos( majorCircumferentialAngle ); - derivative1Contribution_( 1 ) = -sin( majorCircumferentialAngle ); + derivative1Contribution_( 0 ) = -std::cos( majorCircumferentialAngle ); + derivative1Contribution_( 1 ) = -std::sin( majorCircumferentialAngle ); derivative1Contribution_( 2 ) = 0.0; break; case( 3 ): - derivative1Contribution_( 0 ) = sin( majorCircumferentialAngle ); - derivative1Contribution_( 1 ) = -cos( majorCircumferentialAngle ); + derivative1Contribution_( 0 ) = std::sin( majorCircumferentialAngle ); + derivative1Contribution_( 1 ) = -std::cos( majorCircumferentialAngle ); derivative1Contribution_( 2 ) = 0.0; break; default: - cerr << " Bad value for powerOfMajorCircumferentialAngleDerivative" - << " ( mod 4 ) of value is not 0, 1, 2 or 3 " << endl; + throw std::runtime_error( " Bad value for powerOfMajorCircumferentialAngleDerivative ( mod 4 ) of value is not 0, 1, 2 or 3 in torus" ); } // This derivative is "cyclical" in the same manner as the derivative @@ -175,9 +173,9 @@ Eigen::VectorXd Torus::getSurfaceDerivative( const double majorCircumferentialAn { case( 0 ): - derivative2Contribution_( 0 ) = minorRadius_ * cos( minorCircumferentialAngle ); - derivative2Contribution_( 1 ) = minorRadius_ * cos( minorCircumferentialAngle ); - derivative2Contribution_( 2 ) = minorRadius_ * sin( minorCircumferentialAngle ); + derivative2Contribution_( 0 ) = minorRadius_ * std::cos( minorCircumferentialAngle ); + derivative2Contribution_( 1 ) = minorRadius_ * std::cos( minorCircumferentialAngle ); + derivative2Contribution_( 2 ) = minorRadius_ * std::sin( minorCircumferentialAngle ); // For the zeroth derivative, a term needs to be added. if ( powerOfMinorCircumferentialAngleDerivative == 0 ) @@ -190,29 +188,28 @@ Eigen::VectorXd Torus::getSurfaceDerivative( const double majorCircumferentialAn case( 1 ): - derivative2Contribution_( 0 ) = -minorRadius_ * sin( minorCircumferentialAngle ); - derivative2Contribution_( 1 ) = -minorRadius_ * sin( minorCircumferentialAngle ); - derivative2Contribution_( 2 ) = minorRadius_ * cos( minorCircumferentialAngle ); + derivative2Contribution_( 0 ) = -minorRadius_ * std::sin( minorCircumferentialAngle ); + derivative2Contribution_( 1 ) = -minorRadius_ * std::sin( minorCircumferentialAngle ); + derivative2Contribution_( 2 ) = minorRadius_ * std::cos( minorCircumferentialAngle ); break; case( 2 ): - derivative2Contribution_( 0 ) = -minorRadius_ * cos( minorCircumferentialAngle ); - derivative2Contribution_( 1 ) = -minorRadius_ * cos( minorCircumferentialAngle ); - derivative2Contribution_( 2 ) = -minorRadius_ * sin( minorCircumferentialAngle ); + derivative2Contribution_( 0 ) = -minorRadius_ * std::cos( minorCircumferentialAngle ); + derivative2Contribution_( 1 ) = -minorRadius_ * std::cos( minorCircumferentialAngle ); + derivative2Contribution_( 2 ) = -minorRadius_ * std::sin( minorCircumferentialAngle ); break; case( 3 ): - derivative2Contribution_( 0 ) = minorRadius_ * sin( minorCircumferentialAngle ); - derivative2Contribution_( 1 ) = minorRadius_ * sin( minorCircumferentialAngle ); - derivative2Contribution_( 2 ) = -minorRadius_ * cos( minorCircumferentialAngle ); + derivative2Contribution_( 0 ) = minorRadius_ * std::sin( minorCircumferentialAngle ); + derivative2Contribution_( 1 ) = minorRadius_ * std::sin( minorCircumferentialAngle ); + derivative2Contribution_( 2 ) = -minorRadius_ * std::cos( minorCircumferentialAngle ); break; default: - cerr << " Bad value for powerOfMinorCircumferentialAngleDerivative" - << " ( mod 4 ) of value is not 0, 1, 2 or 3 " << endl; + throw std::runtime_error( " Bad value for powerOfMinorCircumferentialAngleDerivative ( mod 4 ) of value is not 0, 1, 2 or 3 in torus" ); } // Construct the full derivative. @@ -245,9 +242,8 @@ double Torus::getParameter( const int index ) break; default: - - cerr << "Parameter " << index << " does not exist in Torus, returning 0" << endl; - break; + std::string errorMessage = "Parameter "+ boost::lexical_cast< std::string >( index ) + "does not exist in torus."; + throw std::runtime_error( errorMessage ); } // Return parameter. @@ -258,15 +254,15 @@ double Torus::getParameter( const int index ) //! Overload ostream to print class information. std::ostream &operator<<( std::ostream &stream, Torus& torus ) { - stream << "This is a torus geometry." << endl; + stream << "This is a torus geometry." << std::endl; stream << "The minor angle runs from: " << torus.getMinimumMinorCircumferentialAngle( ) * 180.0 / PI << " degrees to " - << torus.getMaximumMinorCircumferentialAngle( ) * 180.0 / PI << " degrees" << endl; + << torus.getMaximumMinorCircumferentialAngle( ) * 180.0 / PI << " degrees" << std::endl; stream << "The major angle runs from: " << torus.getMinimumMajorCircumferentialAngle( ) * 180.0 / PI << " degrees to " - << torus.getMaximumMajorCircumferentialAngle( ) * 180.0 / PI << " degrees" << endl; - stream << "The major radius is: " << torus.getMajorRadius( ) << endl; - stream << "The minor ( tube ) radius is: " << torus.getMinorRadius( ) << endl; + << torus.getMaximumMajorCircumferentialAngle( ) * 180.0 / PI << " degrees" << std::endl; + stream << "The major radius is: " << torus.getMajorRadius( ) << std::endl; + stream << "The minor ( tube ) radius is: " << torus.getMinorRadius( ) << std::endl; // Return stream. return stream; diff --git a/Tudat/Mathematics/Interpolators/createInterpolator.h b/Tudat/Mathematics/Interpolators/createInterpolator.h index 92d69febd6..79f5c309cd 100644 --- a/Tudat/Mathematics/Interpolators/createInterpolator.h +++ b/Tudat/Mathematics/Interpolators/createInterpolator.h @@ -101,13 +101,16 @@ class LagrangeInterpolatorSettings: public InterpolatorSettings * \param useLongDoubleTimeStep Boolean denoting whether time step is to be a long double, * time step is a double if false. * \param selectedLookupScheme Selected type of lookup scheme for independent variables. + * \param boundaryHandling Variable denoting the method by which the boundary interpolation is handled. */ LagrangeInterpolatorSettings( const int interpolatorOrder, const bool useLongDoubleTimeStep = 0, - const AvailableLookupScheme selectedLookupScheme = huntingAlgorithm ): + const AvailableLookupScheme selectedLookupScheme = huntingAlgorithm, + const LagrangeInterpolatorBoundaryHandling boundaryHandling = lagrange_cubic_spline_boundary_interpolation ): InterpolatorSettings( lagrange_interpolator, selectedLookupScheme ), - interpolatorOrder_( interpolatorOrder ), useLongDoubleTimeStep_( useLongDoubleTimeStep ) + interpolatorOrder_( interpolatorOrder ), useLongDoubleTimeStep_( useLongDoubleTimeStep ), + boundaryHandling_( boundaryHandling ) { } //! Destructor @@ -133,6 +136,12 @@ class LagrangeInterpolatorSettings: public InterpolatorSettings return useLongDoubleTimeStep_; } + LagrangeInterpolatorBoundaryHandling getBoundaryHandling( ) + { + return boundaryHandling_; + } + + protected: //! Order of the Lagrange interpolator that is to be created. @@ -141,6 +150,8 @@ class LagrangeInterpolatorSettings: public InterpolatorSettings //! Boolean denoting whether time step is to be a long double. bool useLongDoubleTimeStep_; + LagrangeInterpolatorBoundaryHandling boundaryHandling_; + }; @@ -193,14 +204,16 @@ createOneDimensionalInterpolator( createdInterpolator = boost::make_shared< LagrangeInterpolator < IndependentVariableType, DependentVariableType, double > >( dataToInterpolate, lagrangeInterpolatorSettings->getInterpolatorOrder( ), - interpolatorSettings->getSelectedLookupScheme( ) ); + interpolatorSettings->getSelectedLookupScheme( ), + lagrangeInterpolatorSettings->getBoundaryHandling( ) ); } else { createdInterpolator = boost::make_shared< LagrangeInterpolator < IndependentVariableType, DependentVariableType, long double > >( dataToInterpolate, lagrangeInterpolatorSettings->getInterpolatorOrder( ), - interpolatorSettings->getSelectedLookupScheme( ) ); + interpolatorSettings->getSelectedLookupScheme( ), + lagrangeInterpolatorSettings->getBoundaryHandling( ) ); } } else diff --git a/Tudat/Mathematics/Interpolators/cubicSplineInterpolator.h b/Tudat/Mathematics/Interpolators/cubicSplineInterpolator.h index 31e62bc7aa..8fa2501c8b 100644 --- a/Tudat/Mathematics/Interpolators/cubicSplineInterpolator.h +++ b/Tudat/Mathematics/Interpolators/cubicSplineInterpolator.h @@ -85,15 +85,13 @@ std::vector< DependentVariableType > solveTridiagonalMatrixEquation( if ( ( diagonal.size( ) < matrixSize ) || ( superDiagonal.size( ) < matrixSize - 1 ) || ( rightHandSide.size( ) < matrixSize - 1 ) ) { - std::cerr << "Error, input provided for diagonal and sub/super " - " diagonals incorrect." << std::endl; + throw std::runtime_error( "Error, input provided for diagonal and sub/super diagonals incorrect." ); } // Check whether solution will not be singular. if ( diagonal[ 0 ] == 0.0 ) { - std::cerr <<"Error when inverting tridiagonal system, " - "first entry of diagonal is zero" << std::endl; + throw std::runtime_error( "Error when inverting tridiagonal system, first entry of diagonal is zero" ); } std::vector< IndependentVariableType > intermediateVector( matrixSize ); @@ -111,8 +109,7 @@ std::vector< DependentVariableType > solveTridiagonalMatrixEquation( // Check whether solution will not be singular. if ( scalingFactor == 0.0 ) { - std::cerr<<"Error when inverting tridiagonal system," - " scaling factor equals zero!"< HermiteCubicSplineInterpolatorDouble; -} //namespace tudat +} // namespace interpolators -} //namespace interpolators +} // namespace tudat #endif // TUDAT_HERMITE_CUBIC_SPLINE_INTERPOLATOR_H diff --git a/Tudat/Mathematics/Interpolators/lagrangeInterpolator.h b/Tudat/Mathematics/Interpolators/lagrangeInterpolator.h index e8cbc19e8b..e38f4f6fc0 100644 --- a/Tudat/Mathematics/Interpolators/lagrangeInterpolator.h +++ b/Tudat/Mathematics/Interpolators/lagrangeInterpolator.h @@ -475,7 +475,7 @@ class LagrangeInterpolator : public OneDimensionalInterpolator< IndependentVaria //! Typedef for LagrangeInterpolator with double as both its dependent and independent data type. typedef LagrangeInterpolator< double, double > LagrangeInterpolatorDouble; -} +} // namespace interpolators -} +} // namespace tudat #endif // TUDAT_LAGRANGEINTERPOLATOR_H diff --git a/Tudat/Mathematics/Interpolators/multiLinearInterpolator.h b/Tudat/Mathematics/Interpolators/multiLinearInterpolator.h index c2e263db43..e214ee894d 100644 --- a/Tudat/Mathematics/Interpolators/multiLinearInterpolator.h +++ b/Tudat/Mathematics/Interpolators/multiLinearInterpolator.h @@ -57,6 +57,7 @@ #include #include #include +#include #include "Tudat/Mathematics/Interpolators/lookupScheme.h" #include "Tudat/Mathematics/Interpolators/interpolator.h" @@ -104,8 +105,7 @@ class MultiLinearInterpolator: public Interpolator< IndependentVariableType, // Check consistency of template arguments and input variables. if ( independentValues.size( ) != numberOfDimensions ) { - std::cerr << "Error: dimension of independent value vector provided to constructor"; - std::cerr << "incompatible with tenmplate parameter " << std::endl; + throw std::runtime_error( "Error: dimension of independent value vector provided to constructor incompatible with template parameter " ); } // Check consistency of input data of dependent and independent data. @@ -113,8 +113,9 @@ class MultiLinearInterpolator: public Interpolator< IndependentVariableType, { if ( independentValues[ i ].size( ) != dependentData.shape( )[ i ] ) { - std::cerr << "Warning: number of data points in dimension" << i - << " of independent and dependent data incompatible" << std::endl; + std::string errorMessage = "Warning: number of data points in dimension" + + boost::lexical_cast< std::string >( i ) + "of independent and dependent data incompatible"; + throw std::runtime_error( errorMessage ); } } @@ -214,8 +215,7 @@ class MultiLinearInterpolator: public Interpolator< IndependentVariableType, default: - std::cerr << "Warning: lookup scheme not found when making scheme for 1-D interpolator" - << std::endl; + throw std::runtime_error( "Warning: lookup scheme not found when making scheme for 1-D interpolator" ); } } diff --git a/Tudat/Mathematics/Interpolators/oneDimensionalInterpolator.h b/Tudat/Mathematics/Interpolators/oneDimensionalInterpolator.h index 264412e5c1..0dd028e53b 100644 --- a/Tudat/Mathematics/Interpolators/oneDimensionalInterpolator.h +++ b/Tudat/Mathematics/Interpolators/oneDimensionalInterpolator.h @@ -176,8 +176,7 @@ class OneDimensionalInterpolator : break; default: - std::cerr << "Warning: lookup scheme not found when making scheme for 1-D interpolator" - << std::endl; + throw std::runtime_error( "Warning: lookup scheme not found when making scheme for 1-D interpolator" ); } } diff --git a/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h index 17ee3a7939..a741250ebe 100644 --- a/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h @@ -1,5 +1,7 @@ -#ifndef CREATENUMERICALINTEGRATOR_H -#define CREATENUMERICALINTEGRATOR_H +#ifndef TUDAT_CREATENUMERICALINTEGRATOR_H +#define TUDAT_CREATENUMERICALINTEGRATOR_H + +#include #include #include @@ -29,10 +31,9 @@ enum AvailableIntegrators //! Class to define settings of numerical integrator /*! - * Class to define settings of numerical integrator, for instance for use in numerical integration - * of equations of motion/ variational equations. This class can be used for simple integrators - * such as fixed step RK and Euler. Integrators that require more settings to define have their own - * derived class (see below). + * Class to define settings of numerical integrator, for instance for use in numerical integration of equations of motion/ + * variational equations. This class can be used for simple integrators such as fixed step RK and Euler. Integrators that + * require more settings to define have their own derived class (see below). */ template< typename TimeType = double > class IntegratorSettings @@ -44,17 +45,16 @@ class IntegratorSettings * Constructor for integrator settings. * \param integratorType Type of numerical integrator * \param initialTime Start time (independent variable) of numerical integration. - * \param endTime Stop time (independent variable) of numerical integration. - * \param initialTimeStep Initial time (independent variable) step used in numerical - * integration. Adapted during integration for variable step size integrators. - * \param printFrequency Frequency at which to save the numerical integrated states (in units - * of i.e. per n integration time steps, with n = printFrequency). + * \param initialTimeStep Initial time (independent variable) step used in numerical integration. Adapted during integration + * for variable step size integrators. + * \param saveFrequency Frequency at which to save the numerical integrated states (in units of i.e. per n integration + * time steps, with n = saveFrequency). */ IntegratorSettings( const AvailableIntegrators integratorType, const TimeType initialTime, - const TimeType endTime, const TimeType initialTimeStep, - const int printFrequency = 1 ): integratorType_( integratorType ), - initialTime_( initialTime ), endTime_( endTime ), initialTimeStep_( initialTimeStep ), - printFrequency_( printFrequency ){ } + const TimeType initialTimeStep, + const int saveFrequency = 1 ): integratorType_( integratorType ), + initialTime_( initialTime ), initialTimeStep_( initialTimeStep ), + saveFrequency_( saveFrequency ){ } //! Virtual destructor. /*! @@ -74,31 +74,25 @@ class IntegratorSettings */ TimeType initialTime_; - //! Stop time of numerical integration. - /*! - * Stop time (independent variable) of numerical integration. - */ - TimeType endTime_; - //! Initial time step used in numerical integration /*! - * Initial time (independent variable) step used in numerical integration. Adapted during - * integration for variable step size integrators. + * Initial time (independent variable) step used in numerical integration. Adapted during integration + * for variable step size integrators. */ TimeType initialTimeStep_; //! Frequency which with to save numerical integration result. /*! - * Frequency at which to save the numerical integrated states (in units of i.e. per n - * integration time steps, with n = printFrequency). + * Frequency at which to save the numerical integrated states (in units of i.e. per n integration + * time steps, with n = saveFrequency). */ - int printFrequency_; + int saveFrequency_; }; //! Class to define settings of variable step RK numerical integrator /*! - * Class to define settings of variable step RK numerical integrator, for instance for use in - * numerical integration of equations of motion/ variational equations. + * Class to define settings of variable step RK numerical integrator, for instance for use in numerical integration of equations of motion/ + * variational equations. */ template< typename TimeType = double > class RungeKuttaVariableStepSizeSettings: public IntegratorSettings< TimeType > @@ -110,38 +104,34 @@ class RungeKuttaVariableStepSizeSettings: public IntegratorSettings< TimeType > * Constructor for variable step RK integrator settings. * \param integratorType Type of numerical integrator (must be an RK variable step type) * \param initialTime Start time (independent variable) of numerical integration. - * \param endTime Stop time (independent variable) of numerical integration. * \param initialTimeStep Initial time (independent variable) step used in numerical integration. * Adapted during integration * \param coefficientSet Coefficient set (butcher tableau) to use in integration. - * \param minimumStepSize Minimum step size for integration. Integration stops (exception - * thrown) if time step comes below this value. + * \param minimumStepSize Minimum step size for integration. Integration stops (exception thrown) if time step + * comes below this value. * \param maximumStepSize Maximum step size for integration. * \param relativeErrorTolerance Relative error tolerance for step size control * \param absoluteErrorTolerance Absolute error tolerance for step size control - * \param printFrequency Frequency at which to save the numerical integrated states (in units - * of i.e. per n integration time steps, with n = printFrequency). + * \param saveFrequency Frequency at which to save the numerical integrated states (in units of i.e. per n integration + * time steps, with n = saveFrequency). * \param safetyFactorForNextStepSize Safety factor for step size control - * \param maximumFactorIncreaseForNextStepSize Maximum increase factor in time step in - * subsequent iterations. - * \param minimumFactorDecreaseForNextStepSize Maximum decrease factor in time step in - * subsequent iterations. + * \param maximumFactorIncreaseForNextStepSize Maximum increase factor in time step in subsequent iterations. + * \param minimumFactorDecreaseForNextStepSize Maximum decrease factor in time step in subsequent iterations. */ RungeKuttaVariableStepSizeSettings( const AvailableIntegrators integratorType, const TimeType initialTime, - const TimeType endTime, const TimeType initialTimeStep, const numerical_integrators::RungeKuttaCoefficients::CoefficientSets coefficientSet, const TimeType minimumStepSize, const TimeType maximumStepSize, const TimeType relativeErrorTolerance = 1.0E-12, const TimeType absoluteErrorTolerance = 1.0E-12, - const int printFrequency = 1, + const int saveFrequency = 1, const TimeType safetyFactorForNextStepSize = 0.8, const TimeType maximumFactorIncreaseForNextStepSize = 4.0, const TimeType minimumFactorDecreaseForNextStepSize = 0.1 ): - IntegratorSettings< TimeType >( integratorType, initialTime, endTime, initialTimeStep, printFrequency ), + IntegratorSettings< TimeType >( integratorType, initialTime, initialTimeStep, saveFrequency ), coefficientSet_( coefficientSet ), minimumStepSize_( minimumStepSize ), maximumStepSize_( maximumStepSize ), relativeErrorTolerance_( relativeErrorTolerance ), absoluteErrorTolerance_( absoluteErrorTolerance ), safetyFactorForNextStepSize_( safetyFactorForNextStepSize ), @@ -159,8 +149,7 @@ class RungeKuttaVariableStepSizeSettings: public IntegratorSettings< TimeType > //! Minimum step size for integration. /*! - * Minimum step size for integration. Integration stops (exception thrown) if time step comes - * below this value. + * Minimum step size for integration. Integration stops (exception thrown) if time step comes below this value. */ const TimeType minimumStepSize_; @@ -185,17 +174,15 @@ class RungeKuttaVariableStepSizeSettings: public IntegratorSettings< TimeType > //! Function to create a numerical integrator. /*! - * Function to create a numerical integrator from given integrator settings, state derivative - * function and initial state. - * \param stateDerivativeFunction Function returning the state derivative from current time and - * state. + * Function to create a numerical integrator from given integrator settings, state derivative function and initial state. + * \param stateDerivativeFunction Function returning the state derivative from current time and state. * \param initialState Initial state for numerical integration * \param integratorSettings Settings for numerical integrator. * \return Numerical integrator object */ template< typename IndependentVariableType, typename DependentVariableType > -boost::shared_ptr< numerical_integrators::NumericalIntegrator< IndependentVariableType, - DependentVariableType, DependentVariableType > > createIntegrator( +boost::shared_ptr< numerical_integrators::NumericalIntegrator< IndependentVariableType, DependentVariableType, +DependentVariableType > > createIntegrator( boost::function< DependentVariableType( const IndependentVariableType, const DependentVariableType& ) > stateDerivativeFunction, const DependentVariableType initialState, @@ -224,9 +211,9 @@ boost::shared_ptr< numerical_integrators::NumericalIntegrator< IndependentVariab case rungeKuttaVariableStepSize: { // Check input consistency - boost::shared_ptr< RungeKuttaVariableStepSizeSettings< IndependentVariableType > > - variableStepIntegratorSettings = boost::dynamic_pointer_cast - < RungeKuttaVariableStepSizeSettings< IndependentVariableType > >( integratorSettings ); + boost::shared_ptr< RungeKuttaVariableStepSizeSettings< IndependentVariableType > > variableStepIntegratorSettings = + boost::dynamic_pointer_cast< RungeKuttaVariableStepSizeSettings< IndependentVariableType > >( + integratorSettings ); if( variableStepIntegratorSettings == NULL ) { std::runtime_error( "Error, type of integrator settings not compatible with selected integrator" ); @@ -301,8 +288,8 @@ DependentVariableType > > createFixedStepSizeIntegrator( return integrator; } -} +} // namespace numerical_integrators -} +} // namespace tudat -#endif // CREATENUMERICALINTEGRATOR_H +#endif // TUDAT_CREATENUMERICALINTEGRATOR_H diff --git a/Tudat/Mathematics/NumericalIntegrators/euler.h b/Tudat/Mathematics/NumericalIntegrators/euler.h index 48924392bc..ac8eeb24f8 100644 --- a/Tudat/Mathematics/NumericalIntegrators/euler.h +++ b/Tudat/Mathematics/NumericalIntegrators/euler.h @@ -45,9 +45,6 @@ * References * * Notes - * Backwards compatibility of namespaces is implemented for Tudat Core 2 in this file. The - * code block marked "DEPRECATED!" at the end of the file should be removed in Tudat Core 3. - * */ #ifndef TUDAT_EULER_INTEGRATOR_H diff --git a/Tudat/Mathematics/NumericalIntegrators/numericalIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/numericalIntegrator.h index f469893a0d..3cfbc9e9a7 100644 --- a/Tudat/Mathematics/NumericalIntegrators/numericalIntegrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/numericalIntegrator.h @@ -34,8 +34,6 @@ * References * * Notes - * Backwards compatibility of namespaces is implemented for Tudat Core 2 in this file. The - * code block marked "DEPRECATED!" at the end of the file should be removed in Tudat Core 3. * */ @@ -153,6 +151,16 @@ class NumericalIntegrator */ virtual StateType performIntegrationStep( const IndependentVariableType stepSize ) = 0; + //! Function to return the function that computes and returns the state derivative + /*! + * Function to return the function that computes and returns the state derivative + * \return Function that returns the state derivative + */ + StateDerivativeFunction getStateDerivativeFunction( ) + { + return stateDerivativeFunction_; + } + protected: //! Function that returns the state derivative. diff --git a/Tudat/Mathematics/NumericalIntegrators/rungeKutta4Integrator.h b/Tudat/Mathematics/NumericalIntegrators/rungeKutta4Integrator.h index ea58dd22f0..b922107463 100644 --- a/Tudat/Mathematics/NumericalIntegrators/rungeKutta4Integrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/rungeKutta4Integrator.h @@ -38,8 +38,6 @@ * References * * Notes - * Backwards compatibility of namespaces is implemented for Tudat Core 2 in this file. The - * code block marked "DEPRECATED!" at the end of the file should be removed in Tudat Core 3. * */ @@ -259,7 +257,7 @@ typedef boost::shared_ptr< RungeKutta4IntegratorXd > RungeKutta4IntegratorXdPoin */ typedef boost::shared_ptr< RungeKutta4Integratord > RungeKutta4IntegratordPointer; -} // namespace integrators +} // namespace numerical_integrators } // namespace tudat #endif // TUDAT_RUNGE_KUTTA_4_INTEGRATOR_H diff --git a/Tudat/Mathematics/Statistics/boostProbabilityDistributions.h b/Tudat/Mathematics/Statistics/boostProbabilityDistributions.h index 72522d9c3f..5bd4da02c0 100644 --- a/Tudat/Mathematics/Statistics/boostProbabilityDistributions.h +++ b/Tudat/Mathematics/Statistics/boostProbabilityDistributions.h @@ -105,9 +105,9 @@ class BoostContinuousProbabilityDistribution: public InvertibleContinuousProbabi boost::shared_ptr< InvertibleContinuousProbabilityDistribution< double > > createBoostRandomVariable( const ContinuousBoostStatisticalDistributions boostDistribution, const std::vector< double >& parameters ); -} +} // namespace statistics -} +} // namespace tudat #endif // TUDAT_BOOSTPROBABILITYDISTRIBUTIONS_H diff --git a/Tudat/Mathematics/Statistics/continuousProbabilityDistributions.h b/Tudat/Mathematics/Statistics/continuousProbabilityDistributions.h index 5941abc5ef..e5d8caaa84 100644 --- a/Tudat/Mathematics/Statistics/continuousProbabilityDistributions.h +++ b/Tudat/Mathematics/Statistics/continuousProbabilityDistributions.h @@ -106,8 +106,8 @@ class InvertibleContinuousProbabilityDistribution: public ContinuousProbabilityD virtual double evaluateInverseCdf( const IndependentVariableType independentVariable ) = 0; }; -} +} // namespace statistics -} +} // namespace tudat -#endif // CONTINUOUSPROBABILITYDISTRIBUTIONS_H +#endif // TUDAT_CONTINUOUSPROBABILITYDISTRIBUTIONS_H diff --git a/Tudat/Mathematics/Statistics/randomVariableGenerator.h b/Tudat/Mathematics/Statistics/randomVariableGenerator.h index 6bcd086093..4ce9525696 100644 --- a/Tudat/Mathematics/Statistics/randomVariableGenerator.h +++ b/Tudat/Mathematics/Statistics/randomVariableGenerator.h @@ -136,8 +136,8 @@ boost::shared_ptr< RandomVariableGenerator< double > > createBoostContinuousRand const ContinuousBoostStatisticalDistributions boostDistribution, const std::vector< double >& parameters, const double seed ); -} +} // namespace statistics -} +} // namespace tudat -#endif // RANDOMVARIABLEGENERATION_H +#endif // TUDAT_RANDOMVARIABLEGENERATION_H diff --git a/Tudat/SimulationSetup/CMakeLists.txt b/Tudat/SimulationSetup/CMakeLists.txt index 4731714aab..5981cc7ca2 100644 --- a/Tudat/SimulationSetup/CMakeLists.txt +++ b/Tudat/SimulationSetup/CMakeLists.txt @@ -8,50 +8,25 @@ # http://tudat.tudelft.nl/LICENSE. # +set(ESTIMATIONSETUPSDIR "${SIMULATIONSETUPDIR}/EstimationSetup") # Add source files. -set(SIMULATION_SETUP_SOURCES - "${SRCROOT}${SIMULATIONSETUPDIR}/createAccelerationModels.cpp" - "${SRCROOT}${SIMULATIONSETUPDIR}/createAtmosphereModel.cpp" - "${SRCROOT}${SIMULATIONSETUPDIR}/createEphemeris.cpp" - "${SRCROOT}${SIMULATIONSETUPDIR}/createFlightConditions.cpp" - "${SRCROOT}${SIMULATIONSETUPDIR}/createGravityFieldVariations.cpp" - "${SRCROOT}${SIMULATIONSETUPDIR}/createGravityField.cpp" - "${SRCROOT}${SIMULATIONSETUPDIR}/createRotationModel.cpp" - "${SRCROOT}${SIMULATIONSETUPDIR}/createBodies.cpp" - "${SRCROOT}${SIMULATIONSETUPDIR}/createBodyShapeModel.cpp" - "${SRCROOT}${SIMULATIONSETUPDIR}/createRadiationPressureInterface.cpp" - "${SRCROOT}${SIMULATIONSETUPDIR}/defaultBodies.cpp" - "${SRCROOT}${SIMULATIONSETUPDIR}/body.cpp" -) - -# Add header files. -set(SIMULATION_SETUP_HEADERS - "${SRCROOT}${SIMULATIONSETUPDIR}/accelerationSettings.h" - "${SRCROOT}${SIMULATIONSETUPDIR}/createAccelerationModels.h" - "${SRCROOT}${SIMULATIONSETUPDIR}/createAtmosphereModel.h" - "${SRCROOT}${SIMULATIONSETUPDIR}/createEphemeris.h" - "${SRCROOT}${SIMULATIONSETUPDIR}/createFlightConditions.h" - "${SRCROOT}${SIMULATIONSETUPDIR}/createGravityFieldVariations.h" - "${SRCROOT}${SIMULATIONSETUPDIR}/createGravityField.h" - "${SRCROOT}${SIMULATIONSETUPDIR}/createRotationModel.h" - "${SRCROOT}${SIMULATIONSETUPDIR}/createBodies.h" - "${SRCROOT}${SIMULATIONSETUPDIR}/createBodyShapeModel.h" - "${SRCROOT}${SIMULATIONSETUPDIR}/createRadiationPressureInterface.h" - "${SRCROOT}${SIMULATIONSETUPDIR}/defaultBodies.h" - "${SRCROOT}${SIMULATIONSETUPDIR}/body.h" -) + + +file(GLOB_RECURSE SIMULATION_SETUP_HEADERS ${SRCROOT}/SimulationSetup ABSOLUTE ${CODEROOT} *.h) +file(GLOB_RECURSE SIMULATION_SETUP_SOURCES ${SRCROOT}/SimulationSetup ABSOLUTE ${CODEROOT} *.cpp) + # Add static libraries. add_library(tudat_simulation_setup STATIC ${SIMULATION_SETUP_SOURCES} ${SIMULATION_SETUP_HEADERS} ) setup_tudat_library_target(tudat_simulation_setup "${SRCROOT}${SIMULATIONSETUPDIR}") # Add unit tests. +if(USE_CSPICE) add_executable(test_EnvironmentSetup "${SRCROOT}${SIMULATIONSETUPDIR}/UnitTests/unitTestEnvironmentModelSetup.cpp") setup_custom_test_program(test_EnvironmentSetup "${SRCROOT}${SIMULATIONSETUPDIR}/") target_link_libraries(test_EnvironmentSetup ${TUDAT_PROPAGATION_LIBRARIES} ${Boost_LIBRARIES}) -if(USE_CSPICE) add_executable(test_AccelerationModelSetup "${SRCROOT}${SIMULATIONSETUPDIR}/UnitTests/unitTestAccelerationModelSetup.cpp") setup_custom_test_program(test_AccelerationModelSetup "${SRCROOT}${SIMULATIONSETUPDIR}/") target_link_libraries(test_AccelerationModelSetup ${TUDAT_PROPAGATION_LIBRARIES} ${Boost_LIBRARIES}) diff --git a/Tudat/SimulationSetup/body.cpp b/Tudat/SimulationSetup/EnvironmentSetup/body.cpp similarity index 89% rename from Tudat/SimulationSetup/body.cpp rename to Tudat/SimulationSetup/EnvironmentSetup/body.cpp index d2da04fc3d..c5620b8bb3 100644 --- a/Tudat/SimulationSetup/body.cpp +++ b/Tudat/SimulationSetup/EnvironmentSetup/body.cpp @@ -8,7 +8,7 @@ * http://tudat.tudelft.nl/LICENSE. */ -#include "Tudat/SimulationSetup/body.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" namespace tudat { @@ -16,6 +16,18 @@ namespace tudat namespace simulation_setup { +template< > +Eigen::Matrix< double, 6, 1 > Body::getTemplatedState( ) +{ + return getState( ); +} + +template< > +Eigen::Matrix< long double, 6, 1 > Body::getTemplatedState( ) +{ + return getLongState( ); +} + //! Templated function to set the state manually. template< > void Body::setTemplatedState( const Eigen::Matrix< double, 6, 1 >& state ) diff --git a/Tudat/SimulationSetup/body.h b/Tudat/SimulationSetup/EnvironmentSetup/body.h similarity index 77% rename from Tudat/SimulationSetup/body.h rename to Tudat/SimulationSetup/EnvironmentSetup/body.h index 267e3d3596..ab05c66aaf 100644 --- a/Tudat/SimulationSetup/body.h +++ b/Tudat/SimulationSetup/EnvironmentSetup/body.h @@ -30,8 +30,10 @@ #include #include #include +#include #include #include +#include namespace tudat { @@ -191,8 +193,6 @@ class Body template< typename StateScalarType, typename TimeType > Eigen::Matrix< StateScalarType, 6, 1 > getTemplatedStateInBaseFrameFromEphemeris( const TimeType& time ); - - //! Get current state. /*! * Returns the internally stored current state vector. @@ -236,6 +236,8 @@ class Body */ Eigen::Matrix< long double, 3, 1 > getLongVelocity( ) { return currentLongState_.segment( 3, 3 ); } + template< typename ScalarStateType > + Eigen::Matrix< ScalarStateType, 6, 1 > getTemplatedState( ); //! Function to set the rotation from global to body-fixed frame at given time /*! @@ -249,10 +251,14 @@ class Body { currentRotationToLocalFrame_ = rotationalEphemeris_->getRotationToTargetFrame( time ); } + else if( dependentOrientationCalculator_ != NULL ) + { + currentRotationToLocalFrame_ = dependentOrientationCalculator_->getRotationToLocalFrame( time ); + } else { throw std::runtime_error( - "Error, no rotationalEphemeris_ found in Body::setCurrentRotationToLocalFrameFromEphemeris" ); + "Error, no rotation model found in Body::setCurrentRotationToLocalFrameFromEphemeris" ); } } @@ -269,6 +275,10 @@ class Body currentRotationToLocalFrameDerivative_ = rotationalEphemeris_->getDerivativeOfRotationToTargetFrame( time ); } + else if( dependentOrientationCalculator_ != NULL ) + { + currentRotationToLocalFrameDerivative_.setZero( ); + } else { throw std::runtime_error( @@ -289,6 +299,10 @@ class Body currentAngularVelocityVectorInGlobalFrame_ = rotationalEphemeris_->getRotationalVelocityVectorInBaseFrame( time ); } + else if( dependentOrientationCalculator_ != NULL ) + { + currentAngularVelocityVectorInGlobalFrame_.setZero( ); + } else { throw std::runtime_error( @@ -311,6 +325,12 @@ class Body currentRotationToLocalFrame_, currentRotationToLocalFrameDerivative_, currentAngularVelocityVectorInGlobalFrame_, time ); } + else if( dependentOrientationCalculator_ != NULL ) + { + currentRotationToLocalFrame_ = dependentOrientationCalculator_->getRotationToLocalFrame( time ); + currentRotationToLocalFrameDerivative_.setZero( ); + currentAngularVelocityVectorInGlobalFrame_.setZero( ); + } else { throw std::runtime_error( @@ -425,6 +445,7 @@ class Body { std::cerr<<"Warning when settings gravity field model for body, mass function already found: resetting"<getGravitationalParameter( ) / physical_constants::GRAVITATIONAL_CONSTANT; bodyMassFunction_ = boost::lambda::constant( currentMass_ ); @@ -449,9 +470,58 @@ class Body void setRotationalEphemeris( const boost::shared_ptr< ephemerides::RotationalEphemeris > rotationalEphemeris ) { + if( dependentOrientationCalculator_ != NULL ) + { + std::cerr<<"Warning when setting rotational ephemeris, dependentOrientationCalculator_ already found, NOT setting closure"< dependentOrientationCalculator ) + { + // Check if object already exists + if( dependentOrientationCalculator_ != NULL ) + { + // Try to create closure between new and existing objects (i.e ensure that they end up computing the same rotation + // in differen manenrs. + if( ( boost::dynamic_pointer_cast< reference_frames::AerodynamicAngleCalculator >( + dependentOrientationCalculator ) != NULL ) && + ( boost::dynamic_pointer_cast< reference_frames::AerodynamicAngleCalculator >( + dependentOrientationCalculator_ ) == NULL ) ) + { + reference_frames::setAerodynamicDependentOrientationCalculatorClosure( + dependentOrientationCalculator_, + boost::dynamic_pointer_cast< reference_frames::AerodynamicAngleCalculator >( + dependentOrientationCalculator ) ); + } + else if( ( boost::dynamic_pointer_cast< reference_frames::AerodynamicAngleCalculator >( + dependentOrientationCalculator_ ) != NULL ) && + ( boost::dynamic_pointer_cast< reference_frames::AerodynamicAngleCalculator >( + dependentOrientationCalculator ) == NULL ) ) + { + reference_frames::setAerodynamicDependentOrientationCalculatorClosure( + dependentOrientationCalculator, + boost::dynamic_pointer_cast< reference_frames::AerodynamicAngleCalculator >( + dependentOrientationCalculator_ ) ); + } + else + { + std::cerr<< "Warning, cannot reset dependentOrientationCalculator, incompatible object already exists" < aerodynamicFlightConditions ) { aerodynamicFlightConditions_ = aerodynamicFlightConditions; + + // If dependentOrientationCalculator_ object already exists, provide a warning and create closure between the two + if( dependentOrientationCalculator_ != NULL ) + { + reference_frames::setAerodynamicDependentOrientationCalculatorClosure( + dependentOrientationCalculator_, aerodynamicFlightConditions_->getAerodynamicAngleCalculator( ) ); + } + else + { + dependentOrientationCalculator_ = aerodynamicFlightConditions->getAerodynamicAngleCalculator( ); + } + + // Create closure between rotational ephemeris and aerodynamic angle calculator. + if( rotationalEphemeris_ != NULL ) + { + reference_frames::setAerodynamicDependentOrientationCalculatorClosure( + rotationalEphemeris_, aerodynamicFlightConditions_->getAerodynamicAngleCalculator( ) ); + } } //! Function to set the radiation pressure interface of the body, for a single radiation source. @@ -500,6 +588,11 @@ class Body radiationPressureInterfaces_[ radiatingBody ] = radiationPressureInterface; } + //! Function to set object containing all variations in the gravity field of this body. + /*! + * Function to set object containing all variations in the gravity field of this body. + * \param gravityFieldVariationSet Object containing all variations in the gravity field of this body. + */ void setGravityFieldVariationSet( const boost::shared_ptr< gravitation::GravityFieldVariationsSet > gravityFieldVariationSet ) @@ -547,11 +640,32 @@ class Body return rotationalEphemeris_; } + //! Function to retrieve the model to compute the rotation of the body based on the current state of the environment. + /*! + * Function to retrieve the model to compute the rotation of the body based on the current state of the environment + * (model is only valid during propagation). + * \return Model to compute the rotation of the body based on the current state of the environment + */ + boost::shared_ptr< reference_frames::DependentOrientationCalculator > getDependentOrientationCalculator( ) + { + return dependentOrientationCalculator_; + } + + //! Function to retrieve the shape model of body. + /*! + * Function to retrieve the shape model of body. + * \return Shape model of body. + */ boost::shared_ptr< basic_astrodynamics::BodyShapeModel > getShapeModel( ) { return shapeModel_; } + //! Function to retrieve the aerodynamic coefficient model of body. + /*! + * Function to retrieve the body aerodynamic coefficient model of body. + * \return Aerodynamic coefficient model of body. + */ boost::shared_ptr< aerodynamics::AerodynamicCoefficientInterface > getAerodynamicCoefficientInterface( ) { @@ -580,6 +694,14 @@ class Body return radiationPressureInterfaces_; } + //! Function to retrieve a single object describing variation in the gravity field of this body. + /*! + * Function to retrieve a single object describing variation in the gravity field of this body. + * \param deformationType Type of gravity field variation. + * \param identifier Identifier of gravity field variation that is to be retrieved (empty by default; only required + * if multiple variations of same type are present) + * \return Object describing requested variation in the gravity field of this body. + */ std::pair< bool, boost::shared_ptr< gravitation::GravityFieldVariations > > getGravityFieldVariation( const gravitation::BodyDeformationTypes& deformationType, @@ -588,12 +710,35 @@ class Body return gravityFieldVariationSet_->getGravityFieldVariation( deformationType, identifier ); } + //! Function to retrieve object containing all variations in the gravity field of this body. + /*! + * Function to retrieve object containing all variations in the gravity field of this body. + * \return Object containing all variations in the gravity field of this body. + */ boost::shared_ptr< gravitation::GravityFieldVariationsSet > getGravityFieldVariationSet( ) { return gravityFieldVariationSet_; } + //! Function to retrieve container object with hardware systems present on/in body + /*! + * Function to retrieve container object with hardware systems present on/in body. + * \return Container object with hardware systems present on/in body + */ + boost::shared_ptr< system_models::VehicleSystems > getVehicleSystems( ) + { + return vehicleSystems_; + } + //! Function to set container object with hardware systems present on/in body + /*! + * Function to set container object with hardware systems present on/in body (typically only non-NULL for a vehicle). + * \param vehicleSystems Container object with hardware systems present on/in body + */ + void setVehicleSystems( const boost::shared_ptr< system_models::VehicleSystems > vehicleSystems ) + { + vehicleSystems_ = vehicleSystems; + } //! Function to set the function returning body mass as a function of time /*! @@ -654,17 +799,23 @@ class Body } + //! Function to recompute the internal variables of member variables that depend on the ephemerides bodies. + /*! + * Function to recompute the internal variables of member variables that depend on the ephemerides of this and other + * bodies. This function is typically called after equations of motion have been computed and set in environment to + * ensure full model consistency. + */ void updateConstantEphemerisDependentMemberQuantities( ) { if( boost::dynamic_pointer_cast< gravitation::TimeDependentSphericalHarmonicsGravityField >( gravityFieldModel_ ) != NULL ) { - //std::cerr<<"Error, time. dep. update disabled due to circular dependency"<( gravityFieldModel_ )->updateCorrectionFunctions( ); } } + protected: private: @@ -682,12 +833,12 @@ class Body //! Function returning the state of this body's ephemeris origin w.r.t. the global origin //! (as set by setGlobalFrameBodyEphemerides function). boost::function< basic_mathematics::Vector6d( const double& ) > - ephemerisFrameToBaseFrameFunction_; + ephemerisFrameToBaseFrameFunction_; //! Function returning the state of this body's ephemeris origin w.r.t. the global origin //! (as set by setGlobalFrameBodyEphemerides function), with long double precision boost::function< Eigen::Matrix< long double, 6, 1 >( const double& ) > - ephemerisFrameToBaseFrameLongFunction_; + ephemerisFrameToBaseFrameLongFunction_; @@ -716,6 +867,7 @@ class Body //! Gravity field model of body. boost::shared_ptr< gravitation::GravityFieldModel > gravityFieldModel_; + //! Object containing all variations in the gravity field of this body. boost::shared_ptr< gravitation::GravityFieldVariationsSet > gravityFieldVariationSet_; //! Atmosphere model of body. @@ -725,8 +877,7 @@ class Body boost::shared_ptr< basic_astrodynamics::BodyShapeModel > shapeModel_; //! Aerodynamic coefficient model of body. - boost::shared_ptr< aerodynamics::AerodynamicCoefficientInterface > - aerodynamicCoefficientInterface_; + boost::shared_ptr< aerodynamics::AerodynamicCoefficientInterface > aerodynamicCoefficientInterface_; //! Object used for calculating current aerodynamic angles, altitude, etc. boost::shared_ptr< aerodynamics::FlightConditions > aerodynamicFlightConditions_; @@ -734,6 +885,9 @@ class Body //! Rotation model of body. boost::shared_ptr< ephemerides::RotationalEphemeris > rotationalEphemeris_; + //! Model to compute the rotation of the body based on the current state of the environment, only valid during propagation. + boost::shared_ptr< reference_frames::DependentOrientationCalculator > dependentOrientationCalculator_; + //! List of radiation pressure models for the body, with the sources bodies as key std::map< std::string, boost::shared_ptr< electro_magnetism::RadiationPressureInterface > > radiationPressureInterfaces_; @@ -742,9 +896,13 @@ class Body std::map< std::string, boost::shared_ptr< electro_magnetism::RadiationPressureInterface > >::iterator radiationPressureIterator_; + + //! Container object with hardware systems present on/in body (typically only non-NULL for a vehicle). + boost::shared_ptr< system_models::VehicleSystems > vehicleSystems_; + }; -typedef std::map< std::string, boost::shared_ptr< Body > > NamedBodyMap; +typedef std::unordered_map< std::string, boost::shared_ptr< Body > > NamedBodyMap; } // namespace simulation_setup diff --git a/Tudat/SimulationSetup/createAtmosphereModel.cpp b/Tudat/SimulationSetup/EnvironmentSetup/createAtmosphereModel.cpp similarity index 98% rename from Tudat/SimulationSetup/createAtmosphereModel.cpp rename to Tudat/SimulationSetup/EnvironmentSetup/createAtmosphereModel.cpp index bd3e2ccbcf..4b9ad3a03f 100644 --- a/Tudat/SimulationSetup/createAtmosphereModel.cpp +++ b/Tudat/SimulationSetup/EnvironmentSetup/createAtmosphereModel.cpp @@ -20,7 +20,7 @@ #endif #include "Tudat/InputOutput/basicInputOutput.h" #include "Tudat/InputOutput/solarActivityData.h" -#include "Tudat/SimulationSetup/createAtmosphereModel.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createAtmosphereModel.h" namespace tudat { diff --git a/Tudat/SimulationSetup/createAtmosphereModel.h b/Tudat/SimulationSetup/EnvironmentSetup/createAtmosphereModel.h similarity index 100% rename from Tudat/SimulationSetup/createAtmosphereModel.h rename to Tudat/SimulationSetup/EnvironmentSetup/createAtmosphereModel.h diff --git a/Tudat/SimulationSetup/createBodies.cpp b/Tudat/SimulationSetup/EnvironmentSetup/createBodies.cpp similarity index 99% rename from Tudat/SimulationSetup/createBodies.cpp rename to Tudat/SimulationSetup/EnvironmentSetup/createBodies.cpp index 5c5a4cedc9..b32c99fce9 100644 --- a/Tudat/SimulationSetup/createBodies.cpp +++ b/Tudat/SimulationSetup/EnvironmentSetup/createBodies.cpp @@ -16,7 +16,7 @@ #include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h" #include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h" -#include "Tudat/SimulationSetup/createBodies.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h" namespace tudat { diff --git a/Tudat/SimulationSetup/createBodies.h b/Tudat/SimulationSetup/EnvironmentSetup/createBodies.h similarity index 86% rename from Tudat/SimulationSetup/createBodies.h rename to Tudat/SimulationSetup/EnvironmentSetup/createBodies.h index 34ef542de5..6d4c8fb579 100644 --- a/Tudat/SimulationSetup/createBodies.h +++ b/Tudat/SimulationSetup/EnvironmentSetup/createBodies.h @@ -14,15 +14,15 @@ #include "Tudat/Astrodynamics/Ephemerides/ephemeris.h" #include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" -#include "Tudat/SimulationSetup/body.h" -#include "Tudat/SimulationSetup/createEphemeris.h" -#include "Tudat/SimulationSetup/createAtmosphereModel.h" -#include "Tudat/SimulationSetup/createBodyShapeModel.h" -#include "Tudat/SimulationSetup/createEphemeris.h" -#include "Tudat/SimulationSetup/createGravityField.h" -#include "Tudat/SimulationSetup/createRotationModel.h" -#include "Tudat/SimulationSetup/createRadiationPressureInterface.h" -#include "Tudat/SimulationSetup/createFlightConditions.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createEphemeris.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createAtmosphereModel.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createBodyShapeModel.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createEphemeris.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createGravityField.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createRotationModel.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createRadiationPressureInterface.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createFlightConditions.h" namespace tudat { @@ -32,7 +32,7 @@ namespace simulation_setup //! Struct holding settings for a body to be created. /*! - * Struct holding settings for a body to be created. From the settings, a CelestialBody object is + * Struct holding settings for a body to be created. From the settings, a Body object is * created by the createBodies function. Default values can be generated from the function in * defaultBodies.h. */ diff --git a/Tudat/SimulationSetup/createBodyShapeModel.cpp b/Tudat/SimulationSetup/EnvironmentSetup/createBodyShapeModel.cpp similarity index 94% rename from Tudat/SimulationSetup/createBodyShapeModel.cpp rename to Tudat/SimulationSetup/EnvironmentSetup/createBodyShapeModel.cpp index 1f010a7fb5..4c179c760b 100644 --- a/Tudat/SimulationSetup/createBodyShapeModel.cpp +++ b/Tudat/SimulationSetup/EnvironmentSetup/createBodyShapeModel.cpp @@ -7,12 +7,13 @@ * a copy of the license with this file. If not, please or visit: * http://tudat.tudelft.nl/LICENSE. */ + #if USE_CSPICE #include "Tudat/External/SpiceInterface/spiceInterface.h" #endif #include "Tudat/Astrodynamics/BasicAstrodynamics/sphericalBodyShapeModel.h" #include "Tudat/Astrodynamics/BasicAstrodynamics/oblateSpheroidBodyShapeModel.h" -#include "Tudat/SimulationSetup/createBodyShapeModel.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createBodyShapeModel.h" namespace tudat { diff --git a/Tudat/SimulationSetup/createBodyShapeModel.h b/Tudat/SimulationSetup/EnvironmentSetup/createBodyShapeModel.h similarity index 95% rename from Tudat/SimulationSetup/createBodyShapeModel.h rename to Tudat/SimulationSetup/EnvironmentSetup/createBodyShapeModel.h index b2c29cfa5a..b7936d698e 100644 --- a/Tudat/SimulationSetup/createBodyShapeModel.h +++ b/Tudat/SimulationSetup/EnvironmentSetup/createBodyShapeModel.h @@ -14,7 +14,7 @@ #include #include "Tudat/Astrodynamics/BasicAstrodynamics/bodyShapeModel.h" -#include "Tudat/SimulationSetup/body.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" namespace tudat diff --git a/Tudat/SimulationSetup/createEphemeris.cpp b/Tudat/SimulationSetup/EnvironmentSetup/createEphemeris.cpp similarity index 72% rename from Tudat/SimulationSetup/createEphemeris.cpp rename to Tudat/SimulationSetup/EnvironmentSetup/createEphemeris.cpp index c842feb0a9..e613724bfc 100644 --- a/Tudat/SimulationSetup/createEphemeris.cpp +++ b/Tudat/SimulationSetup/EnvironmentSetup/createEphemeris.cpp @@ -21,7 +21,7 @@ #include "Tudat/Astrodynamics/Ephemerides/approximatePlanetPositionsCircularCoplanar.h" #include "Tudat/Astrodynamics/Ephemerides/constantEphemeris.h" #include "Tudat/Mathematics/Interpolators/lagrangeInterpolator.h" -#include "Tudat/SimulationSetup/createEphemeris.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createEphemeris.h" namespace tudat { @@ -31,41 +31,6 @@ namespace simulation_setup using namespace ephemerides; -#if USE_CSPICE -//! Function to create a tabulated ephemeris using data from Spice. -boost::shared_ptr< Ephemeris > createTabulatedEphemerisFromSpice( - const std::string& body, - const double initialTime, - const double endTime, - const double timeStep, - const std::string& observerName, - const std::string& referenceFrameName ) -{ - using namespace interpolators; - - std::map< double, basic_mathematics::Vector6d > timeHistoryOfState; - - // Calculate state from spice at given time intervals and store in timeHistoryOfState. - double currentTime = initialTime; - while( currentTime < endTime ) - { - timeHistoryOfState[ currentTime ] = spice_interface::getBodyCartesianStateAtEpoch( - body, observerName, referenceFrameName, "none", currentTime ); - currentTime += timeStep; - } - - // Create interpolator. - boost::shared_ptr< LagrangeInterpolator< double, basic_mathematics::Vector6d > > interpolator = - boost::make_shared< LagrangeInterpolator< double, basic_mathematics::Vector6d > >( - timeHistoryOfState, 6, huntingAlgorithm, - lagrange_cubic_spline_boundary_interpolation ); - - // Create ephemeris and return. - return boost::make_shared< TabulatedCartesianEphemeris< > >( - interpolator, observerName, referenceFrameName ); -} -#endif - //! Function to create a ephemeris model. boost::shared_ptr< ephemerides::Ephemeris > createBodyEphemeris( const boost::shared_ptr< EphemerisSettings > ephemerisSettings, @@ -77,7 +42,7 @@ boost::shared_ptr< ephemerides::Ephemeris > createBodyEphemeris( // Check which type of ephemeris model is to be created. switch( ephemerisSettings->getEphemerisType( ) ) { - #if USE_CSPICE +#if USE_CSPICE case direct_spice_ephemeris: { // Check consistency of type and class. @@ -128,17 +93,32 @@ boost::shared_ptr< ephemerides::Ephemeris > createBodyEphemeris( } // Create corresponding ephemeris object. - ephemeris = createTabulatedEphemerisFromSpice( + if( !interpolatedEphemerisSettings->getUseLongDoubleStates( ) ) + { + ephemeris = createTabulatedEphemerisFromSpice< double, double >( inputName, interpolatedEphemerisSettings->getInitialTime( ), interpolatedEphemerisSettings->getFinalTime( ), interpolatedEphemerisSettings->getTimeStep( ), interpolatedEphemerisSettings->getFrameOrigin( ), - interpolatedEphemerisSettings->getFrameOrientation( ) ); + interpolatedEphemerisSettings->getFrameOrientation( ), + interpolatedEphemerisSettings->getInterpolatorSettings( ) ); + } + else + { + ephemeris = createTabulatedEphemerisFromSpice< long double, double >( + inputName, + static_cast< long double >( interpolatedEphemerisSettings->getInitialTime( ) ), + static_cast< long double >( interpolatedEphemerisSettings->getFinalTime( ) ), + static_cast< long double >( interpolatedEphemerisSettings->getTimeStep( ) ), + interpolatedEphemerisSettings->getFrameOrigin( ), + interpolatedEphemerisSettings->getFrameOrientation( ), + interpolatedEphemerisSettings->getInterpolatorSettings( ) ); + } } break; } - #endif +#endif case tabulated_ephemeris: { // Check consistency of type and class. @@ -152,14 +132,39 @@ boost::shared_ptr< ephemerides::Ephemeris > createBodyEphemeris( else { // Create corresponding ephemeris object. - ephemeris = boost::make_shared< TabulatedCartesianEphemeris< > >( - boost::make_shared< - interpolators::LagrangeInterpolator< double, basic_mathematics::Vector6d > > - ( tabulatedEphemerisSettings->getBodyStateHistory( ), 6, - interpolators::huntingAlgorithm, - interpolators::lagrange_cubic_spline_boundary_interpolation ), - tabulatedEphemerisSettings->getFrameOrigin( ), - tabulatedEphemerisSettings->getFrameOrientation( ) ); + if( !tabulatedEphemerisSettings->getUseLongDoubleStates( ) ) + { + ephemeris = boost::make_shared< TabulatedCartesianEphemeris< > >( + boost::make_shared< + interpolators::LagrangeInterpolator< double, basic_mathematics::Vector6d > > + ( tabulatedEphemerisSettings->getBodyStateHistory( ), 6, + interpolators::huntingAlgorithm, + interpolators::lagrange_cubic_spline_boundary_interpolation ), + tabulatedEphemerisSettings->getFrameOrigin( ), + tabulatedEphemerisSettings->getFrameOrientation( ) ); + } + else + { + // Cast input history to required type. + std::map< double, basic_mathematics::Vector6d > originalStateHistory = + tabulatedEphemerisSettings->getBodyStateHistory( ); + std::map< double, Eigen::Matrix< long double, 6, 1 > > longStateHistory; + + for( std::map< double, basic_mathematics::Vector6d >::const_iterator stateIterator = + originalStateHistory.begin( ); stateIterator != originalStateHistory.end( ); stateIterator++ ) + { + longStateHistory[ stateIterator->first ] = stateIterator->second.cast< long double >( ); + ephemeris = + boost::make_shared< TabulatedCartesianEphemeris< long double, double > >( + boost::make_shared< interpolators::LagrangeInterpolator< + double, Eigen::Matrix< long double, 6, 1 > > > + ( longStateHistory, 6, + interpolators::huntingAlgorithm, + interpolators::lagrange_cubic_spline_boundary_interpolation ), + tabulatedEphemerisSettings->getFrameOrigin( ), + tabulatedEphemerisSettings->getFrameOrientation( ) ); + } + } } break; } diff --git a/Tudat/SimulationSetup/createEphemeris.h b/Tudat/SimulationSetup/EnvironmentSetup/createEphemeris.h similarity index 87% rename from Tudat/SimulationSetup/createEphemeris.h rename to Tudat/SimulationSetup/EnvironmentSetup/createEphemeris.h index 43d76a9c04..6ac9142137 100644 --- a/Tudat/SimulationSetup/createEphemeris.h +++ b/Tudat/SimulationSetup/EnvironmentSetup/createEphemeris.h @@ -18,7 +18,10 @@ #include "Tudat/InputOutput/matrixTextFileReader.h" #include "Tudat/Astrodynamics/Ephemerides/ephemeris.h" +#include "Tudat/Astrodynamics/Ephemerides/tabulatedEphemeris.h" #include "Tudat/Astrodynamics/Ephemerides/approximatePlanetPositionsBase.h" +#include "Tudat/Mathematics/Interpolators/createInterpolator.h" +#include "Tudat/External/SpiceInterface/spiceInterface.h" namespace tudat { @@ -108,7 +111,6 @@ class EphemerisSettings */ void resetFrameOrientation( const std::string& frameOrientation ){ frameOrientation_ = frameOrientation; } - protected: //! Type of ephemeris model that is to be created. @@ -217,37 +219,62 @@ class InterpolatedSpiceEphemerisSettings: public DirectSpiceEphemerisSettings * (optional "SSB" by default). * \param frameOrientation Orientatioan of the reference frame in which the epehemeris is to be * calculated (optional "ECLIPJ2000" by default). + * \param interpolatorSettings Settings to be used for the state interpolation. */ InterpolatedSpiceEphemerisSettings( double initialTime, double finalTime, double timeStep, std::string frameOrigin = "SSB", - std::string frameOrientation = "ECLIPJ2000" ): + std::string frameOrientation = "ECLIPJ2000", + boost::shared_ptr< interpolators::InterpolatorSettings > interpolatorSettings = + boost::make_shared< interpolators::LagrangeInterpolatorSettings >( 6 ) ): DirectSpiceEphemerisSettings( frameOrigin, frameOrientation, 0, 0, 0, interpolated_spice ), - initialTime_( initialTime ), finalTime_( finalTime ), timeStep_( timeStep ){ } + initialTime_( initialTime ), finalTime_( finalTime ), timeStep_( timeStep ), + interpolatorSettings_( interpolatorSettings ), useLongDoubleStates_( 0 ){ } - //! Function to returns initial time from which interpolated data from Spice should be created. + //! Function to return initial time from which interpolated data from Spice should be created. /*! - * Function to returns initial time from which interpolated data from Spice should be created. + * Function to return initial time from which interpolated data from Spice should be created. * \return Initial time from which interpolated data from Spice should be created. */ double getInitialTime( ){ return initialTime_; } - //! Function to returns final time from which interpolated data from Spice should be created. + //! Function to return final time from which interpolated data from Spice should be created. /*! - * Function to returns final time from which interpolated data from Spice should be created. + * Function to return final time from which interpolated data from Spice should be created. * \return Final time from which interpolated data from Spice should be created. */ double getFinalTime( ){ return finalTime_; } - //! Function to returns time step with which interpolated data from Spice should be created. + //! Function to return time step with which interpolated data from Spice should be created. /*! - * Function to returns time step with which interpolated data from Spice should be created. + * Function to return time step with which interpolated data from Spice should be created. * \return Time step with which interpolated data from Spice should be created. */ double getTimeStep( ){ return timeStep_; } + //! Function to return settings to be used for the state interpolation. + /*! + * Function to return settings to be used for the state interpolation. + * \return Settings to be used for the state interpolation. + */ + + boost::shared_ptr< interpolators::InterpolatorSettings > getInterpolatorSettings( ) + { + return interpolatorSettings_; + } + + bool getUseLongDoubleStates( ) + { + return useLongDoubleStates_; + } + + void setUseLongDoubleStates( const bool useLongDoubleStates ) + { + useLongDoubleStates_ = useLongDoubleStates; + } + private: //! Initial time from which interpolated data from Spice should be created. @@ -259,6 +286,10 @@ class InterpolatedSpiceEphemerisSettings: public DirectSpiceEphemerisSettings //! Time step with which interpolated data from Spice should be created. double timeStep_; + //! Settings to be used for the state interpolation. + boost::shared_ptr< interpolators::InterpolatorSettings > interpolatorSettings_; + + bool useLongDoubleStates_; }; //! EphemerisSettings derived class for defining settings of an approximate ephemeris for major @@ -495,7 +526,7 @@ class TabulatedEphemerisSettings: public EphemerisSettings std::string frameOrigin = "SSB", std::string frameOrientation = "ECLIPJ2000" ): EphemerisSettings( tabulated_ephemeris, frameOrigin, frameOrientation ), - bodyStateHistory_( bodyStateHistory ){ } + bodyStateHistory_( bodyStateHistory ), useLongDoubleStates_( ){ } //! Function returning data map defining discrete data from which an ephemeris is to be created. /*! @@ -505,6 +536,17 @@ class TabulatedEphemerisSettings: public EphemerisSettings std::map< double, basic_mathematics::Vector6d > getBodyStateHistory( ) { return bodyStateHistory_; } + + bool getUseLongDoubleStates( ) + { + return useLongDoubleStates_; + } + + void setUseLongDoubleStates( const bool useLongDoubleStates ) + { + useLongDoubleStates_ = useLongDoubleStates; + } + private: //! Data map defining discrete data from which an ephemeris is to be created. @@ -513,8 +555,12 @@ class TabulatedEphemerisSettings: public EphemerisSettings * ephemeris is to be created. */ std::map< double, basic_mathematics::Vector6d > bodyStateHistory_; + + bool useLongDoubleStates_; }; +#if USE_CSPICE + //! Function to create a tabulated ephemeris using data from Spice. /*! * Function to create a tabulated ephemeris using data from Spice. @@ -532,13 +578,41 @@ class TabulatedEphemerisSettings: public EphemerisSettings * calculated. * \return Tabulated ephemeris using data from Spice. */ +template< typename StateScalarType = double, typename TimeType = double > boost::shared_ptr< ephemerides::Ephemeris > createTabulatedEphemerisFromSpice( const std::string& body, - const double initialTime, - const double endTime, - const double timeStep, + const TimeType initialTime, + const TimeType endTime, + const TimeType timeStep, const std::string& observerName, - const std::string& referenceFrameName ); + const std::string& referenceFrameName, + boost::shared_ptr< interpolators::InterpolatorSettings > interpolatorSettings = + boost::make_shared< interpolators::LagrangeInterpolatorSettings >( 8 ) ) +{ + using namespace interpolators; + + std::map< TimeType, Eigen::Matrix< StateScalarType, 6, 1 > > timeHistoryOfState; + + // Calculate state from spice at given time intervals and store in timeHistoryOfState. + TimeType currentTime = initialTime; + while( currentTime < endTime ) + { + timeHistoryOfState[ currentTime ] = spice_interface::getBodyCartesianStateAtEpoch( + body, observerName, referenceFrameName, "none", static_cast< double >( currentTime ) ). + template cast< StateScalarType >( ); + currentTime += timeStep; + } + + // Create interpolator. + boost::shared_ptr< OneDimensionalInterpolator< TimeType, Eigen::Matrix< StateScalarType, 6, 1 > > > interpolator = + interpolators::createOneDimensionalInterpolator( + timeHistoryOfState, interpolatorSettings ); + + // Create ephemeris and return. + return boost::make_shared< ephemerides::TabulatedCartesianEphemeris< StateScalarType, TimeType > >( + interpolator, observerName, referenceFrameName ); +} +#endif //! Function to create a ephemeris model. /*! diff --git a/Tudat/SimulationSetup/createFlightConditions.cpp b/Tudat/SimulationSetup/EnvironmentSetup/createFlightConditions.cpp similarity index 65% rename from Tudat/SimulationSetup/createFlightConditions.cpp rename to Tudat/SimulationSetup/EnvironmentSetup/createFlightConditions.cpp index 7e1368b9e9..da4d040acd 100644 --- a/Tudat/SimulationSetup/createFlightConditions.cpp +++ b/Tudat/SimulationSetup/EnvironmentSetup/createFlightConditions.cpp @@ -13,7 +13,7 @@ #include "Tudat/Astrodynamics/Aerodynamics/aerodynamicCoefficientInterface.h" #include "Tudat/Astrodynamics/Aerodynamics/customAerodynamicCoefficientInterface.h" -#include "Tudat/SimulationSetup/createFlightConditions.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createFlightConditions.h" namespace tudat { @@ -147,7 +147,8 @@ boost::shared_ptr< aerodynamics::FlightConditions > createFlightConditions( const std::string& nameOfBodyExertingAcceleration, const boost::function< double( ) > angleOfAttackFunction, const boost::function< double( ) > angleOfSideslipFunction, - const boost::function< double( ) > bankAngleFunction ) + const boost::function< double( ) > bankAngleFunction, + const boost::function< void( const double ) > angleUpdateFunction ) { // Check whether all required environment models are set. if( centralBody->getAtmosphereModel( ) == NULL ) @@ -188,33 +189,104 @@ boost::shared_ptr< aerodynamics::FlightConditions > createFlightConditions( boost::bind( &Body::getCurrentRotationToLocalFrame, centralBody ); boost::function< Eigen::Matrix3d( ) > rotationMatrixToFrameDerivativeFunction = boost::bind( &Body::getCurrentRotationMatrixDerivativeToLocalFrame, centralBody ); - boost::function< basic_mathematics::Vector6d( const basic_mathematics::Vector6d& ) > - transformationToCentralBodyFrame = - boost::bind( &ephemerides::transformStateToFrameFromRotationFunctions< double >, - _1, rotationToFrameFunction, rotationMatrixToFrameDerivativeFunction ); + + boost::function< Eigen::Matrix< double, 6, 1 >( ) > bodyStateFunction = boost::bind( &Body::getState, bodyWithFlightConditions ); + boost::function< Eigen::Matrix< double, 6, 1 >( ) > centralBodyStateFunction = boost::bind( &Body::getState, centralBody ); + + boost::function< Eigen::Matrix< double, 6, 1 >( ) > relativeBodyFixedStateFunction = + boost::bind( &ephemerides::transformRelativeStateToFrame< double >, + bodyStateFunction, centralBodyStateFunction, + rotationToFrameFunction, + rotationMatrixToFrameDerivativeFunction ); + + // Create aerodynamic angles calculator and set in flight conditions. + boost::shared_ptr< reference_frames::AerodynamicAngleCalculator > aerodynamicAngleCalculator = + boost::make_shared< reference_frames::AerodynamicAngleCalculator >( + relativeBodyFixedStateFunction, + boost::bind( &simulation_setup::Body::getCurrentRotationToGlobalFrame, centralBody ), + nameOfBodyExertingAcceleration, 1, + angleOfAttackFunction, angleOfSideslipFunction, bankAngleFunction, angleUpdateFunction ); + // Create flight conditions. boost::shared_ptr< aerodynamics::FlightConditions > flightConditions = boost::make_shared< aerodynamics::FlightConditions >( centralBody->getAtmosphereModel( ), altitudeFunction, - boost::bind( &Body::getState, bodyWithFlightConditions ), - boost::bind( &Body::getState, centralBody ), - transformationToCentralBodyFrame, - bodyWithFlightConditions->getAerodynamicCoefficientInterface( ) ); + bodyWithFlightConditions->getAerodynamicCoefficientInterface( ), aerodynamicAngleCalculator ); + + - // Create aerodynamic angles calculator and set in flight conditions. - boost::shared_ptr< reference_frames::AerodynamicAngleCalculator > aerodynamicAngleCalculator = - boost::make_shared< reference_frames::AerodynamicAngleCalculator >( - boost::bind( &aerodynamics::FlightConditions::getCurrentBodyCenteredBodyFixedState, - flightConditions ), - angleOfAttackFunction, angleOfSideslipFunction, bankAngleFunction, 1 ); - flightConditions->setAerodynamicAngleCalculator( aerodynamicAngleCalculator ); return flightConditions; } + +//! Function to set the angle of attack to trimmed conditions. +boost::shared_ptr< aerodynamics::TrimOrientationCalculator > setTrimmedConditions( + const boost::shared_ptr< aerodynamics::FlightConditions > flightConditions ) +{ + // Create trim object. + boost::shared_ptr< aerodynamics::TrimOrientationCalculator > trimOrientation = + boost::make_shared< aerodynamics::TrimOrientationCalculator >( + flightConditions->getAerodynamicCoefficientInterface( ) ); + + // Create angle-of-attack function from trim object. + boost::function< std::vector< double >( ) > untrimmedIndependentVariablesFunction = + boost::bind( &aerodynamics::FlightConditions::getAerodynamicCoefficientIndependentVariables, + flightConditions ); + flightConditions->getAerodynamicAngleCalculator( )->setOrientationAngleFunctions( + boost::bind( &aerodynamics::TrimOrientationCalculator::findTrimAngleOfAttackFromFunction, trimOrientation, + untrimmedIndependentVariablesFunction ) ); + + return trimOrientation; +} + +//! Function to set the angle of attack to trimmed conditions. +boost::shared_ptr< aerodynamics::TrimOrientationCalculator > setTrimmedConditions( + const boost::shared_ptr< Body > bodyWithFlightConditions ) +{ + if( bodyWithFlightConditions->getFlightConditions( ) == NULL ) + { + throw std::runtime_error( "Error, body does not have FlightConditions when setting trim conditions." ); + } + + return setTrimmedConditions( bodyWithFlightConditions->getFlightConditions( ) ); +} + +//! Function that must be called to link the AerodynamicGuidance object to the simulation +void setGuidanceAnglesFunctions( + const boost::shared_ptr< aerodynamics::AerodynamicGuidance > aerodynamicGuidance, + const boost::shared_ptr< reference_frames::AerodynamicAngleCalculator > angleCalculator ) +{ + angleCalculator->setOrientationAngleFunctions( + boost::bind( &aerodynamics::AerodynamicGuidance::getCurrentAngleOfAttack, aerodynamicGuidance ), + boost::bind( &aerodynamics::AerodynamicGuidance::getCurrentAngleOfSideslip, aerodynamicGuidance ), + boost::bind( &aerodynamics::AerodynamicGuidance::getCurrentBankAngle, aerodynamicGuidance ), + boost::bind( &aerodynamics::AerodynamicGuidance::updateGuidance, aerodynamicGuidance,_1 ) ); +} + +//! Function that must be called to link the AerodynamicGuidance object to the simulation +void setGuidanceAnglesFunctions( + const boost::shared_ptr< aerodynamics::AerodynamicGuidance > aerodynamicGuidance, + const boost::shared_ptr< simulation_setup::Body > bodyWithAngles ) +{ + boost::shared_ptr< reference_frames::DependentOrientationCalculator > orientationCalculator = + bodyWithAngles->getDependentOrientationCalculator( ); + boost::shared_ptr< reference_frames::AerodynamicAngleCalculator > angleCalculator = + boost::dynamic_pointer_cast< reference_frames::AerodynamicAngleCalculator >( orientationCalculator ); + + if( angleCalculator == NULL ) + { + throw std::runtime_error( "Error, body does not have AerodynamicAngleCalculator when setting aerodynamic guidance" ); + } + else + { + setGuidanceAnglesFunctions( aerodynamicGuidance, angleCalculator ); + } +} + } // namespace simulation_setup } // namespace tudat diff --git a/Tudat/SimulationSetup/createFlightConditions.h b/Tudat/SimulationSetup/EnvironmentSetup/createFlightConditions.h similarity index 90% rename from Tudat/SimulationSetup/createFlightConditions.h rename to Tudat/SimulationSetup/EnvironmentSetup/createFlightConditions.h index f6c7cc499a..4a9fbe7b4d 100644 --- a/Tudat/SimulationSetup/createFlightConditions.h +++ b/Tudat/SimulationSetup/EnvironmentSetup/createFlightConditions.h @@ -15,10 +15,11 @@ #include +#include "Tudat/Astrodynamics/Aerodynamics/aerodynamicGuidance.h" #include "Tudat/Mathematics/Interpolators/multiLinearInterpolator.h" #include "Tudat/Astrodynamics/Aerodynamics/flightConditions.h" #include "Tudat/Astrodynamics/Aerodynamics/customAerodynamicCoefficientInterface.h" -#include "Tudat/SimulationSetup/body.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" namespace tudat { @@ -218,33 +219,6 @@ class ConstantAerodynamicCoefficientSettings: public AerodynamicCoefficientSetti { public: - //! Constructor. - /*! - * Constructor, omitting all moment coefficient data. - * \param constantForceCoefficient Constant force coefficients. - * \param referenceArea Reference area with which aerodynamic forces and moments are - * non-dimensionalized. - * \param areCoefficientsInAerodynamicFrame Boolean to define whether the aerodynamic - * coefficients are defined in the aerodynamic frame (lift, drag, side force) or in the body - * frame (typically denoted as Cx, Cy, Cz). - * \param areCoefficientsInNegativeAxisDirection Boolean to define whether the aerodynamic - * coefficients are positiver along tyhe positive axes of the body or aerodynamic frame - * (see areCoefficientsInAerodynamicFrame). Note that for (lift, drag, side force), the - * coefficients are typically defined in negative direction. - */ - ConstantAerodynamicCoefficientSettings( - const double referenceArea, - const Eigen::Vector3d& constantForceCoefficient, - const bool areCoefficientsInAerodynamicFrame = 0, - const bool areCoefficientsInNegativeAxisDirection = 1 ): - AerodynamicCoefficientSettings( - constant_aerodynamic_coefficients, TUDAT_NAN, referenceArea, - TUDAT_NAN, Eigen::Vector3d::Zero( ), - std::vector< aerodynamics::AerodynamicCoefficientsIndependentVariables >( ), - areCoefficientsInAerodynamicFrame, areCoefficientsInNegativeAxisDirection ), - constantForceCoefficient_( constantForceCoefficient ), - constantMomentCoefficient_( Eigen::Vector3d::Zero( ) ){ } - //! Constructor. /*! * Constructor. @@ -283,6 +257,34 @@ class ConstantAerodynamicCoefficientSettings: public AerodynamicCoefficientSetti constantMomentCoefficient_( constantMomentCoefficient ) { } + //! Constructor. + /*! + * Constructor, omitting all moment coefficient data. + * \param constantForceCoefficient Constant force coefficients. + * \param referenceArea Reference area with which aerodynamic forces and moments are + * non-dimensionalized. + * \param areCoefficientsInAerodynamicFrame Boolean to define whether the aerodynamic + * coefficients are defined in the aerodynamic frame (lift, drag, side force) or in the body + * frame (typically denoted as Cx, Cy, Cz). + * \param areCoefficientsInNegativeAxisDirection Boolean to define whether the aerodynamic + * coefficients are positiver along tyhe positive axes of the body or aerodynamic frame + * (see areCoefficientsInAerodynamicFrame). Note that for (lift, drag, side force), the + * coefficients are typically defined in negative direction. + */ + ConstantAerodynamicCoefficientSettings( + const double referenceArea, + const Eigen::Vector3d& constantForceCoefficient, + const bool areCoefficientsInAerodynamicFrame = 0, + const bool areCoefficientsInNegativeAxisDirection = 1 ): + AerodynamicCoefficientSettings( + constant_aerodynamic_coefficients, TUDAT_NAN, referenceArea, + TUDAT_NAN, Eigen::Vector3d::Zero( ), + std::vector< aerodynamics::AerodynamicCoefficientsIndependentVariables >( ), + areCoefficientsInAerodynamicFrame, areCoefficientsInNegativeAxisDirection ), + constantForceCoefficient_( constantForceCoefficient ), + constantMomentCoefficient_( Eigen::Vector3d::Zero( ) ){ } + + //! Function to return constant force coefficients. /*! * Function to return constant force coefficients. @@ -598,6 +600,7 @@ createAerodynamicCoefficientInterface( * \param angleOfAttackFunction Function returning the current angle of attack (default 0). * \param angleOfSideslipFunction Function returning the current angle of sideslip (default 0). * \param bankAngleFunction Function returning the current bank angle (default 0). + * \param angleUpdateFunction Function to update the aerodynamic angles to the current time (default none). * \return Flight conditions object for given bodies and settings. */ boost::shared_ptr< aerodynamics::FlightConditions > createFlightConditions( @@ -605,15 +608,54 @@ boost::shared_ptr< aerodynamics::FlightConditions > createFlightConditions( const boost::shared_ptr< Body > centralBody, const std::string& nameOfBodyUndergoingAcceleration, const std::string& nameOfBodyExertingAcceleration, - const boost::function< double( ) > angleOfAttackFunction = - boost::lambda::constant ( 0.0 ), - const boost::function< double( ) > angleOfSideslipFunction = - boost::lambda::constant ( 0.0 ), - const boost::function< double( ) > bankAngleFunction = - boost::lambda::constant ( 0.0 ) ); + const boost::function< double( ) > angleOfAttackFunction = boost::function< double( ) >( ), + const boost::function< double( ) > angleOfSideslipFunction = boost::function< double( ) >( ), + const boost::function< double( ) > bankAngleFunction = boost::function< double( ) >( ), + const boost::function< void( const double ) > angleUpdateFunction = boost::function< void( const double ) >( ) ); + +//! Function to set the angle of attack to trimmed conditions. +/*! + * Function to set the angle of attack to trimmed conditions. Using this function requires teh aerodynamic coefficient + * interface to be dependent on the angle of attack. + * \param flightConditions Flight conditions for body that is to have trimmed conditions. + */ +boost::shared_ptr< aerodynamics::TrimOrientationCalculator > setTrimmedConditions( + const boost::shared_ptr< aerodynamics::FlightConditions > flightConditions ); + + +//! Function to set the angle of attack to trimmed conditions. +/*! + * Function to set the angle of attack to trimmed conditions. Using this function requires teh aerodynamic coefficient + * interface to be dependent on the angle of attack. + * \param bodyWithFlightConditions Body for which trimmed conditions are to be imposed. + */ +boost::shared_ptr< aerodynamics::TrimOrientationCalculator > setTrimmedConditions( + const boost::shared_ptr< Body > bodyWithFlightConditions ); + + +//! Function that must be called to link the AerodynamicGuidance object to the simulation +/*! + * Function that must be called to link the AerodynamicGuidance object to the simulation + * \param aerodynamicGuidance Object computing the current aerodynamic angles. + * \param angleCalculator Object that handles all aerodynamic angles in the numerical propagation + */ +void setGuidanceAnglesFunctions( + const boost::shared_ptr< aerodynamics::AerodynamicGuidance > aerodynamicGuidance, + const boost::shared_ptr< reference_frames::AerodynamicAngleCalculator > angleCalculator ); + +//! Function that must be called to link the AerodynamicGuidance object to the simulation +/*! + * Function that must be called to link the AerodynamicGuidance object to the simulation + * \param aerodynamicGuidance Object computing the current aerodynamic angles. + * \param bodyWithAngles Body for which the orientation is to be controlled. + */ +void setGuidanceAnglesFunctions( + const boost::shared_ptr< aerodynamics::AerodynamicGuidance > aerodynamicGuidance, + const boost::shared_ptr< simulation_setup::Body > bodyWithAngles ); } // namespace simulation_setup } // namespace tudat + #endif // TUDAT_CREATEACCELERATIONMODELS_H diff --git a/Tudat/SimulationSetup/createGravityField.cpp b/Tudat/SimulationSetup/EnvironmentSetup/createGravityField.cpp similarity index 92% rename from Tudat/SimulationSetup/createGravityField.cpp rename to Tudat/SimulationSetup/EnvironmentSetup/createGravityField.cpp index ba55bd6245..c7dacf105b 100644 --- a/Tudat/SimulationSetup/createGravityField.cpp +++ b/Tudat/SimulationSetup/EnvironmentSetup/createGravityField.cpp @@ -16,7 +16,7 @@ #include "Tudat/Astrodynamics/Gravitation/timeDependentSphericalHarmonicsGravityField.h" -#include "Tudat/SimulationSetup/createGravityField.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createGravityField.h" namespace tudat { @@ -104,8 +104,9 @@ std::pair< double, double > readGravityFieldFile( // Check current line for consistency if( vectorOfIndividualStrings.size( ) < 4 ) { - std::cerr<<"Error when reading pds gravity field file, number of fields is " - <( vectorOfIndividualStrings.size( ) ); + throw std::runtime_error( errorMessage ); } else { @@ -159,7 +160,7 @@ boost::shared_ptr< gravitation::GravityFieldModel > createGravityFieldModel( } else if( gravityFieldVariationSettings.size( ) != 0 ) { - std::cerr<<"Error, requested central gravity field, but field variations settings are not empty."< createGravityFieldModel( { if( gravityFieldVariationSettings.size( ) != 0 ) { - std::cerr<<"Error, requested central gravity field, but field variations settings are not empty."< createGravityFieldModel( { if( bodyMap.at( body )->getGravityFieldModel( ) != NULL ) { - std::cerr<<"Warning when making time-dependent gravity field model for body "< #include -#include "Tudat/SimulationSetup/body.h" -#include "Tudat/SimulationSetup/createGravityFieldVariations.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createGravityFieldVariations.h" #include "Tudat/Astrodynamics/Gravitation/gravityFieldModel.h" #include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.h" #include "Tudat/Astrodynamics/Gravitation/gravityFieldVariations.h" diff --git a/Tudat/SimulationSetup/createGravityFieldVariations.cpp b/Tudat/SimulationSetup/EnvironmentSetup/createGravityFieldVariations.cpp similarity index 96% rename from Tudat/SimulationSetup/createGravityFieldVariations.cpp rename to Tudat/SimulationSetup/EnvironmentSetup/createGravityFieldVariations.cpp index 8c9daff31c..476d8b93ec 100644 --- a/Tudat/SimulationSetup/createGravityFieldVariations.cpp +++ b/Tudat/SimulationSetup/EnvironmentSetup/createGravityFieldVariations.cpp @@ -14,7 +14,7 @@ #include "Tudat/Astrodynamics/Gravitation/basicSolidBodyTideGravityFieldVariations.h" #include "Tudat/Astrodynamics/Gravitation/tabulatedGravityFieldVariations.h" #include "Tudat/Astrodynamics/Gravitation/timeDependentSphericalHarmonicsGravityField.h" -#include "Tudat/SimulationSetup/createGravityFieldVariations.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createGravityFieldVariations.h" namespace tudat { @@ -95,7 +95,7 @@ boost::shared_ptr< gravitation::GravityFieldVariationsSet > createGravityFieldMo boost::shared_ptr< gravitation::GravityFieldVariations > createGravityFieldVariationsModel( const boost::shared_ptr< GravityFieldVariationSettings > gravityFieldVariationSettings, const std::string body, - const NamedBodyMap bodyMap ) + const NamedBodyMap& bodyMap ) { using namespace tudat::gravitation; diff --git a/Tudat/SimulationSetup/createGravityFieldVariations.h b/Tudat/SimulationSetup/EnvironmentSetup/createGravityFieldVariations.h similarity index 97% rename from Tudat/SimulationSetup/createGravityFieldVariations.h rename to Tudat/SimulationSetup/EnvironmentSetup/createGravityFieldVariations.h index 99b25efdd3..ae979c6f5e 100644 --- a/Tudat/SimulationSetup/createGravityFieldVariations.h +++ b/Tudat/SimulationSetup/EnvironmentSetup/createGravityFieldVariations.h @@ -17,7 +17,7 @@ #include -#include "Tudat/SimulationSetup/body.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" #include "Tudat/Astrodynamics/Gravitation/gravityFieldVariations.h" #include "Tudat/Mathematics/Interpolators/createInterpolator.h" @@ -309,7 +309,7 @@ boost::shared_ptr< gravitation::GravityFieldVariationsSet > createGravityFieldMo boost::shared_ptr< gravitation::GravityFieldVariations > createGravityFieldVariationsModel( const boost::shared_ptr< GravityFieldVariationSettings > gravityFieldVariationSettings, const std::string body, - const NamedBodyMap bodyMap ); + const NamedBodyMap& bodyMap ); } // namespace simulation_setup } // namespace tudat diff --git a/Tudat/SimulationSetup/createRadiationPressureInterface.cpp b/Tudat/SimulationSetup/EnvironmentSetup/createRadiationPressureInterface.cpp similarity index 96% rename from Tudat/SimulationSetup/createRadiationPressureInterface.cpp rename to Tudat/SimulationSetup/EnvironmentSetup/createRadiationPressureInterface.cpp index 778ae6d938..294d2bbb28 100644 --- a/Tudat/SimulationSetup/createRadiationPressureInterface.cpp +++ b/Tudat/SimulationSetup/EnvironmentSetup/createRadiationPressureInterface.cpp @@ -10,7 +10,7 @@ #include -#include "Tudat/SimulationSetup/createRadiationPressureInterface.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createRadiationPressureInterface.h" #include "Tudat/Astrodynamics/BasicAstrodynamics/sphericalBodyShapeModel.h" #include "Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h" diff --git a/Tudat/SimulationSetup/createRadiationPressureInterface.h b/Tudat/SimulationSetup/EnvironmentSetup/createRadiationPressureInterface.h similarity index 96% rename from Tudat/SimulationSetup/createRadiationPressureInterface.h rename to Tudat/SimulationSetup/EnvironmentSetup/createRadiationPressureInterface.h index 8281b88a86..4189b92531 100644 --- a/Tudat/SimulationSetup/createRadiationPressureInterface.h +++ b/Tudat/SimulationSetup/EnvironmentSetup/createRadiationPressureInterface.h @@ -14,7 +14,7 @@ #include #include -#include "Tudat/SimulationSetup/body.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" #include "Tudat/Astrodynamics/ElectroMagnetism/radiationPressureInterface.h" diff --git a/Tudat/SimulationSetup/createRotationModel.cpp b/Tudat/SimulationSetup/EnvironmentSetup/createRotationModel.cpp similarity index 97% rename from Tudat/SimulationSetup/createRotationModel.cpp rename to Tudat/SimulationSetup/EnvironmentSetup/createRotationModel.cpp index 5636c88491..d05a4a4c9e 100644 --- a/Tudat/SimulationSetup/createRotationModel.cpp +++ b/Tudat/SimulationSetup/EnvironmentSetup/createRotationModel.cpp @@ -15,7 +15,7 @@ #if USE_CSPICE #include "Tudat/External/SpiceInterface/spiceRotationalEphemeris.h" #endif -#include "Tudat/SimulationSetup/createRotationModel.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createRotationModel.h" namespace tudat { diff --git a/Tudat/SimulationSetup/createRotationModel.h b/Tudat/SimulationSetup/EnvironmentSetup/createRotationModel.h similarity index 99% rename from Tudat/SimulationSetup/createRotationModel.h rename to Tudat/SimulationSetup/EnvironmentSetup/createRotationModel.h index 9ee90e5f32..06a65fa610 100644 --- a/Tudat/SimulationSetup/createRotationModel.h +++ b/Tudat/SimulationSetup/EnvironmentSetup/createRotationModel.h @@ -16,7 +16,7 @@ #include -#include "Tudat/SimulationSetup/body.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" #include "Tudat/Astrodynamics/Ephemerides/rotationalEphemeris.h" @@ -87,6 +87,7 @@ class RotationModelSettings */ std::string getTargetFrame( ){ return targetFrame_; } + //! Function to rese the orientation of the base frame. /*! * Function to reset the orientation of the base frame. diff --git a/Tudat/SimulationSetup/defaultBodies.cpp b/Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.cpp similarity index 98% rename from Tudat/SimulationSetup/defaultBodies.cpp rename to Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.cpp index 5d087dacb1..d49b8e9e85 100644 --- a/Tudat/SimulationSetup/defaultBodies.cpp +++ b/Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.cpp @@ -12,7 +12,7 @@ #include "Tudat/External/SpiceInterface/spiceInterface.h" #endif #include "Tudat/InputOutput/basicInputOutput.h" -#include "Tudat/SimulationSetup/defaultBodies.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h" namespace tudat { diff --git a/Tudat/SimulationSetup/defaultBodies.h b/Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h similarity index 99% rename from Tudat/SimulationSetup/defaultBodies.h rename to Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h index a548985548..a7da7f9d08 100644 --- a/Tudat/SimulationSetup/defaultBodies.h +++ b/Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h @@ -11,7 +11,7 @@ #ifndef TUDAT_DEFAULTBODIES_H #define TUDAT_DEFAULTBODIES_H -#include "Tudat/SimulationSetup/createBodies.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h" namespace tudat { diff --git a/Tudat/SimulationSetup/EstimationSetup/createAccelerationPartials.cpp b/Tudat/SimulationSetup/EstimationSetup/createAccelerationPartials.cpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tudat/SimulationSetup/EstimationSetup/createAccelerationPartials.h b/Tudat/SimulationSetup/EstimationSetup/createAccelerationPartials.h new file mode 100644 index 0000000000..a8d9702c5a --- /dev/null +++ b/Tudat/SimulationSetup/EstimationSetup/createAccelerationPartials.h @@ -0,0 +1,310 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_CREATEACCELERATIONPARTIALS_H +#define TUDAT_CREATEACCELERATIONPARTIALS_H + +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" + +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/accelerationPartial.h" +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/centralGravityAccelerationPartial.h" +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/radiationPressureAccelerationPartial.h" +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/thirdBodyGravityPartial.h" +#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/sphericalHarmonicAccelerationPartial.h" +#include "Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/rotationMatrixPartial.h" +#include "Tudat/SimulationSetup/EstimationSetup/createPositionPartials.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h" + + +namespace tudat +{ + +namespace simulation_setup +{ + +//! Function to create a single acceleration partial derivative object. +/*! + * Function to create a single acceleration partial derivative object. + * \param accelerationModel Acceleration model for which a partial derivative is to be computed. + * \param acceleratedBody Pair of name and object of body undergoing acceleration + * \param acceleratingBody Pair of name and object of body exerting acceleration + * \param bodyMap List of all body objects + * \param parametersToEstimate List of parameters that are to be estimated. Empty by default, only required for selected + * types of partials (e.g. spherical harmonic acceleration w.r.t. rotational parameters). + * \return Single acceleration partial derivative object. + */ +template< typename InitialStateParameterType = double, typename ParameterScalarType = InitialStateParameterType > +boost::shared_ptr< acceleration_partials::AccelerationPartial > createAnalyticalAccelerationPartial( + boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > accelerationModel, + const std::pair< std::string, boost::shared_ptr< simulation_setup::Body > > acceleratedBody, + const std::pair< std::string, boost::shared_ptr< simulation_setup::Body > > acceleratingBody, + const simulation_setup::NamedBodyMap& bodyMap, + const boost::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterScalarType > > + parametersToEstimate = + boost::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterScalarType > >( ) ) +{ + using namespace gravitation; + using namespace basic_astrodynamics; + using namespace electro_magnetism; + using namespace aerodynamics; + using namespace acceleration_partials; + + boost::shared_ptr< acceleration_partials::AccelerationPartial > accelerationPartial; + + // Identify current acceleration model type + AvailableAcceleration accelerationType = getAccelerationModelType( accelerationModel ); + switch( accelerationType ) + { + case central_gravity: + + // Check if identifier is consistent with type. + if( boost::dynamic_pointer_cast< CentralGravitationalAccelerationModel3d >( accelerationModel ) == NULL ) + { + throw std::runtime_error( "Acceleration class type does not match acceleration type (central_gravity) when making acceleration partial" ); + } + else + { + // Create partial-calculating object. + accelerationPartial = boost::make_shared< CentralGravitationPartial > + ( boost::dynamic_pointer_cast< CentralGravitationalAccelerationModel3d >( accelerationModel ), + acceleratedBody.first, acceleratingBody.first ); + } + break; + + case third_body_central_gravity: + // Check if identifier is consistent with type. + if( boost::dynamic_pointer_cast< ThirdBodyCentralGravityAcceleration >( accelerationModel ) == NULL ) + { + throw std::runtime_error( "Acceleration class type does not match acceleration type (third_body_central_gravity) when making acceleration partial" ); + } + else + { + boost::shared_ptr< ThirdBodyCentralGravityAcceleration > thirdBodyAccelerationModel = + boost::dynamic_pointer_cast< ThirdBodyCentralGravityAcceleration >( accelerationModel ); + + // Create partials for constituent central gravity accelerations + boost::shared_ptr< CentralGravitationPartial > accelerationPartialForBodyUndergoingAcceleration = + boost::dynamic_pointer_cast< CentralGravitationPartial >( + createAnalyticalAccelerationPartial( + thirdBodyAccelerationModel->getAccelerationModelForBodyUndergoingAcceleration( ), + acceleratedBody, acceleratingBody, bodyMap, parametersToEstimate ) ); + boost::shared_ptr< CentralGravitationPartial > accelerationPartialForCentralBody = + boost::dynamic_pointer_cast< CentralGravitationPartial >( + createAnalyticalAccelerationPartial( + thirdBodyAccelerationModel->getAccelerationModelForCentralBody( ), + std::make_pair( thirdBodyAccelerationModel->getCentralBodyName( ), + bodyMap.at( thirdBodyAccelerationModel->getCentralBodyName( ) ) ), + acceleratingBody, bodyMap, parametersToEstimate ) ); + + // Create partial-calculating object. + accelerationPartial = boost::make_shared< ThirdBodyGravityPartial< CentralGravitationPartial > >( + accelerationPartialForBodyUndergoingAcceleration, + accelerationPartialForCentralBody, acceleratedBody.first, acceleratingBody.first, + thirdBodyAccelerationModel->getCentralBodyName( ) ); + + } + break; + case spherical_harmonic_gravity: + { + // Check if identifier is consistent with type. + boost::shared_ptr< SphericalHarmonicsGravitationalAccelerationModel > sphericalHarmonicAcceleration = + boost::dynamic_pointer_cast< SphericalHarmonicsGravitationalAccelerationModel >( accelerationModel ); + if( sphericalHarmonicAcceleration == NULL ) + { + throw std::runtime_error( + "Acceleration class type does not match acceleration type enum (spher. harm. grav.) set when making acceleration partial" ); + } + else + { + std::map< std::pair< estimatable_parameters::EstimatebleParametersEnum, std::string >, + boost::shared_ptr< observation_partials::RotationMatrixPartial > > + rotationMatrixPartials = observation_partials::createRotationMatrixPartials( + parametersToEstimate, acceleratingBody.first, bodyMap ); + + // Create partial-calculating object. + accelerationPartial = boost::make_shared< SphericalHarmonicsGravityPartial > + ( acceleratedBody.first, acceleratingBody.first, + sphericalHarmonicAcceleration, rotationMatrixPartials ); + } + break; + } + case third_body_spherical_harmonic_gravity: + // Check if identifier is consistent with type. + if( boost::dynamic_pointer_cast< ThirdBodySphericalHarmonicsGravitationalAccelerationModel >( accelerationModel ) == NULL ) + { + throw std::runtime_error( "Acceleration class type does not match acceleration type (third_body_spherical_harmonic_gravity) when making acceleration partial" ); + } + else + { + boost::shared_ptr< ThirdBodySphericalHarmonicsGravitationalAccelerationModel > thirdBodyAccelerationModel = + boost::dynamic_pointer_cast< ThirdBodySphericalHarmonicsGravitationalAccelerationModel >( + accelerationModel ); + + // Create partials for constituent central gravity accelerations + boost::shared_ptr< SphericalHarmonicsGravityPartial > accelerationPartialForBodyUndergoingAcceleration = + boost::dynamic_pointer_cast< SphericalHarmonicsGravityPartial >( + createAnalyticalAccelerationPartial( + thirdBodyAccelerationModel->getAccelerationModelForBodyUndergoingAcceleration( ), + acceleratedBody, acceleratingBody, bodyMap, parametersToEstimate ) ); + boost::shared_ptr< SphericalHarmonicsGravityPartial > accelerationPartialForCentralBody = + boost::dynamic_pointer_cast< SphericalHarmonicsGravityPartial >( + createAnalyticalAccelerationPartial( + thirdBodyAccelerationModel->getAccelerationModelForCentralBody( ), + std::make_pair( thirdBodyAccelerationModel->getCentralBodyName( ), + bodyMap.at( thirdBodyAccelerationModel->getCentralBodyName( ) ) ), + acceleratingBody, bodyMap, parametersToEstimate ) ); + + // Create partial-calculating object. + accelerationPartial = boost::make_shared< ThirdBodyGravityPartial< SphericalHarmonicsGravityPartial > >( + accelerationPartialForBodyUndergoingAcceleration, + accelerationPartialForCentralBody, acceleratedBody.first, acceleratingBody.first, + thirdBodyAccelerationModel->getCentralBodyName( ) ); + + } + break; + case cannon_ball_radiation_pressure: + { + // Check if identifier is consistent with type. + boost::shared_ptr< CannonBallRadiationPressureAcceleration > radiationPressureAcceleration = + boost::dynamic_pointer_cast< CannonBallRadiationPressureAcceleration >( accelerationModel ); + if( radiationPressureAcceleration == NULL ) + { + throw std::runtime_error( "Acceleration class type does not match acceleration type (cannon_ball_radiation_pressure) when making acceleration partial" ); + } + else + { + std::map< std::string, boost::shared_ptr< RadiationPressureInterface > > radiationPressureInterfaces = + acceleratedBody.second->getRadiationPressureInterfaces( ); + + if( radiationPressureInterfaces.count( acceleratingBody.first ) == 0 ) + { + throw std::runtime_error( "No radiation pressure coefficient interface found when making acceleration partial." ); + } + else + { + boost::shared_ptr< RadiationPressureInterface > radiationPressureInterface = + radiationPressureInterfaces.at( acceleratingBody.first ); + + // Create partial-calculating object. + accelerationPartial = boost::make_shared< CannonBallRadiationPressurePartial > + ( radiationPressureInterface, radiationPressureAcceleration->getMassFunction( ), + acceleratedBody.first, acceleratingBody.first ); + } + } + break; + } + default: + std::string errorMessage = "Acceleration model " + boost::lexical_cast< std::string >( accelerationType ) + + " not found when making acceleration partial"; + throw std::runtime_error( errorMessage ); + break; + } + + return accelerationPartial; +} + +//! This function creates acceleration partial objects for translational dynamics +/*! + * This function creates acceleration partial objects for translational dynamics from acceleration models and + * list of bodies' states of which derivatives are needed. The return type is an StateDerivativePartialsMap, + * a standardized type for communicating such lists of these objects. + * \param accelerationMap Map of maps containing list of acceleration models, identifying which acceleration acts on which + * body. + * \param bodyMap List of body objects constituting environment for calculations. + * \param parametersToEstimate List of parameters which are to be estimated. + * \return List of acceleration-partial-calculating objects in StateDerivativePartialsMap type. + */ +template< typename InitialStateParameterType > +orbit_determination::StateDerivativePartialsMap createAccelerationPartialsMap( + const basic_astrodynamics::AccelerationMap& accelerationMap, + const simulation_setup::NamedBodyMap& bodyMap, + const boost::shared_ptr< estimatable_parameters::EstimatableParameterSet< InitialStateParameterType > > + parametersToEstimate ) +{ + // Declare return map. + orbit_determination::StateDerivativePartialsMap accelerationPartialsList; + std::map< std::string, std::map< std::string, + std::vector< boost::shared_ptr< acceleration_partials::AccelerationPartial > > > > + accelerationPartialsMap; + + std::vector< boost::shared_ptr< estimatable_parameters::EstimatableParameter< + Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 > > > > initialDynamicalParameters = + parametersToEstimate->getEstimatedInitialStateParameters( ); + accelerationPartialsList.resize( initialDynamicalParameters.size( ) ); + + // Iterate over list of bodies of which the partials of the accelerations acting on them are required. + for( basic_astrodynamics::AccelerationMap::const_iterator accelerationIterator = accelerationMap.begin( ); + accelerationIterator != accelerationMap.end( ); accelerationIterator++ ) + { + for( unsigned int i = 0; i < initialDynamicalParameters.size( ); i++ ) + { + if( initialDynamicalParameters.at( i )->getParameterName( ).second.first == accelerationIterator->first ) + { + if( ( initialDynamicalParameters.at( i )->getParameterName( ).first == + estimatable_parameters::initial_body_state ) ) + { + // Get object for body undergoing acceleration + const std::string acceleratedBody = accelerationIterator->first; + boost::shared_ptr< simulation_setup::Body > acceleratedBodyObject = bodyMap.at( acceleratedBody ); + + // Retrieve list of accelerations acting on current body. + basic_astrodynamics::SingleBodyAccelerationMap accelerationVector = + accelerationMap.at( acceleratedBody ); + + // Declare list of acceleration partials of current body. + std::vector< boost::shared_ptr< orbit_determination::StateDerivativePartial > > accelerationPartialVector; + + // Iterate over all acceleration models and generate their partial-calculating objects. + for( basic_astrodynamics::SingleBodyAccelerationMap::iterator + innerAccelerationIterator = accelerationVector.begin( ); + innerAccelerationIterator != accelerationVector.end( ); innerAccelerationIterator++ ) + { + // Get object for body exerting acceleration + std::string acceleratingBody = innerAccelerationIterator->first; + boost::shared_ptr< simulation_setup::Body > acceleratingBodyObject; + if( acceleratingBody != "" ) + { + acceleratingBodyObject = bodyMap.at( acceleratingBody ); + } + + for( unsigned int j = 0; j < innerAccelerationIterator->second.size( ); j++ ) + { + // Create single partial object + boost::shared_ptr< acceleration_partials::AccelerationPartial > currentAccelerationPartial = + createAnalyticalAccelerationPartial( + innerAccelerationIterator->second[ j ], + std::make_pair( acceleratedBody, acceleratedBodyObject ), + std::make_pair( acceleratingBody, acceleratingBodyObject ), + bodyMap, parametersToEstimate ); + + accelerationPartialVector.push_back( currentAccelerationPartial ); + accelerationPartialsMap[ acceleratedBody ][ acceleratingBody ].push_back( + currentAccelerationPartial ); + } + } + + // Add partials of current body's accelerations to vector. + accelerationPartialsList[ i ] = accelerationPartialVector; + + } + } + } + } + return accelerationPartialsList; +} + +} // namespace simulation_setup + +} // namespace tudat + +#endif // TUDAT_CREATEACCELERATIONPARTIALS_H diff --git a/Tudat/SimulationSetup/EstimationSetup/createEstimatableParameters.cpp b/Tudat/SimulationSetup/EstimationSetup/createEstimatableParameters.cpp new file mode 100644 index 0000000000..d63629fc1a --- /dev/null +++ b/Tudat/SimulationSetup/EstimationSetup/createEstimatableParameters.cpp @@ -0,0 +1,306 @@ +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/constantRotationRate.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/constantRotationalOrientation.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/gravitationalParameter.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicCosineCoefficients.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicSineCoefficients.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/radiationPressureCoefficient.h" +#include "Tudat/SimulationSetup/EstimationSetup/createEstimatableParameters.h" + +namespace tudat +{ + +namespace simulation_setup +{ + + +using namespace simulation_setup; +using namespace ephemerides; +using namespace gravitation; +using namespace estimatable_parameters; + +//! Function to create an interface object for estimating a parameter defined by a single double value +boost::shared_ptr< EstimatableParameter< double > > createDoubleParameterToEstimate( + const boost::shared_ptr< EstimatableParameterSettings >& doubleParameterName, + const NamedBodyMap& bodyMap, const basic_astrodynamics::AccelerationMap& accelerationModelMap ) +{ + boost::shared_ptr< EstimatableParameter< double > > doubleParameterToEstimate; + + // Check input consistency. + if( isDoubleParameter( doubleParameterName->parameterType_.first ) != true ) + { + std::string errorMessage = "Error when requesting to make double parameter " + + boost::lexical_cast< std::string >( doubleParameterName->parameterType_.first ) + " of " + + boost::lexical_cast< std::string >( doubleParameterName->parameterType_.second.first ) + + ", parameter is not a double parameter "; + throw std::runtime_error( errorMessage ); + } + else + { + // Check if body associated with parameter exists. + std::string currentBodyName = doubleParameterName->parameterType_.second.first; + boost::shared_ptr< Body > currentBody; + if( ( currentBodyName != "" ) && ( bodyMap.count( currentBodyName ) == 0 ) ) + { + std::string errorMessage = "Error when creating parameters to estimate, body " + + boost::lexical_cast< std::string >( currentBodyName ) + + " not in body map " + + boost::lexical_cast< std::string >( doubleParameterName->parameterType_.first ); + throw std::runtime_error( errorMessage ); + } + else if( currentBodyName != "" ) + { + currentBody = bodyMap.at( currentBodyName ); + } + + // Identify parameter type. + switch( doubleParameterName->parameterType_.first ) + { + case gravitational_parameter: + { + if( currentBody->getGravityFieldModel( )== NULL ) + { + std::string errorMessage = "Error, body " + + boost::lexical_cast< std::string >( currentBodyName ) + + " has no gravity field, cannot estimate gravitational parameter."; + throw std::runtime_error( errorMessage ); + } + else + { + boost::shared_ptr< GravityFieldModel > gravityFieldModel = currentBody->getGravityFieldModel( ); + doubleParameterToEstimate = boost::make_shared< GravitationalParameter > + ( gravityFieldModel, currentBodyName ); + } + break; + } + case radiation_pressure_coefficient: + { + if( currentBody->getRadiationPressureInterfaces( ).size( ) == 0 ) + { + std::string errorMessage = "Error, no radiation pressure interfaces found in body " + + boost::lexical_cast< std::string >( currentBodyName) + + " when making Cr parameter."; + throw std::runtime_error( errorMessage ); + } + else if( currentBody->getRadiationPressureInterfaces( ).size( ) > 1 ) + { + std::string errorMessage = "Error, multiple radiation pressure interfaces found in body " + + boost::lexical_cast< std::string >( currentBodyName) + + " when making Cr parameter."; + throw std::runtime_error( errorMessage ); + } + else + { + doubleParameterToEstimate = boost::make_shared< RadiationPressureCoefficient >( + currentBody->getRadiationPressureInterfaces( ).begin( )->second, + currentBodyName ); + } + break; + } + case constant_rotation_rate: + { + if( boost::dynamic_pointer_cast< SimpleRotationalEphemeris >( currentBody->getRotationalEphemeris( ) ) == NULL ) + { + std::string errorMessage = "Warning, no simple rotational ephemeris present in body " + currentBodyName + + " when making constant rotation rate parameter"; + throw std::runtime_error( errorMessage ); + } + else + { + doubleParameterToEstimate = boost::make_shared< RotationRate >( + boost::dynamic_pointer_cast< ephemerides::SimpleRotationalEphemeris > + ( currentBody->getRotationalEphemeris( ) ), currentBodyName ); + } + break; + } + default: + throw std::runtime_error( "Warning, this double parameter has not yet been implemented when making parameters" ); + break; + } + } + + return doubleParameterToEstimate; +} + +boost::shared_ptr< EstimatableParameter< Eigen::VectorXd > > createVectorParameterToEstimate( + const boost::shared_ptr< EstimatableParameterSettings >& vectorParameterName, + const NamedBodyMap& bodyMap, const basic_astrodynamics::AccelerationMap& accelerationModelMap ) +{ + boost::shared_ptr< EstimatableParameter< Eigen::VectorXd > > vectorParameterToEstimate; + + // Check input consistency. + if( isDoubleParameter( vectorParameterName->parameterType_.first ) != false ) + { + std::string errorMessage = "Error when requesting to make vector parameter " + + boost::lexical_cast< std::string >( vectorParameterName->parameterType_.first ) + + " of " + boost::lexical_cast< std::string >( vectorParameterName->parameterType_.second.first ) + + ", parameter is not a vector parameter "; + throw std::runtime_error( errorMessage ); + } + else + { + // Check if body associated with parameter exists. + std::string currentBodyName = vectorParameterName->parameterType_.second.first; + boost::shared_ptr< Body > currentBody; + if( ( currentBodyName != "" ) && ( bodyMap.count( currentBodyName ) == 0 ) ) + { + std::string errorMessage = "Warning when creating parameters to estimate, body " + + boost::lexical_cast< std::string >( currentBodyName ) + + "not in body map " + + boost::lexical_cast< std::string >( vectorParameterName->parameterType_.first ); + throw std::runtime_error( errorMessage ); + } + else if( ( currentBodyName != "" ) ) + { + currentBody = bodyMap.at( currentBodyName ); + } + + // Identify parameter type. + switch( vectorParameterName->parameterType_.first ) + { + case rotation_pole_position: + if( boost::dynamic_pointer_cast< SimpleRotationalEphemeris >( currentBody->getRotationalEphemeris( ) ) == NULL ) + { + std::string errorMessage = "Warning, no simple rotational ephemeris present in body " + currentBodyName + + " when making constant rotation orientation parameter"; + throw std::runtime_error( errorMessage ); + } + else + { + vectorParameterToEstimate = boost::make_shared< ConstantRotationalOrientation > + ( boost::dynamic_pointer_cast< ephemerides::SimpleRotationalEphemeris > + ( currentBody->getRotationalEphemeris( ) ), currentBodyName ); + + } + break; + case spherical_harmonics_cosine_coefficient_block: + { + boost::shared_ptr< GravityFieldModel > gravityField = currentBody->getGravityFieldModel( ); + boost::shared_ptr< SphericalHarmonicsGravityField > shGravityField = + boost::dynamic_pointer_cast< SphericalHarmonicsGravityField >( gravityField ); + if( shGravityField == NULL ) + { + std::string errorMessage = "Error, requested spherical harmonic cosine coefficient block parameter of " + + boost::lexical_cast< std::string >( vectorParameterName->parameterType_.second.first ) + + ", but body does not have a spherical harmonic gravity field."; + throw std::runtime_error( errorMessage ); + } + else + { + // Check if spherical harmonic gravity field is static or time-dependent; set associated + // functions accordingly + boost::shared_ptr< TimeDependentSphericalHarmonicsGravityField > timeDependentShField = + boost::dynamic_pointer_cast< TimeDependentSphericalHarmonicsGravityField >( shGravityField ); + + boost::function< Eigen::MatrixXd( ) > getCosineCoefficientsFunction; + boost::function< void( Eigen::MatrixXd ) > setCosineCoefficientsFunction; + + if( timeDependentShField == NULL ) + { + getCosineCoefficientsFunction = boost::bind( &SphericalHarmonicsGravityField::getCosineCoefficients, + shGravityField ); + setCosineCoefficientsFunction = boost::bind( &SphericalHarmonicsGravityField::setCosineCoefficients, + shGravityField, _1 ); + } + else + { + getCosineCoefficientsFunction = boost::bind( + &TimeDependentSphericalHarmonicsGravityField::getNominalCosineCoefficients, + timeDependentShField ); + setCosineCoefficientsFunction = boost::bind( + &TimeDependentSphericalHarmonicsGravityField::setNominalCosineCoefficients, + timeDependentShField, _1 ); + } + + // Create cosine coefficients estimation object. + boost::shared_ptr< SphericalHarmonicEstimatableParameterSettings > blockParameterSettings = + boost::dynamic_pointer_cast< SphericalHarmonicEstimatableParameterSettings >( vectorParameterName ); + if( blockParameterSettings != NULL ) + { + vectorParameterToEstimate = boost::make_shared< SphericalHarmonicsCosineCoefficients >( + getCosineCoefficientsFunction, + setCosineCoefficientsFunction, + blockParameterSettings->blockIndices_, + vectorParameterName->parameterType_.second.first ); + } + else + { + throw std::runtime_error( "Error, expected SphericalHarmonicEstimatableParameterSettings for cosine coefficients" ); + } + } + break; + } + case spherical_harmonics_sine_coefficient_block: + { + boost::shared_ptr< GravityFieldModel > gravityField = currentBody->getGravityFieldModel( ); + boost::shared_ptr< SphericalHarmonicsGravityField > shGravityField = + boost::dynamic_pointer_cast< SphericalHarmonicsGravityField >( gravityField ); + if( shGravityField == NULL ) + { + std::string errorMessage = "Error, requested spherical harmonic sine coefficient block parameter of " + + boost::lexical_cast< std::string >( vectorParameterName->parameterType_.second.first ) + + ", but body does not have a spherical harmonic gravity field."; + throw std::runtime_error( errorMessage ); + + } + else + { + boost::shared_ptr< SphericalHarmonicEstimatableParameterSettings > blockParameterSettings = + boost::dynamic_pointer_cast< SphericalHarmonicEstimatableParameterSettings >( vectorParameterName ); + + // Check if spherical harmonic gravity field is static or time-dependent; set associated + // functions accordingly + boost::function< Eigen::MatrixXd( ) > getSineCoefficientsFunction; + boost::function< void( Eigen::MatrixXd ) > setSineCoefficientsFunction; + boost::shared_ptr< TimeDependentSphericalHarmonicsGravityField > timeDependentShField = + boost::dynamic_pointer_cast< TimeDependentSphericalHarmonicsGravityField >( shGravityField ); + + if( timeDependentShField == NULL ) + { + getSineCoefficientsFunction = boost::bind( &SphericalHarmonicsGravityField::getSineCoefficients, + shGravityField ); + setSineCoefficientsFunction = boost::bind( &SphericalHarmonicsGravityField::setSineCoefficients, + shGravityField, _1 ); + } + else + { + getSineCoefficientsFunction = boost::bind( + &TimeDependentSphericalHarmonicsGravityField::getNominalSineCoefficients, + timeDependentShField ); + setSineCoefficientsFunction = boost::bind( + &TimeDependentSphericalHarmonicsGravityField::setNominalSineCoefficients, + timeDependentShField, _1 ); + } + + // Create sine coefficients estimation object. + if( blockParameterSettings != NULL ) + { + vectorParameterToEstimate = boost::make_shared< SphericalHarmonicsSineCoefficients >( + getSineCoefficientsFunction, + setSineCoefficientsFunction, + blockParameterSettings->blockIndices_, + vectorParameterName->parameterType_.second.first ); + } + else + { + throw std::runtime_error( "Error, expected SphericalHarmonicEstimatableParameterSettings for sine coefficients" ); + } + } + + break; + } + default: + std::string errorMessage = "Warning, this vector parameter (" + + boost::lexical_cast< std::string >( vectorParameterName->parameterType_.first ) + + ") has not yet been implemented when making parameters"; + throw std::runtime_error( errorMessage ); + + break; + } + } + + return vectorParameterToEstimate; +} + +} + +} diff --git a/Tudat/SimulationSetup/EstimationSetup/createEstimatableParameters.h b/Tudat/SimulationSetup/EstimationSetup/createEstimatableParameters.h new file mode 100644 index 0000000000..aaadbacbaa --- /dev/null +++ b/Tudat/SimulationSetup/EstimationSetup/createEstimatableParameters.h @@ -0,0 +1,210 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_CREATEESTIMATABLEPARAMETERS_H +#define TUDAT_CREATEESTIMATABLEPARAMETERS_H + +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" + +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/initialTranslationalState.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h" +#include "Tudat/SimulationSetup/EstimationSetup/estimatableParameterSettings.h" +#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" + +namespace tudat +{ + +namespace simulation_setup +{ + +//! Function to create interface object for estimating parameters representing an initial dynamical state. +/*! + * Function to create interface object for estimating parameters representing an initial dynamical state. + * \param bodyMap Map of body objects containing the fll simulation environment. + * \param parameterSettings Object defining the parameter interface that is to be created. + * \return Interface object for estimating an initial state. + */ +template< typename InitialStateParameterType = double > +boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::Matrix +< InitialStateParameterType, Eigen::Dynamic, 1 > > > createInitialDynamicalStateParameterToEstimate( + const NamedBodyMap& bodyMap, + const boost::shared_ptr< estimatable_parameters::EstimatableParameterSettings >& parameterSettings ) +{ + using namespace tudat::estimatable_parameters; + + boost::shared_ptr< EstimatableParameter< Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 > > > + initialStateParameterToEstimate; + + // Check consistency of input. + if( !isParameterDynamicalPropertyInitialState( parameterSettings->parameterType_.first ) ) + { + std::string errorMessage = "Error when requesting to make initial state parameter " + + boost::lexical_cast< std::string >( parameterSettings->parameterType_.first ) + " of " + + boost::lexical_cast< std::string >( parameterSettings->parameterType_.second.first ) + + ", parameter is not an initial state parameter "; + throw std::runtime_error( errorMessage ); + } + else + { + // Identify state that is to be estimation + switch( parameterSettings->parameterType_.first ) + { + case initial_body_state: + + // Check consistency of input. + if( boost::dynamic_pointer_cast< + InitialTranslationalStateEstimatableParameterSettings< InitialStateParameterType > >( + parameterSettings ) == NULL ) + { + throw std::runtime_error( "Error when making body initial state parameter, settings type is incompatible" ); + } + else + { + boost::shared_ptr< InitialTranslationalStateEstimatableParameterSettings< InitialStateParameterType > > + initialStateSettings = + boost::dynamic_pointer_cast< + InitialTranslationalStateEstimatableParameterSettings< InitialStateParameterType > >( + parameterSettings ); + + Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 > initialTranslationalState; + + // If initial time is not defined, use preset initial state + if( ! ( initialStateSettings->initialTime_ == initialStateSettings->initialTime_ ) ) + { + initialTranslationalState = initialStateSettings->initialStateValue_; + + + } + // Compute initial state from environment + else + { + initialTranslationalState = propagators::getInitialStateOfBody + < double, InitialStateParameterType >( + initialStateSettings->parameterType_.second.first, initialStateSettings->centralBody_, + bodyMap, initialStateSettings->initialTime_ ); + + } + + // Create translational state estimation interface object + initialStateParameterToEstimate = + boost::make_shared< InitialTranslationalStateParameter< InitialStateParameterType > >( + initialStateSettings->parameterType_.second.first, initialTranslationalState, + initialStateSettings->centralBody_, + initialStateSettings->frameOrientation_ ); + } + break; + default: + std::string errorMessage = "Error, could not create parameter for initial state of type " + + boost::lexical_cast< std::string >( parameterSettings->parameterType_.first ); + throw std::runtime_error( errorMessage ); + } + } + + return initialStateParameterToEstimate; +} + +//! Function to create an interface object for estimating a parameter defined by a single double value +/*! + * Function to create an interface object for estimating a parameter defined by a single double value + * \param doubleParameterName Object defining the parameter interface that is to be created. + * \param bodyMap Map of body objects containing the fll simulation environment. + * \param accelerationModelMap List of acceleration models used in simulations; empty by default (only required for + * selected parameters). + * \return Interface object for estimating parameter. + */ +boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > createDoubleParameterToEstimate( + const boost::shared_ptr< estimatable_parameters::EstimatableParameterSettings >& doubleParameterName, + const NamedBodyMap& bodyMap, const basic_astrodynamics::AccelerationMap& accelerationModelMap = + basic_astrodynamics::AccelerationMap( ) ); + +//! Function to create an interface object for estimating a parameter defined by a list of double values +/*! + * Function to create an interface object for estimating a parameter defined by a list of single double values + * \param vectorParameterName Object defining the parameter interface that is to be created. + * \param bodyMap Map of body objects containing the fll simulation environment. + * \param accelerationModelMap List of acceleration models used in simulations; empty by default (only required for + * selected parameters). + * \return Interface object for estimating parameter. + */ +boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > createVectorParameterToEstimate( + const boost::shared_ptr< estimatable_parameters::EstimatableParameterSettings >& vectorParameterName, + const NamedBodyMap& bodyMap, const basic_astrodynamics::AccelerationMap& accelerationModelMap = + basic_astrodynamics::AccelerationMap( ) ); + +//! Function to create the interface object for estimating any number/type of parameters. +/*! + * Function to create the interface object for estimating any number/type of parameters. This can include both + * environmental parameters and initial dynamical states. The types of parameters are defined by the parameterNames m + * input variables + * \param parameterNames List of objects defining the parameters that are to be estimated. + * \param bodyMap Map of body objects containing the fll simulation environment. + * \param accelerationModelMap List of acceleration models used in simulations; empty by default (only required for + * selected parameters). + * \return Interface object for estimating a set of parameters. + */ +template< typename InitialStateParameterType = double > +boost::shared_ptr< estimatable_parameters::EstimatableParameterSet< InitialStateParameterType > > createParametersToEstimate( + const std::vector< boost::shared_ptr< estimatable_parameters::EstimatableParameterSettings > >& parameterNames, + const NamedBodyMap& bodyMap, + const basic_astrodynamics::AccelerationMap& accelerationModelMap = basic_astrodynamics::AccelerationMap( ) ) + +{ + using namespace tudat::estimatable_parameters; + + std::vector< boost::shared_ptr< EstimatableParameter< Eigen::Matrix< InitialStateParameterType, Eigen::Dynamic, 1 > > > > + initialDynamicalParametersToEstimate; + std::vector< boost::shared_ptr< EstimatableParameter< double > > > doubleParametersToEstimate; + std::vector< boost::shared_ptr< EstimatableParameter< Eigen::VectorXd > > > vectorParametersToEstimate; + + // Iterate over all parameters. + for( unsigned int i = 0; i < parameterNames.size( ); i++ ) + { + // Create initial dynamical parameters. + if( isParameterDynamicalPropertyInitialState( parameterNames.at( i )->parameterType_.first ) ) + { + initialDynamicalParametersToEstimate.push_back( + createInitialDynamicalStateParameterToEstimate< InitialStateParameterType >( + bodyMap, parameterNames.at( i ) ) ); + } + // Create parameters defined by single double value + else if( isDoubleParameter( parameterNames[ i ]->parameterType_.first ) == true ) + { + doubleParametersToEstimate.push_back( createDoubleParameterToEstimate( + parameterNames[ i ], bodyMap, accelerationModelMap ) ); + } + // Create parameters defined by list of double values + else if( isDoubleParameter( parameterNames[ i ]->parameterType_.first ) == false ) + { + vectorParametersToEstimate.push_back( createVectorParameterToEstimate( + parameterNames[ i ], bodyMap, accelerationModelMap ) ); + } + else + { + std::string errorMessage = "Error, parameter type of " + + boost::lexical_cast< std::string >( parameterNames[ i ]->parameterType_.second.first ) + "of " + + boost::lexical_cast< std::string >( parameterNames[ i ]->parameterType_.first ) + + "not recognized when making estimatable parameter set."; + + throw std::runtime_error( errorMessage ); + } + } + + return boost::make_shared< EstimatableParameterSet< InitialStateParameterType > >( + doubleParametersToEstimate, vectorParametersToEstimate, initialDynamicalParametersToEstimate ); +} + + +} // namespace simulation_setup + +} // namespace tudat + +#endif // TUDAT_CREATEESTIMATABLEPARAMETERS_H diff --git a/Tudat/SimulationSetup/EstimationSetup/createLightTimeCalculator.cpp b/Tudat/SimulationSetup/EstimationSetup/createLightTimeCalculator.cpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tudat/SimulationSetup/EstimationSetup/createLightTimeCalculator.h b/Tudat/SimulationSetup/EstimationSetup/createLightTimeCalculator.h new file mode 100644 index 0000000000..9d79d79271 --- /dev/null +++ b/Tudat/SimulationSetup/EstimationSetup/createLightTimeCalculator.h @@ -0,0 +1,120 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_CREATELIGHTTIMECALCULATOR_H +#define TUDAT_CREATELIGHTTIMECALCULATOR_H + +#include "Tudat/Astrodynamics/ObservationModels/lightTimeSolution.h" +#include "Tudat/Astrodynamics/ObservationModels/linkTypeDefs.h" +#include "Tudat/SimulationSetup/EstimationSetup/createLightTimeCorrection.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" + +namespace tudat +{ +namespace observation_models +{ + +//! Function to create a state function of a link end, expressed in base frame. +/*! + * Function to create a state function of a link end, expressed in base frame. + * \param linkEndId Name of the reference point for which state function is to be created. + * \param bodyMap List of body objects that comprises the environment + * \return Requested state function. + */ +template< typename TimeType = double, typename ScalarStateType = double > +boost::function< Eigen::Matrix< ScalarStateType, 6, 1 >( const TimeType ) > getLinkEndCompleteEphemerisFunction( + const std::pair< std::string, std::string > linkEndId, const simulation_setup::NamedBodyMap& bodyMap ) +{ + if( bodyMap.count( linkEndId.first ) == 0 ) + { + std::cerr<<"Error when making ephemeris function for "<, + bodyMap.at( linkEndId.first ), _1 ); +} + +//! Function to create a light-time calculation object +/*! + * Function to create a light-time calculation object from light time correction settings environment and link end + * state functions. + * \param transmitterCompleteEphemeris Function returning the transmitter Cartesian state as a function of time. + * \param receiverCompleteEphemeris Function returning the receiver Cartesian state as a function of time. + * \param bodyMap List of body objects that comprises the environment + * \param lightTimeCorrections List of light time corrections (w.r.t. Euclidean distance) that are applied when computing + * light time. + * \param transmittingLinkEnd Identifier for transmitting link end. + * \param receivingLinkEnd Identifier for receiving link end. + */ +template< typename ObservationScalarType = double, typename TimeType = double, + typename StateScalarType = ObservationScalarType > +boost::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType, StateScalarType > > +createLightTimeCalculator( + const boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) >& transmitterCompleteEphemeris, + const boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) >& receiverCompleteEphemeris, + const simulation_setup::NamedBodyMap& bodyMap, + const std::vector< boost::shared_ptr< LightTimeCorrectionSettings > >& lightTimeCorrections, + const LinkEndId& transmittingLinkEnd, + const LinkEndId& receivingLinkEnd ) +{ + std::vector< boost::shared_ptr< LightTimeCorrection > > lightTimeCorrectionFunctions; + + // Create lighttime correction functions from lightTimeCorrections + for( unsigned int i = 0; i < lightTimeCorrections.size( ); i++ ) + { + + lightTimeCorrectionFunctions.push_back( + createLightTimeCorrections( + lightTimeCorrections[ i ], bodyMap, transmittingLinkEnd, receivingLinkEnd ) ); + } + + // Create light time calculator. + return boost::make_shared< LightTimeCalculator< ObservationScalarType, TimeType, StateScalarType > > + ( transmitterCompleteEphemeris, receiverCompleteEphemeris, lightTimeCorrectionFunctions ); +} + +//! Function to create a light-time calculation object +/*! + * Function to create a light-time calculation object from light time correction settings environment and link end + * identifiers. + * \param transmittingLinkEnd Identifier for transmitting link end. + * \param receivingLinkEnd Identifier for receiving link end. + * \param bodyMap List of body objects that comprises the environment + * \param lightTimeCorrections List of light time corrections (w.r.t. Euclidean distance) that are applied when computing + * light time. + */ +template< typename ObservationScalarType = double, typename TimeType = double, + typename StateScalarType = ObservationScalarType > +boost::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType, StateScalarType > > +createLightTimeCalculator( + const LinkEndId& transmittingLinkEnd, + const LinkEndId& receivingLinkEnd, + const simulation_setup::NamedBodyMap& bodyMap, + const std::vector< boost::shared_ptr< LightTimeCorrectionSettings > >& lightTimeCorrections ) +{ + + // Get link end state functions and create light time calculator. + return createLightTimeCalculator< ObservationScalarType, TimeType, StateScalarType >( + getLinkEndCompleteEphemerisFunction< StateScalarType, TimeType >( + transmittingLinkEnd, bodyMap ), + getLinkEndCompleteEphemerisFunction< StateScalarType, TimeType >( + receivingLinkEnd, bodyMap ), + bodyMap, lightTimeCorrections, transmittingLinkEnd, receivingLinkEnd ); +} + +} // namespace observation_models + +} // namespace tudat +#endif // TUDAT_CREATELIGHTTIMECALCULATOR_H diff --git a/Tudat/SimulationSetup/EstimationSetup/createLightTimeCorrection.cpp b/Tudat/SimulationSetup/EstimationSetup/createLightTimeCorrection.cpp new file mode 100644 index 0000000000..7ab1a23c58 --- /dev/null +++ b/Tudat/SimulationSetup/EstimationSetup/createLightTimeCorrection.cpp @@ -0,0 +1,103 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" +#include "Tudat/SimulationSetup/EstimationSetup/createLightTimeCorrection.h" +#include "Tudat/Astrodynamics/ObservationModels/ObservableCorrections/firstOrderRelativisticLightTimeCorrection.h" + +namespace tudat +{ + +namespace observation_models +{ + +//! Function to create object that computes a single (type of) correction to the light-time +boost::shared_ptr< LightTimeCorrection > createLightTimeCorrections( + const boost::shared_ptr< LightTimeCorrectionSettings > correctionSettings, + const simulation_setup::NamedBodyMap& bodyMap, + const std::pair< std::string, std::string >& transmitter, + const std::pair< std::string, std::string >& receiver ) +{ + + using namespace tudat::ephemerides; + using namespace tudat::gravitation; + + boost::shared_ptr< LightTimeCorrection > lightTimeCorrection; + + // Identify type of light time correction to be created. + switch( correctionSettings->getCorrectionType( ) ) + { + case first_order_relativistic: + { + // Check input consistency + if( boost::dynamic_pointer_cast< FirstOrderRelativisticLightTimeCorrectionSettings >( correctionSettings ) != NULL ) + { + // Retrieve list of bodies causing light time perturbation + std::vector< std::string > perturbingBodies = + boost::dynamic_pointer_cast< FirstOrderRelativisticLightTimeCorrectionSettings >( correctionSettings )-> + getPerturbingBodies( ); + + std::vector< boost::function< basic_mathematics::Vector6d( const double ) > > perturbingBodyStateFunctions; + std::vector< boost::function< double( ) > > perturbingBodyGravitationalParameterFunctions; + + // Retrieve mass and state functions for each perturbing body. + for( unsigned int i = 0; i < perturbingBodies.size( ); i++ ) + { + if( bodyMap.count( perturbingBodies[ i ] ) == 0 ) + { + throw std::runtime_error( + "Error when making 1st order relativistic light time correction, could not find body " + + perturbingBodies.at( i ) ); + } + else + { + // Set state function. + perturbingBodyStateFunctions.push_back( + boost::bind( &simulation_setup::Body::getStateInBaseFrameFromEphemeris, + bodyMap.at( perturbingBodies[ i ] ), _1 ) ); + + // Set gravitational parameter function. + perturbingBodyGravitationalParameterFunctions.push_back( + boost::bind( &gravitation::GravityFieldModel::getGravitationalParameter, + bodyMap.at( perturbingBodies[ i ] )-> + getGravityFieldModel( ) ) ); + } + } + + // Create light-time correction function + lightTimeCorrection = boost::make_shared< FirstOrderLightTimeCorrectionCalculator >( + perturbingBodyStateFunctions, perturbingBodyGravitationalParameterFunctions, perturbingBodies, + transmitter.first, receiver.first ); + + } + else + { + throw std::runtime_error( + "Error, correction settings type (1st order relativistic) does not coincide with data type." ); + } + + break; + } + default: + { + std::string errorMessage = "Error, light time correction type " + + boost::lexical_cast< std::string >( correctionSettings->getCorrectionType( ) ) + " not recognized."; + throw std::runtime_error( errorMessage ); + + break; + } + + } + return lightTimeCorrection; +} + +} + +} diff --git a/Tudat/SimulationSetup/EstimationSetup/createLightTimeCorrection.h b/Tudat/SimulationSetup/EstimationSetup/createLightTimeCorrection.h new file mode 100644 index 0000000000..ab4c6bc264 --- /dev/null +++ b/Tudat/SimulationSetup/EstimationSetup/createLightTimeCorrection.h @@ -0,0 +1,122 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_CREATELIGHTTIMECORRECTION_H +#define TUDAT_CREATELIGHTTIMECORRECTION_H + +#include + +#include +#include + +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" +#include "Tudat/Astrodynamics/ObservationModels/linkTypeDefs.h" +#include "Tudat/Astrodynamics/ObservationModels/ObservableCorrections/lightTimeCorrection.h" + +namespace tudat +{ + +namespace observation_models +{ + +//! Typedef for function calculating light-time correction in light-time calculation loop. +typedef boost::function< double( + const basic_mathematics::Vector6d&, const basic_mathematics::Vector6d&, + const double, const double ) > LightTimeCorrectionFunction; + +//! Base class for light-time correction settings. +/*! + * Base class for light-time correction settings. This class is not used for calculations of corrections, + * but is used for the purpose of defining the light time correction properties. + * The createLightTimeCorrections function produces the classes that calculate the actual corrections, based on settings + * in instances of derived LightTimeCorrectionSettings classes. + */ +class LightTimeCorrectionSettings +{ +public: + + //! Constructor, takes light-time correction type. + /*! + * \param correctionType Type of light-time correction that is to be created + */ + LightTimeCorrectionSettings( const LightTimeCorrectionType correctionType ): + correctionType_( correctionType ){ } + + //! Default destructor. + virtual ~LightTimeCorrectionSettings( ){ } + + //! Function returning the type of light-time correction that is to be created + /*! + * Function returning the type of light-time correction that is to be created + * \return Type of light-time correction that is to be created + */ + LightTimeCorrectionType getCorrectionType( ) + { + return correctionType_; + } + +protected: + + //! Type of light-time correction that is to be created + LightTimeCorrectionType correctionType_; +}; + +//! Class to defining settings for first-order relativistic light time correction (Shapiro time delay) due to a +//! set of point masses +class FirstOrderRelativisticLightTimeCorrectionSettings: public LightTimeCorrectionSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param perturbingBodies List of bodies for which the point masses are used to compute the light-time correction. + */ + FirstOrderRelativisticLightTimeCorrectionSettings( const std::vector< std::string >& perturbingBodies ): + LightTimeCorrectionSettings( first_order_relativistic ), perturbingBodies_( perturbingBodies ){ } + + //! Destructor + ~FirstOrderRelativisticLightTimeCorrectionSettings( ){ } + + //! Function returning the list of bodies for which the point masses are used to compute the light-time correction. + /*! + * Function returning the list of bodies for which the point masses are used to compute the light-time correction. + * \return List of bodies for which the point masses are used to compute the light-time correction. + */ + std::vector< std::string > getPerturbingBodies( ){ return perturbingBodies_; } + +private: + + //! List of bodies for which the point masses are used to compute the light-time correction. + std::vector< std::string > perturbingBodies_; + +}; + +//! Function to create object that computes a single (type of) correction to the light-time +/*! + * Function to create object that computes a single (type of) correction to the light-time + * \param correctionSettings User-defined settings for the light-time correction that is to be created + * \param bodyMap List of body objects that constitutes the environment + * \param transmitter Id of transmitting body/reference point (first/second) + * \param receiver Id of receiving body/reference point (first/second) + * \return Object for computing required light-time correction + */ +boost::shared_ptr< LightTimeCorrection > createLightTimeCorrections( + const boost::shared_ptr< LightTimeCorrectionSettings > correctionSettings, + const simulation_setup::NamedBodyMap& bodyMap, + const std::pair< std::string, std::string >& transmitter, + const std::pair< std::string, std::string >& receiver ); + +} // namespace observation_models + +} // namespace tudat + + +#endif // TUDAT_CREATELIGHTTIMECORRECTION_H diff --git a/Tudat/SimulationSetup/EstimationSetup/createObservationModel.h b/Tudat/SimulationSetup/EstimationSetup/createObservationModel.h new file mode 100644 index 0000000000..bf0127fd6f --- /dev/null +++ b/Tudat/SimulationSetup/EstimationSetup/createObservationModel.h @@ -0,0 +1,290 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_CREATEOBSERVATIONMODEL_H +#define TUDAT_CREATEOBSERVATIONMODEL_H + +#include + +#include +#include + + +#include "Tudat/Astrodynamics/ObservationModels/observationModel.h" +#include "Tudat/Astrodynamics/ObservationModels/linkTypeDefs.h" +#include "Tudat/SimulationSetup/EstimationSetup/createLightTimeCorrection.h" +#include "Tudat/Astrodynamics/ObservationModels/oneWayRangeObservationModel.h" +#include "Tudat/Astrodynamics/ObservationModels/angularPositionObservationModel.h" +#include "Tudat/Astrodynamics/ObservationModels/positionObservationModel.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" +#include "Tudat/SimulationSetup/EstimationSetup/createLightTimeCalculator.h" + + +namespace tudat +{ + +namespace observation_models +{ + +//! Interface class for creating observation models +/*! + * Interface class for creating observation models. This class is used instead of a single templated free function to + * allow ObservationModel deroved classed with different ObservationSize template arguments to be created using the same + * interface. This class has template specializations for each value of ObservationSize, and contains a single + * createObservationModel function that performs the required operation. + */ +template< int ObservationSize = 1, typename ObservationScalarType = double, + typename TimeType = double, typename StateScalarType = ObservationScalarType > +class ObservationModelCreator +{ +public: + + //! Function to create an observation model. + /*! + * Function to create an observation model. + * \param observableType Type of observation model that is to be created. + * \param linkEnds Link ends for observation model that is to be created + * \param bodyMap List of body objects that comprises the environment + * \param singleObservableCorrections List of light time corrections that are used when computing the observable. + * \param observationBiasCalculator + * \return Observation model of required settings. + */ + static boost::shared_ptr< observation_models::ObservationModel< + ObservationSize, ObservationScalarType, TimeType, StateScalarType > > createObservationModel( + const ObservableType observableType, + const LinkEnds& linkEnds, + const simulation_setup::NamedBodyMap &bodyMap, + const std::vector< boost::shared_ptr< LightTimeCorrectionSettings > >& singleObservableCorrections = + std::vector< boost::shared_ptr< LightTimeCorrectionSettings > >( ), + const boost::shared_ptr< ObservationBias< ObservationSize > > observationBiasCalculator = NULL ); +}; + +//! Interface class for creating observation models of size 1. +template< typename ObservationScalarType, typename TimeType, typename StateScalarType > +class ObservationModelCreator< 1, ObservationScalarType, TimeType, StateScalarType > +{ +public: + + //! Function to create an observation model of size 1. + /*! + * Function to create an observation model of size 1. + * \param observableType Type of observation model that is to be created. + * \param linkEnds Link ends for observation model that is to be created + * \param bodyMap List of body objects that comprises the environment + * \param singleObservableCorrections List of light time corrections that are used when computing the observable. + * \param observationBiasCalculator + * \return Observation model of required settings. + */ + static boost::shared_ptr< observation_models::ObservationModel< + 1, ObservationScalarType, TimeType, StateScalarType > > createObservationModel( + const ObservableType observableType, + const LinkEnds& linkEnds, + const simulation_setup::NamedBodyMap &bodyMap, + const std::vector< boost::shared_ptr< LightTimeCorrectionSettings > >& singleObservableCorrections = + std::vector< boost::shared_ptr< LightTimeCorrectionSettings > >( ), + const boost::shared_ptr< ObservationBias< 1 > > observationBiasCalculator = NULL ) + { + using namespace observation_models; + + boost::shared_ptr< observation_models::ObservationModel< + 1, ObservationScalarType, TimeType, StateScalarType > > observationModel; + + // Check type of observation model. + switch( observableType ) + { + case oneWayRange: + { + // Check consistency input. + if( linkEnds.size( ) != 2 ) + { + std::string errorMessage = + "Error when making 1 way range model, " + + boost::lexical_cast< std::string >( linkEnds.size( ) ) + " link ends found"; + throw std::runtime_error( errorMessage ); + } + if( linkEnds.count( receiver ) == 0 ) + { + throw std::runtime_error( "Error when making 1 way range model, no receiver found" ); + } + if( linkEnds.count( transmitter ) == 0 ) + { + throw std::runtime_error( "Error when making 1 way range model, no transmitter found" ); + } + + // Create observation model + observationModel = boost::make_shared< OneWayRangeObservationModel< + ObservationScalarType, TimeType, StateScalarType > >( + createLightTimeCalculator< ObservationScalarType, TimeType, StateScalarType >( + linkEnds.at( transmitter ), linkEnds.at( receiver ), + bodyMap, singleObservableCorrections ), observationBiasCalculator ); + + break; + } + default: + std::string errorMessage = "Error, observable " + boost::lexical_cast< std::string >( observableType ) + + " not recognized when making size 1 observation model."; + throw std::runtime_error( errorMessage ); + } + return observationModel; + } +}; + +//! Interface class for creating observation models of size 2. +template< typename ObservationScalarType, typename TimeType, typename StateScalarType > +class ObservationModelCreator< 2, ObservationScalarType, TimeType, StateScalarType > +{ +public: + + //! Function to create an observation model of size 2. + /*! + * Function to create an observation model of size 2. + * \param observableType Type of observation model that is to be created. + * \param linkEnds Link ends for observation model that is to be created + * \param bodyMap List of body objects that comprises the environment + * \param singleObservableCorrections List of light time corrections that are used when computing the observable. + * \param observationBiasCalculator + * \return Observation model of required settings. + */ + static boost::shared_ptr< observation_models::ObservationModel< + 2, ObservationScalarType, TimeType, StateScalarType > > createObservationModel( + const ObservableType observableType, + const LinkEnds& linkEnds, + const simulation_setup::NamedBodyMap &bodyMap, + const std::vector< boost::shared_ptr< LightTimeCorrectionSettings > >& singleObservableCorrections = + std::vector< boost::shared_ptr< LightTimeCorrectionSettings > >( ), + const boost::shared_ptr< ObservationBias< 2 > > observationBiasCalculator = NULL ) + { + using namespace observation_models; + boost::shared_ptr< observation_models::ObservationModel< + 2, ObservationScalarType, TimeType, StateScalarType > > observationModel; + + // Check type of observation model. + switch( observableType ) + { + case angular_position: + { + // Check consistency input. + if( linkEnds.size( ) != 2 ) + { + std::string errorMessage = + "Error when making angular position model, " + + boost::lexical_cast< std::string >( linkEnds.size( ) ) + " link ends found"; + throw std::runtime_error( errorMessage ); + } + if( linkEnds.count( receiver ) == 0 ) + { + throw std::runtime_error( "Error when making angular position model, no receiver found" ); + } + if( linkEnds.count( transmitter ) == 0 ) + { + throw std::runtime_error( "Error when making angular position model, no transmitter found" ); + } + + // Create observation model + observationModel = boost::make_shared< AngularPositionObservationModel< + ObservationScalarType, TimeType, StateScalarType > >( + createLightTimeCalculator< ObservationScalarType, TimeType, StateScalarType >( + linkEnds.at( transmitter ), linkEnds.at( receiver ), + bodyMap, singleObservableCorrections ), observationBiasCalculator ); + + break; + } + default: + std::string errorMessage = "Error, observable " + boost::lexical_cast< std::string >( observableType ) + + " not recognized when making size 2 observation model."; + throw std::runtime_error( errorMessage ); + break; + } + + return observationModel; + } +}; + +//! Interface class for creating observation models of size 3. +template< typename ObservationScalarType, typename TimeType, typename StateScalarType > +class ObservationModelCreator< 3, ObservationScalarType, TimeType, StateScalarType > +{ +public: + + //! Function to create an observation model of size 3. + /*! + * Function to create an observation model of size 3. + * \param observableType Type of observation model that is to be created. + * \param linkEnds Link ends for observation model that is to be created + * \param bodyMap List of body objects that comprises the environment + * \param singleObservableCorrections List of light time corrections that are used when computing the observable. + * \param observationBiasCalculator + * \return Observation model of required settings. + */ + static boost::shared_ptr< observation_models::ObservationModel< + 3, ObservationScalarType, TimeType, StateScalarType > > createObservationModel( + const ObservableType observableType, + const LinkEnds& linkEnds, + const simulation_setup::NamedBodyMap &bodyMap, + const std::vector< boost::shared_ptr< LightTimeCorrectionSettings > >& singleObservableCorrections = + std::vector< boost::shared_ptr< LightTimeCorrectionSettings > >( ), + const boost::shared_ptr< ObservationBias < 3 > > observationBiasCalculator = NULL ) + { + using namespace observation_models; + boost::shared_ptr< observation_models::ObservationModel< + 3, ObservationScalarType, TimeType, StateScalarType > > observationModel; + + // Check type of observation model. + switch( observableType ) + { + case position_observable: + { + // Check consistency input. + if( linkEnds.size( ) != 1 ) + { + std::string errorMessage = + "Error when making position observable model, " + + boost::lexical_cast< std::string >( linkEnds.size( ) ) + " link ends found"; + throw std::runtime_error( errorMessage ); + } + + if( linkEnds.count( observed_body ) == 0 ) + { + throw std::runtime_error( "Error when making position observable model, no observed_body found" ); + } + + if( singleObservableCorrections.size( ) > 0 ) + { + throw std::runtime_error( "Error when making position observable model, found light time corrections" ); + } + if( linkEnds.at( observed_body ).second != "" ) + { + throw std::runtime_error( "Error, cannot yet create position function for reference point" ); + } + + // Create observation model + observationModel = boost::make_shared< PositionObservationModel< + ObservationScalarType, TimeType, StateScalarType > >( + boost::bind( &simulation_setup::Body::getTemplatedStateInBaseFrameFromEphemeris< + StateScalarType, TimeType >, + bodyMap.at( linkEnds.at( observed_body ).first ), _1 ), observationBiasCalculator ); + + break; + } + default: + std::string errorMessage = "Error, observable " + boost::lexical_cast< std::string >( observableType ) + + " not recognized when making size 3 observation model."; + throw std::runtime_error( errorMessage ); + break; + } + return observationModel; + } +}; + +} // namespace observation_models + +} // namespace tudat + +#endif // TUDAT_CREATEOBSERVATIONMODEL_H diff --git a/Tudat/SimulationSetup/EstimationSetup/createPositionPartials.cpp b/Tudat/SimulationSetup/EstimationSetup/createPositionPartials.cpp new file mode 100644 index 0000000000..da397287c0 --- /dev/null +++ b/Tudat/SimulationSetup/EstimationSetup/createPositionPartials.cpp @@ -0,0 +1,101 @@ +#include "Tudat/Astrodynamics/Ephemerides/simpleRotationalEphemeris.h" +#include "Tudat/SimulationSetup/EstimationSetup/createPositionPartials.h" + + +namespace tudat +{ + +namespace observation_partials +{ + + +//! Function to create partial object(s) of rotation matrix wrt a (double) parameter. +boost::shared_ptr< RotationMatrixPartial > createRotationMatrixPartialsWrtParameter( + const simulation_setup::NamedBodyMap& bodyMap, + const boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameterToEstimate ) +{ + using namespace simulation_setup; + using namespace ephemerides; + + // Declare return object. + boost::shared_ptr< RotationMatrixPartial > rotationMatrixPartial; + + // Get body for rotation of which partial is to be created. + boost::shared_ptr< Body > currentBody = bodyMap.at( parameterToEstimate->getParameterName( ).second.first ); + + // Check for which rotation model parameter the partial object is to be created. + switch( parameterToEstimate->getParameterName( ).first ) + { + case estimatable_parameters::constant_rotation_rate: + + if( boost::dynamic_pointer_cast< ephemerides::SimpleRotationalEphemeris >( + currentBody->getRotationalEphemeris( ) ) == NULL ) + { + throw std::runtime_error( "Warning, body's rotation model is not simple when making position w.r.t. constant rtoation rate partial" ) ; + } + + // Create rotation matrix partial object + rotationMatrixPartial = boost::make_shared< RotationMatrixPartialWrtConstantRotationRate >( + boost::dynamic_pointer_cast< SimpleRotationalEphemeris>( currentBody->getRotationalEphemeris( ) ) ); + break; + default: + std::string errorMessage = "Warning, rotation matrix partial not implemented for parameter " + + boost::lexical_cast< std::string >( parameterToEstimate->getParameterName( ).first ); + throw std::runtime_error( errorMessage ); + + break; + } + + return rotationMatrixPartial; + + +} + +//! Function to create partial object(s) of rotation matrix wrt a (vector) parameter. +boost::shared_ptr< RotationMatrixPartial > createRotationMatrixPartialsWrtParameter( + const simulation_setup::NamedBodyMap& bodyMap, + const boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > parameterToEstimate ) + +{ + using namespace simulation_setup; + using namespace ephemerides; + + // Declare return object. + boost::shared_ptr< RotationMatrixPartial > rotationMatrixPartial; + + // Get body for rotation of which partial is to be created. + boost::shared_ptr< Body > currentBody = bodyMap.at( parameterToEstimate->getParameterName( ).second.first ); + + // Check for which rotation model parameter the partial object is to be created. + switch( parameterToEstimate->getParameterName( ).first ) + { + case estimatable_parameters::rotation_pole_position: + + + if( boost::dynamic_pointer_cast< ephemerides::SimpleRotationalEphemeris >( + currentBody->getRotationalEphemeris( ) ) == NULL ) + { + std::string errorMessage = "Warning, body's rotation model is not simple when making position w.r.t. pole position partial"; + throw std::runtime_error( errorMessage ); + } + + // Create rotation matrix partial object + rotationMatrixPartial = boost::make_shared< RotationMatrixPartialWrtPoleOrientation >( + boost::dynamic_pointer_cast< SimpleRotationalEphemeris>( currentBody->getRotationalEphemeris( ) ) ); + break; + + default: + std::string errorMessage = "Warning, rotation matrix partial not implemented for parameter " + + boost::lexical_cast< std::string >( parameterToEstimate->getParameterName( ).first ); + throw std::runtime_error( errorMessage ); + break; + } + + return rotationMatrixPartial; + +} + + +} + +} diff --git a/Tudat/SimulationSetup/EstimationSetup/createPositionPartials.h b/Tudat/SimulationSetup/EstimationSetup/createPositionPartials.h new file mode 100644 index 0000000000..57f8ce1479 --- /dev/null +++ b/Tudat/SimulationSetup/EstimationSetup/createPositionPartials.h @@ -0,0 +1,128 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_CREATEPOSITIONPARTIALS_H +#define TUDAT_CREATEPOSITIONPARTIALS_H + +#include +#include + +#include + +#include + +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" +#include "Tudat/Astrodynamics/ObservationModels/linkTypeDefs.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h" +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/initialTranslationalState.h" +#include "Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/rotationMatrixPartial.h" + + +namespace tudat +{ + +namespace observation_partials +{ + + +//! Function to create partial object(s) of rotation matrix wrt a (double) parameter. +/*! + * Function to create partial object(s) of rotation matrix from a body-fixed to inertial frame wrt a (double) parameter. + * \param bodyMap Map of body objects, used in the creation of the partial. + * \param parameterToEstimate Parameter wrt which rotation matrix partial object is to be created. + * \return Requested rotation matrix partial object. + */ +boost::shared_ptr< RotationMatrixPartial > createRotationMatrixPartialsWrtParameter( + const simulation_setup::NamedBodyMap& bodyMap, + const boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameterToEstimate ); + +//! Function to create partial object(s) of rotation matrix wrt a (vector) parameter. +/*! + * Function to create partial object(s) of rotation matrix from a body-fixed to inertial frame wrt a (vector) parameter. + * \param bodyMap Map of body objects, used in the creation of the partial. + * \param parameterToEstimate Parameter wrt which rotation matrix partial object is to be created. + * \return Requested rotation matrix partial object. + */ +boost::shared_ptr< RotationMatrixPartial > createRotationMatrixPartialsWrtParameter( + const simulation_setup::NamedBodyMap& bodyMap, + const boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > parameterToEstimate ); + +//! Function to create rotation matrix partial objects for all rotation model parameters of given body in given set of +//! parameters. +/*! + * Function to create rotation matrix partial objects for all rotation model parameters of given body in given set of + * parameters. All parameter objects of given EstimatableParameterSet are checked whether they are firstly a property of + * the requested body, and secondly if they represent a property of the rotation from the body-fixed to base frame. + * If both conditions are met, a partial is created and added to teh return list. + * \param parametersToEstimate Set of all parameters which are to be estimated. + * \param bodyName Name of body for which to create partials of rotation from body-fixed to base frame + * \param bodyMap Map of body objects, used in the creation of the partials. + * \return Rotation matrix partial objects for all rotation model parameters of bodyName in parametersToEstimate. + */ +template< typename ParameterType > +RotationMatrixPartialNamedList createRotationMatrixPartials( + const boost::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > parametersToEstimate, + const std::string& bodyName, const simulation_setup::NamedBodyMap& bodyMap ) + +{ + // Declare map to return. + std::map< std::pair< estimatable_parameters::EstimatebleParametersEnum, std::string >, + boost::shared_ptr< RotationMatrixPartial > > + rotationMatrixPartials; + + // Retrieve double and vector parameters from total set of parameters. + std::map< int, boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > > doubleParameters = + parametersToEstimate->getDoubleParameters( ); + std::map< int, boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > > vectorParameters = + parametersToEstimate->getVectorParameters( ); + + // Iterate over double parameters. + for( std::map< int, boost::shared_ptr< estimatable_parameters::EstimatableParameter< double > > >::iterator + parameterIterator = doubleParameters.begin( ); parameterIterator != doubleParameters.end( ); parameterIterator++ ) + { + // Check parameter is rotational property of requested body. + if( ( parameterIterator->second->getParameterName( ).second.first == bodyName ) && + ( estimatable_parameters::isParameterRotationMatrixProperty( + parameterIterator->second->getParameterName( ).first ) ) ) + { + // Create partial object. + rotationMatrixPartials[ std::make_pair( + parameterIterator->second->getParameterName( ).first, + parameterIterator->second->getSecondaryIdentifier( ) ) ] = + createRotationMatrixPartialsWrtParameter( bodyMap, parameterIterator->second ); + } + } + + // Iterate over vector parameters. + for( std::map< int, boost::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > >::iterator + parameterIterator = vectorParameters.begin( ); parameterIterator != vectorParameters.end( ); parameterIterator++ ) + { + // Check parameter is rotational property of requested body. + if( ( parameterIterator->second->getParameterName( ).second.first == bodyName ) && + ( estimatable_parameters::isParameterRotationMatrixProperty( + parameterIterator->second->getParameterName( ).first ) ) ) + { + // Create partial object. + rotationMatrixPartials[ std::make_pair( + parameterIterator->second->getParameterName( ).first, + parameterIterator->second->getSecondaryIdentifier( ) ) ] = + createRotationMatrixPartialsWrtParameter( bodyMap, parameterIterator->second ); + } + } + + return rotationMatrixPartials; +} + + +} // namespace observation_partials + +} // namespace tudat + +#endif // TUDAT_CREATEPOSITIONPARTIALS_H diff --git a/Tudat/SimulationSetup/EstimationSetup/createStateDerivativePartials.cpp b/Tudat/SimulationSetup/EstimationSetup/createStateDerivativePartials.cpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tudat/SimulationSetup/EstimationSetup/createStateDerivativePartials.h b/Tudat/SimulationSetup/EstimationSetup/createStateDerivativePartials.h new file mode 100644 index 0000000000..623d095da6 --- /dev/null +++ b/Tudat/SimulationSetup/EstimationSetup/createStateDerivativePartials.h @@ -0,0 +1,89 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_CREATESTATEDERIVATIVEPARTIALS_H +#define TUDAT_CREATESTATEDERIVATIVEPARTIALS_H + +#include "Tudat/Astrodynamics/OrbitDetermination/stateDerivativePartial.h" +#include "Tudat/SimulationSetup/PropagationSetup/propagationSettings.h" +#include "Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h" +#include "Tudat/Astrodynamics/Propagators/nBodyStateDerivative.h" +#include "Tudat/SimulationSetup/EstimationSetup/createAccelerationPartials.h" + +namespace tudat +{ + +namespace simulation_setup +{ + +//! Function to create a set of state derivative partial objects. +/*! + * Function to create a set of state derivative partial objects for any propagated state types. + * \param stateDerivativeModels List of state derivative models, ordered by state type (key) + * \param bodyMap List of boy objects storing environment models of simulation + * \param parametersToEstimate Object containing all parameters that are to be estimated and their current settings and + * values. + * return List partials of state derivative models from. The key is the type of dynamics for which partials are taken, + * the values are StateDerivativePartialsMap (see StateDerivativePartialsMap definition for details). + */ +template< typename StateScalarType, typename TimeType, typename InitialStateParameterType > +std::map< propagators::IntegratedStateType, orbit_determination::StateDerivativePartialsMap > createStateDerivativePartials( + const std::unordered_map< propagators::IntegratedStateType, + std::vector< boost::shared_ptr< propagators::SingleStateTypeDerivative< StateScalarType, TimeType > > > > + stateDerivativeModels, + const simulation_setup::NamedBodyMap& bodyMap, + const boost::shared_ptr< estimatable_parameters::EstimatableParameterSet< InitialStateParameterType > > + parametersToEstimate ) +{ + std::map< propagators::IntegratedStateType, orbit_determination::StateDerivativePartialsMap > stateDerivativePartials; + + // Iterate over all state types + for( typename std::unordered_map< propagators::IntegratedStateType, + std::vector< boost::shared_ptr< propagators::SingleStateTypeDerivative< StateScalarType, TimeType > > > >:: + const_iterator stateDerivativeIterator = stateDerivativeModels.begin( ); + stateDerivativeIterator != stateDerivativeModels.end( ); + stateDerivativeIterator++ ) + { + // Identify state types + switch( stateDerivativeIterator->first ) + { + case propagators::transational_state: + { + if( stateDerivativeIterator->second.size( ) > 1 ) + { + throw std::runtime_error( + "Error, cannot yet process multiple separate same type propagators when making partial derivatives of translational state." ); + } + else + { + // Retrieve acceleration models and create partials + basic_astrodynamics::AccelerationMap accelerationModelList = + boost::dynamic_pointer_cast< propagators::NBodyStateDerivative< StateScalarType, TimeType > >( + stateDerivativeIterator->second.at( 0 ) )->getFullAccelerationsMap( ); + stateDerivativePartials[ propagators::transational_state ] = + createAccelerationPartialsMap< InitialStateParameterType >( + accelerationModelList, bodyMap, parametersToEstimate ); + } + break; + } + default: + std::string errorMessage = "Cannot yet create state derivative partial models for type " + + boost::lexical_cast< std::string >( stateDerivativeIterator->first ); + throw std::runtime_error( errorMessage ); + } + } + return stateDerivativePartials; +} + +} // namespace simulation_setup + +} // namespace tudat + +#endif // TUDAT_CREATESTATEDERIVATIVEPARTIALS_H diff --git a/Tudat/SimulationSetup/EstimationSetup/estimatableParameterSettings.h b/Tudat/SimulationSetup/EstimationSetup/estimatableParameterSettings.h new file mode 100644 index 0000000000..736d9aa9c5 --- /dev/null +++ b/Tudat/SimulationSetup/EstimationSetup/estimatableParameterSettings.h @@ -0,0 +1,179 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_ESTIMATABLEPARAMETERSETTINGS_H +#define TUDAT_ESTIMATABLEPARAMETERSETTINGS_H + +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h" + +namespace tudat +{ + +namespace estimatable_parameters +{ + +//! Class for providing settings for parameters to estimate +/*! + * Class for providing settings for parameters to estimate. This class is a functional base class for parameters that + * require no information in addition to their type. + * This class can be used for the easy setup of parameter objects (see createEstimatableParameters.h), but users may also + * chose to do so manually. (Derived) Class members are all public, for ease of access and modification. + */ +class EstimatableParameterSettings +{ +public: + + //! Constructor. + /*! + * Constructor, takes parameter type and body of which it is a property. + * \param associatedBody Body of which parameter is a property. + * \param parameterType Type of parameter. + * \param pointOnBodyId Reference point on body associated with parameter (empty by default). + */ + EstimatableParameterSettings( const std::string associatedBody , + const EstimatebleParametersEnum parameterType, + const std::string pointOnBodyId = "" ): + parameterType_( std::make_pair( parameterType, std::make_pair( associatedBody, pointOnBodyId ) ) ){ } + + //! Virtual destructor + /*! + * Virtual destructor + */ + virtual ~EstimatableParameterSettings( ){ } + + + //! Identifier for parameter. + /*! + * Identifier for parameter, contains type of parameter and body of which parameter is a property. + */ + EstimatebleParameterIdentifier parameterType_; + +}; + +//! Class for providing settings spherical harmonic coefficient(s) parameter +class SphericalHarmonicEstimatableParameterSettings: public EstimatableParameterSettings +{ +public: + + //! Constructor + /*! + * Constructor, used to set user-defined list of degrees and orders. + * \param blockIndices List of degrees and orders that are to estimated (first and second of each entry are + * degree and order). + * \param associatedBody Body for which coefficients are to be estimated. + * \param parameterType Type of parameter that is to be estimated (must be spherical_harmonics_cosine_coefficient_block + * of spherical_harmonics_sine_coefficient_block). + */ + SphericalHarmonicEstimatableParameterSettings( const std::vector< std::pair< int, int > > blockIndices, + const std::string associatedBody, + const EstimatebleParametersEnum parameterType ): + EstimatableParameterSettings( associatedBody, parameterType ), blockIndices_( blockIndices ) + { + if( ( parameterType != spherical_harmonics_cosine_coefficient_block ) && + ( parameterType != spherical_harmonics_sine_coefficient_block ) ) + { + std::cerr<<"Error when making spherical harmonic parameter settings, input parameter type is inconsistent."< > blockIndices_; +}; + +//! Class to define settings for estimating an initial translational state. +template< typename InitialStateParameterType > +class InitialTranslationalStateEstimatableParameterSettings: public EstimatableParameterSettings +{ +public: + + //! Constructor, sets initial value of translational state. + /*! + * Constructor, sets initial value of translational state. + * \param associatedBody Body for which initial state is to be estimated. + * \param initialStateValue Current value of initial state (w.r.t. centralBody) + * \param centralBody Body w.r.t. which the initial state is to be estimated. + * \param frameOrientation Orientation of the frame in which the state is defined. + */ + InitialTranslationalStateEstimatableParameterSettings( + const std::string& associatedBody, + const Eigen::Matrix< InitialStateParameterType, 6, 1 > initialStateValue, + const std::string& centralBody = "SSB", const std::string& frameOrientation = "ECLIPJ2000" ): + EstimatableParameterSettings( associatedBody, initial_body_state ), initialTime_( TUDAT_NAN ), + initialStateValue_( initialStateValue ), + centralBody_( centralBody ), frameOrientation_( frameOrientation ){ } + + //! Constructor, without initial value of translational state. + /*! + * Constructor, without initial value of translational state. Current initial state is retrieved from environment + * (ephemeris objects) during creation of parameter object. + * \param associatedBody Body for which initial state is to be estimated. + * \param initialTime Time at which initial state is defined. + * \param centralBody Body w.r.t. which the initial state is to be estimated. + * \param frameOrientation Orientation of the frame in which the state is defined. + */ + InitialTranslationalStateEstimatableParameterSettings( + const std::string& associatedBody, + const double initialTime, + const std::string& centralBody = "SSB", const std::string& frameOrientation = "ECLIPJ2000" ): + EstimatableParameterSettings( associatedBody, initial_body_state ), initialTime_( initialTime ), + centralBody_( centralBody ), frameOrientation_( frameOrientation ){ } + + //! Time at which initial state is defined (NaN for user-defined initial state value). + double initialTime_; + + //! Current value of initial state (w.r.t. centralBody), set manually by used. + Eigen::Matrix< InitialStateParameterType, 6, 1 > initialStateValue_; + + //! Body w.r.t. which the initial state is to be estimated. + std::string centralBody_; + + //! Orientation of the frame in which the state is defined. + std::string frameOrientation_; + +}; + +} // namespace estimatable_parameters + +} // namespace tudat +#endif // TUDAT_ESTIMATABLEPARAMETERSETTINGS_H diff --git a/Tudat/SimulationSetup/PropagationSetup/accelerationSettings.h b/Tudat/SimulationSetup/PropagationSetup/accelerationSettings.h new file mode 100644 index 0000000000..ae87a1ac6f --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/accelerationSettings.h @@ -0,0 +1,180 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_ACCELERATIONSETTINGS_H +#define TUDAT_ACCELERATIONSETTINGS_H + +#include "Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.h" +#include "Tudat/Astrodynamics/Gravitation/centralGravityModel.h" +#include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h" +#include "Tudat/Astrodynamics/Gravitation/thirdBodyPerturbation.h" +#include "Tudat/Astrodynamics/Aerodynamics/aerodynamicAcceleration.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h" +#include "Tudat/SimulationSetup/PropagationSetup/createThrustModelGuidance.h" + + + +namespace tudat +{ + +namespace simulation_setup +{ + +//! Class for providing settings for acceleration model. +/*! + * Class for providing settings for acceleration model. This class is a functional (base) class for + * settings of acceleration models that require no information in addition to their type. + * Classes defining settings for acceleration models requiring additional information must be + * derived from this class. + * Bodies exerting and undergong acceleration are set externally from this class. + * This class can be used for the easy setup of acceleration models + * (see createAccelerationModels.h), but users may also chose to do so manually. + * (Derived) Class members are all public, for ease of access and modification. + */ +class AccelerationSettings +{ +public: + //! Constructor, sets type of acceleration. + /*! + * Constructor, sets type of acceleration. + * \param accelerationType Type of acceleration from AvailableAcceleration enum. + */ + AccelerationSettings( const basic_astrodynamics::AvailableAcceleration accelerationType ): + accelerationType_( accelerationType ){ } + + //! Destructor. + virtual ~AccelerationSettings( ){ } + + //! Type of acceleration from AvailableAcceleration enum. + basic_astrodynamics::AvailableAcceleration accelerationType_; + +}; + +//! Class for providing settings for spherical harmonics acceleration model. +/*! + * Class for providing settings for spherical harmonics acceleration model, + * specifically the maximum degree and order up to which the field is to be expanded. Note that + * the minimum degree and order are currently always set to zero. + */ +class SphericalHarmonicAccelerationSettings: public AccelerationSettings +{ +public: + //! Constructor to set maximum degree and order that is to be taken into account. + /*! + * Constructor to set maximum degree and order that is to be taken into account. + * \param maximumDegree Maximum degree + * \param maximumOrder Maximum order + */ + SphericalHarmonicAccelerationSettings( const int maximumDegree, + const int maximumOrder ): + AccelerationSettings( basic_astrodynamics::spherical_harmonic_gravity ), + maximumDegree_( maximumDegree ), maximumOrder_( maximumOrder ){ } + + //! Maximum degree that is to be used for spherical harmonic acceleration + int maximumDegree_; + + //! Maximum order that is to be used for spherical harmonic acceleration + int maximumOrder_; +}; + +//! Class for providing acceleration settings for mutual spherical harmonics acceleration model. +/*! + * Class for providing acceleration settings for mutual spherical harmonics acceleration model, + * specifically the maximum degree and order up to which the fields of the bodies are be expanded. + * Please note that the minimum degrees and orders are currently always set to zero. + */ +class MutualSphericalHarmonicAccelerationSettings: public AccelerationSettings +{ +public: + + //! Constructor to set maximum degrees and orders that are to be taken into account. + /*! + * Constructor to set maximum degrees and orders that are to be taken into account. + * \param maximumDegreeOfBodyExertingAcceleration Maximum degree of body exerting acceleration. + * \param maximumOrderOfBodyExertingAcceleration Maximum order of body exerting acceleration. + * \param maximumDegreeOfBodyUndergoingAcceleration Maximum degree of body undergoing acceleration. + * \param maximumOrderOfBodyUndergoingAcceleration Maximum order of body undergoing acceleration. + * \param maximumDegreeOfCentralBody Maximum degree of central body (only releveant for 3rd body acceleration). + * \param maximumOrderOfCentralBody Maximum order of central body (only releveant for 3rd body acceleration). + */ + MutualSphericalHarmonicAccelerationSettings( const int maximumDegreeOfBodyExertingAcceleration, + const int maximumOrderOfBodyExertingAcceleration, + const int maximumDegreeOfBodyUndergoingAcceleration, + const int maximumOrderOfBodyUndergoingAcceleration, + const int maximumDegreeOfCentralBody = 0, + const int maximumOrderOfCentralBody = 0 ): + AccelerationSettings( basic_astrodynamics::mutual_spherical_harmonic_gravity ), + maximumDegreeOfBodyExertingAcceleration_( maximumDegreeOfBodyExertingAcceleration ), + maximumOrderOfBodyExertingAcceleration_( maximumOrderOfBodyExertingAcceleration ), + maximumDegreeOfBodyUndergoingAcceleration_( maximumDegreeOfBodyUndergoingAcceleration ), + maximumOrderOfBodyUndergoingAcceleration_( maximumOrderOfBodyUndergoingAcceleration ), + maximumDegreeOfCentralBody_( maximumDegreeOfCentralBody ), maximumOrderOfCentralBody_( maximumOrderOfCentralBody ){ } + + //! Maximum degree of body exerting acceleration. + int maximumDegreeOfBodyExertingAcceleration_; + + //! Maximum order of body exerting acceleration. + int maximumOrderOfBodyExertingAcceleration_; + + //! Maximum degree of body undergoing acceleration. + int maximumDegreeOfBodyUndergoingAcceleration_; + + //! Maximum order of body undergoing acceleration. + int maximumOrderOfBodyUndergoingAcceleration_; + + //! Maximum degree of central body (only releveant for 3rd body acceleration). + int maximumDegreeOfCentralBody_; + + //! Maximum order of central body (only releveant for 3rd body acceleration). + int maximumOrderOfCentralBody_; +}; + +//! Class for providing acceleration settings for a thrust acceleration model +/*! + * Class for providing acceleration settings for a thrust acceleration model. Settings for the direction and magnitude + * guidance of the thrust are provided/ + */ +class ThrustAccelerationSettings: public AccelerationSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param thrustDirectionGuidanceSettings Settings for the direction of the thrust + * \param thrustMagnitudeSettings Settings for the magnitude of the thrust + */ + ThrustAccelerationSettings( + const boost::shared_ptr< ThrustDirectionGuidanceSettings > thrustDirectionGuidanceSettings, + const boost::shared_ptr< ThrustEngineSettings > thrustMagnitudeSettings ): + AccelerationSettings( basic_astrodynamics::thrust_acceleration ), + thrustDirectionGuidanceSettings_( thrustDirectionGuidanceSettings ), + thrustMagnitudeSettings_( thrustMagnitudeSettings ){ } + + //! Destructor. + ~ThrustAccelerationSettings( ){ } + + //! Settings for the direction of the thrust + boost::shared_ptr< ThrustDirectionGuidanceSettings > thrustDirectionGuidanceSettings_; + + //! Settings for the magnitude of the thrust + boost::shared_ptr< ThrustEngineSettings > thrustMagnitudeSettings_; +}; + +//! Typedef defining a list of acceleration settings, set up in the same manner as the +//! AccelerationMap typedef. +typedef std::map< std::string, std::map< std::string, std::vector< boost::shared_ptr< +AccelerationSettings > > > > SelectedAccelerationMap; + +} // namespace simulation_setup + +} // namespace tudat + +#endif // TUDAT_ACCELERATIONSETTINGS_H diff --git a/Tudat/SimulationSetup/PropagationSetup/createAccelerationModels.cpp b/Tudat/SimulationSetup/PropagationSetup/createAccelerationModels.cpp new file mode 100644 index 0000000000..b026ef1d76 --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/createAccelerationModels.cpp @@ -0,0 +1,970 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include + +#include +#include +#include + +#include "Tudat/Astrodynamics/Aerodynamics/flightConditions.h" +#include "Tudat/Astrodynamics/Ephemerides/frameManager.h" +#include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.h" +#include "Tudat/Astrodynamics/Propulsion/thrustMagnitudeWrapper.h" +#include "Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.h" +#include "Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h" +#include "Tudat/Basics/utilities.h" +#include "Tudat/SimulationSetup/PropagationSetup/accelerationSettings.h" +#include "Tudat/SimulationSetup/PropagationSetup/createAccelerationModels.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createFlightConditions.h" + +namespace tudat +{ + +namespace simulation_setup +{ + +using namespace aerodynamics; +using namespace gravitation; +using namespace basic_astrodynamics; +using namespace electro_magnetism; +using namespace ephemerides; + + +//! Function to create a direct (i.e. not third-body) gravitational acceleration (of any type) +boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > createDirectGravitationalAcceleration( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const boost::shared_ptr< AccelerationSettings > accelerationSettings, + const std::string& nameOfCentralBody, + const bool isCentralBody ) +{ + // Check if sum of gravitational parameters (i.e. inertial force w.r.t. central body) should be used. + bool sumGravitationalParameters = 0; + if( ( nameOfCentralBody == nameOfBodyExertingAcceleration ) && bodyUndergoingAcceleration != NULL ) + { + sumGravitationalParameters = 1; + } + + + // Check type of acceleration model and create. + boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > accelerationModel; + switch( accelerationSettings->accelerationType_ ) + { + case central_gravity: + accelerationModel = createCentralGravityAcceleratioModel( + bodyUndergoingAcceleration, + bodyExertingAcceleration, + nameOfBodyUndergoingAcceleration, + nameOfBodyExertingAcceleration, + sumGravitationalParameters ); + break; + case spherical_harmonic_gravity: + accelerationModel = createSphericalHarmonicsGravityAcceleration( + bodyUndergoingAcceleration, + bodyExertingAcceleration, + nameOfBodyUndergoingAcceleration, + nameOfBodyExertingAcceleration, + accelerationSettings, + sumGravitationalParameters ); + break; + case mutual_spherical_harmonic_gravity: + accelerationModel = createMutualSphericalHarmonicsGravityAcceleration( + bodyUndergoingAcceleration, + bodyExertingAcceleration, + nameOfBodyUndergoingAcceleration, + nameOfBodyExertingAcceleration, + accelerationSettings, + sumGravitationalParameters, + isCentralBody ); + break; + default: + + std::string errorMessage = "Error when making gravitional acceleration model, cannot parse type " + + boost::lexical_cast< std::string >( accelerationSettings->accelerationType_ ); + throw std::runtime_error( errorMessage ); + } + return accelerationModel; +} + +//! Function to create a third-body gravitational acceleration (of any type) +boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > createThirdBodyGravitationalAcceleration( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const boost::shared_ptr< Body > centralBody, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const std::string& nameOfCentralBody, + const boost::shared_ptr< AccelerationSettings > accelerationSettings ) +{ + // Check type of acceleration model and create. + boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > accelerationModel; + switch( accelerationSettings->accelerationType_ ) + { + case central_gravity: + accelerationModel = boost::make_shared< ThirdBodyCentralGravityAcceleration >( + boost::dynamic_pointer_cast< CentralGravitationalAccelerationModel3d >( + createDirectGravitationalAcceleration( + bodyUndergoingAcceleration, bodyExertingAcceleration, + nameOfBodyUndergoingAcceleration, nameOfBodyExertingAcceleration, + accelerationSettings, "", 0 ) ), + boost::dynamic_pointer_cast< CentralGravitationalAccelerationModel3d >( + createDirectGravitationalAcceleration( + centralBody, bodyExertingAcceleration, + nameOfCentralBody, nameOfBodyExertingAcceleration, + accelerationSettings, "", 1 ) ), nameOfCentralBody ); + break; + case spherical_harmonic_gravity: + accelerationModel = boost::make_shared< ThirdBodySphericalHarmonicsGravitationalAccelerationModel >( + boost::dynamic_pointer_cast< SphericalHarmonicsGravitationalAccelerationModel >( + createDirectGravitationalAcceleration( + bodyUndergoingAcceleration, bodyExertingAcceleration, + nameOfBodyUndergoingAcceleration, nameOfBodyExertingAcceleration, + accelerationSettings, "", 0 ) ), + boost::dynamic_pointer_cast< SphericalHarmonicsGravitationalAccelerationModel >( + createDirectGravitationalAcceleration( + centralBody, bodyExertingAcceleration, nameOfCentralBody, nameOfBodyExertingAcceleration, + accelerationSettings, "", 1 ) ), nameOfCentralBody ); + break; + case mutual_spherical_harmonic_gravity: + accelerationModel = boost::make_shared< ThirdBodyMutualSphericalHarmonicsGravitationalAccelerationModel >( + boost::dynamic_pointer_cast< MutualSphericalHarmonicsGravitationalAccelerationModel >( + createDirectGravitationalAcceleration( + bodyUndergoingAcceleration, bodyExertingAcceleration, + nameOfBodyUndergoingAcceleration, nameOfBodyExertingAcceleration, + accelerationSettings, "", 0 ) ), + boost::dynamic_pointer_cast< MutualSphericalHarmonicsGravitationalAccelerationModel >( + createDirectGravitationalAcceleration( + centralBody, bodyExertingAcceleration, nameOfCentralBody, nameOfBodyExertingAcceleration, + accelerationSettings, "", 1 ) ), nameOfCentralBody ); + break; + default: + + std::string errorMessage = "Error when making third-body gravitional acceleration model, cannot parse type " + + boost::lexical_cast< std::string >( accelerationSettings->accelerationType_ ); + throw std::runtime_error( errorMessage ); + } + return accelerationModel; +} + +//! Function to create gravitational acceleration (of any type) +boost::shared_ptr< AccelerationModel< Eigen::Vector3d > > createGravitationalAccelerationModel( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const boost::shared_ptr< AccelerationSettings > accelerationSettings, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const boost::shared_ptr< Body > centralBody, + const std::string& nameOfCentralBody ) +{ + + boost::shared_ptr< AccelerationModel< Eigen::Vector3d > > accelerationModelPointer; + if( accelerationSettings->accelerationType_ != central_gravity && + accelerationSettings->accelerationType_ != spherical_harmonic_gravity && + accelerationSettings->accelerationType_ != mutual_spherical_harmonic_gravity ) + { + throw std::runtime_error( "Error when making gravitational acceleration, type is inconsistent" ); + } + + if( nameOfCentralBody == nameOfBodyExertingAcceleration || ephemerides::isFrameInertial( nameOfCentralBody ) ) + { + accelerationModelPointer = createDirectGravitationalAcceleration( bodyUndergoingAcceleration, + bodyExertingAcceleration, + nameOfBodyUndergoingAcceleration, + nameOfBodyExertingAcceleration, + accelerationSettings, + nameOfCentralBody, false ); + } + else + { + accelerationModelPointer = createThirdBodyGravitationalAcceleration( bodyUndergoingAcceleration, + bodyExertingAcceleration, + centralBody, + nameOfBodyUndergoingAcceleration, + nameOfBodyExertingAcceleration, + nameOfCentralBody, accelerationSettings ); + } + + return accelerationModelPointer; +} + + +//! Function to create central gravity acceleration model. +boost::shared_ptr< CentralGravitationalAccelerationModel3d > createCentralGravityAcceleratioModel( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const bool useCentralBodyFixedFrame ) +{ + // Declare pointer to return object. + boost::shared_ptr< CentralGravitationalAccelerationModel3d > accelerationModelPointer; + + // Check if body is endowed with a gravity field model (i.e. is capable of exerting + // gravitation acceleration). + if( bodyExertingAcceleration->getGravityFieldModel( ) == NULL ) + { + throw std::runtime_error( + std::string( "Error, gravity field model not set when making central ") + + " gravitational acceleration of " + nameOfBodyExertingAcceleration + " on " + + nameOfBodyUndergoingAcceleration ); + } + else + { + boost::function< double( ) > gravitationalParameterFunction; + + // Set correct value for gravitational parameter. + if( useCentralBodyFixedFrame == 0 || + bodyUndergoingAcceleration->getGravityFieldModel( ) == NULL ) + { + gravitationalParameterFunction = + boost::bind( &gravitation::GravityFieldModel::getGravitationalParameter, + bodyExertingAcceleration->getGravityFieldModel( ) ); + } + else + { + boost::function< double( ) > gravitationalParameterOfBodyExertingAcceleration = + boost::bind( &gravitation::GravityFieldModel::getGravitationalParameter, + bodyExertingAcceleration->getGravityFieldModel( ) ); + boost::function< double( ) > gravitationalParameterOfBodyUndergoingAcceleration = + boost::bind( &gravitation::GravityFieldModel::getGravitationalParameter, + bodyUndergoingAcceleration->getGravityFieldModel( ) ); + gravitationalParameterFunction = + boost::bind( &utilities::sumFunctionReturn< double >, + gravitationalParameterOfBodyExertingAcceleration, + gravitationalParameterOfBodyUndergoingAcceleration ); + } + + // Create acceleration object. + accelerationModelPointer = + boost::make_shared< CentralGravitationalAccelerationModel3d >( + boost::bind( &Body::getPosition, bodyUndergoingAcceleration ), + gravitationalParameterFunction, + boost::bind( &Body::getPosition, bodyExertingAcceleration ), + useCentralBodyFixedFrame ); + } + + + return accelerationModelPointer; +} + +//! Function to create spherical harmonic gravity acceleration model. +boost::shared_ptr< gravitation::SphericalHarmonicsGravitationalAccelerationModel > +createSphericalHarmonicsGravityAcceleration( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const boost::shared_ptr< AccelerationSettings > accelerationSettings, + const bool useCentralBodyFixedFrame ) +{ + // Declare pointer to return object + boost::shared_ptr< SphericalHarmonicsGravitationalAccelerationModel > accelerationModel; + + // Dynamic cast acceleration settings to required type and check consistency. + boost::shared_ptr< SphericalHarmonicAccelerationSettings > sphericalHarmonicsSettings = + boost::dynamic_pointer_cast< SphericalHarmonicAccelerationSettings >( + accelerationSettings ); + if( sphericalHarmonicsSettings == NULL ) + { + throw std::runtime_error( + std::string( "Error, acceleration settings inconsistent ") + + " making sh gravitational acceleration of " + nameOfBodyExertingAcceleration + + " on " + nameOfBodyUndergoingAcceleration ); + } + else + { + // Get pointer to gravity field of central body and cast to required type. + boost::shared_ptr< SphericalHarmonicsGravityField > sphericalHarmonicsGravityField = + boost::dynamic_pointer_cast< SphericalHarmonicsGravityField >( + bodyExertingAcceleration->getGravityFieldModel( ) ); + + boost::shared_ptr< RotationalEphemeris> rotationalEphemeris = + bodyExertingAcceleration->getRotationalEphemeris( ); + if( sphericalHarmonicsGravityField == NULL ) + { + throw std::runtime_error( + std::string( "Error, spherical harmonic gravity field model not set when ") + + " making sh gravitational acceleration of " + + nameOfBodyExertingAcceleration + + " on " + nameOfBodyUndergoingAcceleration ); + } + else + { + if( rotationalEphemeris == NULL ) + { + throw std::runtime_error( "Warning when making spherical harmonic acceleration on body " + + nameOfBodyUndergoingAcceleration + ", no rotation model found for " + + nameOfBodyExertingAcceleration ); + } + + if( rotationalEphemeris->getTargetFrameOrientation( ) != + sphericalHarmonicsGravityField->getFixedReferenceFrame( ) ) + { + throw std::runtime_error( "Warning when making spherical harmonic acceleration on body " + + nameOfBodyUndergoingAcceleration + ", rotation model found for " + + nameOfBodyExertingAcceleration + " is incompatible, frames are: " + + rotationalEphemeris->getTargetFrameOrientation( ) + " and " + + sphericalHarmonicsGravityField->getFixedReferenceFrame( ) ); + } + + boost::function< double( ) > gravitationalParameterFunction; + + // Check if mutual acceleration is to be used. + if( useCentralBodyFixedFrame == false || + bodyUndergoingAcceleration->getGravityFieldModel( ) == NULL ) + { + gravitationalParameterFunction = + boost::bind( &SphericalHarmonicsGravityField::getGravitationalParameter, + sphericalHarmonicsGravityField ); + } + else + { + // Create function returning summed gravitational parameter of the two bodies. + boost::function< double( ) > gravitationalParameterOfBodyExertingAcceleration = + boost::bind( &gravitation::GravityFieldModel::getGravitationalParameter, + sphericalHarmonicsGravityField ); + boost::function< double( ) > gravitationalParameterOfBodyUndergoingAcceleration = + boost::bind( &gravitation::GravityFieldModel::getGravitationalParameter, + bodyUndergoingAcceleration->getGravityFieldModel( ) ); + gravitationalParameterFunction = + boost::bind( &utilities::sumFunctionReturn< double >, + gravitationalParameterOfBodyExertingAcceleration, + gravitationalParameterOfBodyUndergoingAcceleration ); + } + + // Create acceleration object. + accelerationModel = + boost::make_shared< SphericalHarmonicsGravitationalAccelerationModel > + ( boost::bind( &Body::getPosition, bodyUndergoingAcceleration ), + gravitationalParameterFunction, + sphericalHarmonicsGravityField->getReferenceRadius( ), + boost::bind( &SphericalHarmonicsGravityField::getCosineCoefficients, + sphericalHarmonicsGravityField, + sphericalHarmonicsSettings->maximumDegree_, + sphericalHarmonicsSettings->maximumOrder_ ), + boost::bind( &SphericalHarmonicsGravityField::getSineCoefficients, + sphericalHarmonicsGravityField, + sphericalHarmonicsSettings->maximumDegree_, + sphericalHarmonicsSettings->maximumOrder_ ), + boost::bind( &Body::getPosition, bodyExertingAcceleration ), + boost::bind( &Body::getCurrentRotationToGlobalFrame, + bodyExertingAcceleration ), useCentralBodyFixedFrame ); + } + } + return accelerationModel; +} + +//! Function to create mutual spherical harmonic gravity acceleration model. +boost::shared_ptr< gravitation::MutualSphericalHarmonicsGravitationalAccelerationModel > +createMutualSphericalHarmonicsGravityAcceleration( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const boost::shared_ptr< AccelerationSettings > accelerationSettings, + const bool useCentralBodyFixedFrame, + const bool acceleratedBodyIsCentralBody ) +{ + using namespace basic_astrodynamics; + + // Declare pointer to return object + boost::shared_ptr< MutualSphericalHarmonicsGravitationalAccelerationModel > accelerationModel; + + // Dynamic cast acceleration settings to required type and check consistency. + boost::shared_ptr< MutualSphericalHarmonicAccelerationSettings > mutualSphericalHarmonicsSettings = + boost::dynamic_pointer_cast< MutualSphericalHarmonicAccelerationSettings >( accelerationSettings ); + if( mutualSphericalHarmonicsSettings == NULL ) + { + std::string errorMessage = "Error, expected mutual spherical harmonics acceleration settings when making acceleration model on " + + nameOfBodyUndergoingAcceleration + "due to " + nameOfBodyExertingAcceleration; + throw std::runtime_error( errorMessage ); + } + else + { + // Get pointer to gravity field of central body and cast to required type. + boost::shared_ptr< SphericalHarmonicsGravityField > sphericalHarmonicsGravityFieldOfBodyExertingAcceleration = + boost::dynamic_pointer_cast< SphericalHarmonicsGravityField >( + bodyExertingAcceleration->getGravityFieldModel( ) ); + boost::shared_ptr< SphericalHarmonicsGravityField > sphericalHarmonicsGravityFieldOfBodyUndergoingAcceleration = + boost::dynamic_pointer_cast< SphericalHarmonicsGravityField >( + bodyUndergoingAcceleration->getGravityFieldModel( ) ); + + if( sphericalHarmonicsGravityFieldOfBodyExertingAcceleration == NULL ) + { + + std::string errorMessage = "Error " + nameOfBodyExertingAcceleration + " does not have a spherical harmonics gravity field " + + "when making mutual spherical harmonics gravity acceleration on " + + nameOfBodyUndergoingAcceleration; + throw std::runtime_error( errorMessage ); + + } + else if( sphericalHarmonicsGravityFieldOfBodyUndergoingAcceleration == NULL ) + { + + std::string errorMessage = "Error " + nameOfBodyUndergoingAcceleration + " does not have a spherical harmonics gravity field " + + "when making mutual spherical harmonics gravity acceleration on " + + nameOfBodyUndergoingAcceleration; + throw std::runtime_error( errorMessage ); + } + else + { + boost::function< double( ) > gravitationalParameterFunction; + + // Create function returning summed gravitational parameter of the two bodies. + if( useCentralBodyFixedFrame == false ) + { + gravitationalParameterFunction = + boost::bind( &SphericalHarmonicsGravityField::getGravitationalParameter, + sphericalHarmonicsGravityFieldOfBodyExertingAcceleration ); + } + else + { + // Create function returning summed gravitational parameter of the two bodies. + boost::function< double( ) > gravitationalParameterOfBodyExertingAcceleration = + boost::bind( &gravitation::GravityFieldModel::getGravitationalParameter, + sphericalHarmonicsGravityFieldOfBodyExertingAcceleration ); + boost::function< double( ) > gravitationalParameterOfBodyUndergoingAcceleration = + boost::bind( &gravitation::GravityFieldModel::getGravitationalParameter, + sphericalHarmonicsGravityFieldOfBodyUndergoingAcceleration ); + gravitationalParameterFunction = + boost::bind( &utilities::sumFunctionReturn< double >, + gravitationalParameterOfBodyExertingAcceleration, + gravitationalParameterOfBodyUndergoingAcceleration ); + } + + // Create acceleration object. + + int maximumDegreeOfUndergoingBody, maximumOrderOfUndergoingBody; + if( !acceleratedBodyIsCentralBody ) + { + maximumDegreeOfUndergoingBody = mutualSphericalHarmonicsSettings->maximumDegreeOfBodyUndergoingAcceleration_; + maximumOrderOfUndergoingBody = mutualSphericalHarmonicsSettings->maximumOrderOfBodyUndergoingAcceleration_; + } + else + { + maximumDegreeOfUndergoingBody = mutualSphericalHarmonicsSettings->maximumDegreeOfCentralBody_; + maximumOrderOfUndergoingBody = mutualSphericalHarmonicsSettings->maximumOrderOfCentralBody_; + } + + accelerationModel = boost::make_shared< MutualSphericalHarmonicsGravitationalAccelerationModel >( + boost::bind( &Body::getPosition, bodyUndergoingAcceleration ), + boost::bind( &Body::getPosition, bodyExertingAcceleration ), + gravitationalParameterFunction, + sphericalHarmonicsGravityFieldOfBodyExertingAcceleration->getReferenceRadius( ), + sphericalHarmonicsGravityFieldOfBodyUndergoingAcceleration->getReferenceRadius( ), + boost::bind( &SphericalHarmonicsGravityField::getCosineCoefficients, + sphericalHarmonicsGravityFieldOfBodyExertingAcceleration, + mutualSphericalHarmonicsSettings->maximumDegreeOfBodyExertingAcceleration_, + mutualSphericalHarmonicsSettings->maximumOrderOfBodyExertingAcceleration_ ), + boost::bind( &SphericalHarmonicsGravityField::getSineCoefficients, + sphericalHarmonicsGravityFieldOfBodyExertingAcceleration, + mutualSphericalHarmonicsSettings->maximumDegreeOfBodyExertingAcceleration_, + mutualSphericalHarmonicsSettings->maximumOrderOfBodyExertingAcceleration_ ), + boost::bind( &SphericalHarmonicsGravityField::getCosineCoefficients, + sphericalHarmonicsGravityFieldOfBodyUndergoingAcceleration, + maximumDegreeOfUndergoingBody, + maximumOrderOfUndergoingBody ), + boost::bind( &SphericalHarmonicsGravityField::getSineCoefficients, + sphericalHarmonicsGravityFieldOfBodyUndergoingAcceleration, + maximumDegreeOfUndergoingBody, + maximumOrderOfUndergoingBody ), + boost::bind( &Body::getCurrentRotationToGlobalFrame, + bodyExertingAcceleration ), + boost::bind( &Body::getCurrentRotationToGlobalFrame, + bodyUndergoingAcceleration ), + useCentralBodyFixedFrame ); + } + } + return accelerationModel; +} + + +//! Function to create a third body central gravity acceleration model. +boost::shared_ptr< gravitation::ThirdBodyCentralGravityAcceleration > +createThirdBodyCentralGravityAccelerationModel( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const boost::shared_ptr< Body > centralBody, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const std::string& nameOfCentralBody ) +{ + // Declare pointer to return object. + boost::shared_ptr< ThirdBodyCentralGravityAcceleration > accelerationModelPointer; + + // Create acceleration object. + accelerationModelPointer = boost::make_shared< ThirdBodyCentralGravityAcceleration >( + boost::dynamic_pointer_cast< CentralGravitationalAccelerationModel3d >( + createCentralGravityAcceleratioModel( bodyUndergoingAcceleration, + bodyExertingAcceleration, + nameOfBodyUndergoingAcceleration, + nameOfBodyExertingAcceleration, 0 ) ), + boost::dynamic_pointer_cast< CentralGravitationalAccelerationModel3d >( + createCentralGravityAcceleratioModel( centralBody, bodyExertingAcceleration, + nameOfCentralBody, + nameOfBodyExertingAcceleration, 0 ) ), nameOfCentralBody ); + + return accelerationModelPointer; +} + +//! Function to create a third body spheric harmonic gravity acceleration model. +boost::shared_ptr< gravitation::ThirdBodySphericalHarmonicsGravitationalAccelerationModel > +createThirdBodySphericalHarmonicGravityAccelerationModel( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const boost::shared_ptr< Body > centralBody, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const std::string& nameOfCentralBody, + const boost::shared_ptr< AccelerationSettings > accelerationSettings ) +{ + using namespace basic_astrodynamics; + + // Declare pointer to return object + boost::shared_ptr< ThirdBodySphericalHarmonicsGravitationalAccelerationModel > accelerationModel; + + // Dynamic cast acceleration settings to required type and check consistency. + boost::shared_ptr< SphericalHarmonicAccelerationSettings > sphericalHarmonicsSettings = + boost::dynamic_pointer_cast< SphericalHarmonicAccelerationSettings >( accelerationSettings ); + if( sphericalHarmonicsSettings == NULL ) + { + std::string errorMessage = "Error, expected spherical harmonics acceleration settings when making acceleration model on " + + nameOfBodyUndergoingAcceleration + " due to " + nameOfBodyExertingAcceleration; + throw std::runtime_error( errorMessage ); + } + else + { + // Get pointer to gravity field of central body and cast to required type. + boost::shared_ptr< SphericalHarmonicsGravityField > sphericalHarmonicsGravityField = + boost::dynamic_pointer_cast< SphericalHarmonicsGravityField >( + bodyExertingAcceleration->getGravityFieldModel( ) ); + if( sphericalHarmonicsGravityField == NULL ) + { + std::string errorMessage = "Error " + nameOfBodyExertingAcceleration + " does not have a spherical harmonics gravity field " + + "when making third body spherical harmonics gravity acceleration on " + + nameOfBodyUndergoingAcceleration; + throw std::runtime_error( errorMessage ); + } + else + { + + accelerationModel = boost::make_shared< ThirdBodySphericalHarmonicsGravitationalAccelerationModel >( + boost::dynamic_pointer_cast< SphericalHarmonicsGravitationalAccelerationModel >( + createSphericalHarmonicsGravityAcceleration( + bodyUndergoingAcceleration, bodyExertingAcceleration, nameOfBodyUndergoingAcceleration, + nameOfBodyExertingAcceleration, sphericalHarmonicsSettings, 0 ) ), + boost::dynamic_pointer_cast< SphericalHarmonicsGravitationalAccelerationModel >( + createSphericalHarmonicsGravityAcceleration( + centralBody, bodyExertingAcceleration, nameOfCentralBody, + nameOfBodyExertingAcceleration, sphericalHarmonicsSettings, 0 ) ), nameOfCentralBody ); + } + } + return accelerationModel; +} + +//! Function to create a third body mutual spheric harmonic gravity acceleration model. +boost::shared_ptr< gravitation::ThirdBodyMutualSphericalHarmonicsGravitationalAccelerationModel > +createThirdBodyMutualSphericalHarmonicGravityAccelerationModel( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const boost::shared_ptr< Body > centralBody, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const std::string& nameOfCentralBody, + const boost::shared_ptr< AccelerationSettings > accelerationSettings ) +{ + // Declare pointer to return object + boost::shared_ptr< ThirdBodyMutualSphericalHarmonicsGravitationalAccelerationModel > accelerationModel; + + // Dynamic cast acceleration settings to required type and check consistency. + boost::shared_ptr< MutualSphericalHarmonicAccelerationSettings > mutualSphericalHarmonicsSettings = + boost::dynamic_pointer_cast< MutualSphericalHarmonicAccelerationSettings >( accelerationSettings ); + if( mutualSphericalHarmonicsSettings == NULL ) + { + + std::string errorMessage = "Error, expected mutual spherical harmonics acceleration settings when making acceleration model on " + + nameOfBodyUndergoingAcceleration + + " due to " + nameOfBodyExertingAcceleration; + throw std::runtime_error( errorMessage ); + } + else + { + // Get pointer to gravity field of central body and cast to required type. + boost::shared_ptr< SphericalHarmonicsGravityField > sphericalHarmonicsGravityFieldOfBodyExertingAcceleration = + boost::dynamic_pointer_cast< SphericalHarmonicsGravityField >( + bodyExertingAcceleration->getGravityFieldModel( ) ); + boost::shared_ptr< SphericalHarmonicsGravityField > sphericalHarmonicsGravityFieldOfBodyUndergoingAcceleration = + boost::dynamic_pointer_cast< SphericalHarmonicsGravityField >( + bodyUndergoingAcceleration->getGravityFieldModel( ) ); + boost::shared_ptr< SphericalHarmonicsGravityField > sphericalHarmonicsGravityFieldOfCentralBody = + boost::dynamic_pointer_cast< SphericalHarmonicsGravityField >( + centralBody->getGravityFieldModel( ) ); + + if( sphericalHarmonicsGravityFieldOfBodyExertingAcceleration == NULL ) + { + std::string errorMessage = "Error " + nameOfBodyExertingAcceleration + " does not have a spherical harmonics gravity field " + + "when making mutual spherical harmonics gravity acceleration on " + + nameOfBodyUndergoingAcceleration; + throw std::runtime_error( errorMessage ); + } + else if( sphericalHarmonicsGravityFieldOfBodyUndergoingAcceleration == NULL ) + { + std::string errorMessage = "Error " + nameOfBodyUndergoingAcceleration + " does not have a spherical harmonics gravity field " + + "when making mutual spherical harmonics gravity acceleration on " + + nameOfBodyUndergoingAcceleration; + throw std::runtime_error( errorMessage ); + } + else if( sphericalHarmonicsGravityFieldOfCentralBody == NULL ) + { + std::string errorMessage = "Error " + nameOfCentralBody + " does not have a spherical harmonics gravity field " + + "when making mutual spherical harmonics gravity acceleration on " + + nameOfBodyUndergoingAcceleration; + throw std::runtime_error( errorMessage ); + } + else + { + boost::shared_ptr< MutualSphericalHarmonicAccelerationSettings > accelerationSettingsForCentralBodyAcceleration = + boost::make_shared< MutualSphericalHarmonicAccelerationSettings >( + mutualSphericalHarmonicsSettings->maximumDegreeOfBodyExertingAcceleration_, + mutualSphericalHarmonicsSettings->maximumOrderOfBodyExertingAcceleration_, + mutualSphericalHarmonicsSettings->maximumDegreeOfCentralBody_, + mutualSphericalHarmonicsSettings->maximumOrderOfCentralBody_ ); + accelerationModel = boost::make_shared< ThirdBodyMutualSphericalHarmonicsGravitationalAccelerationModel >( + boost::dynamic_pointer_cast< MutualSphericalHarmonicsGravitationalAccelerationModel >( + createMutualSphericalHarmonicsGravityAcceleration( + bodyUndergoingAcceleration, bodyExertingAcceleration, nameOfBodyUndergoingAcceleration, + nameOfBodyExertingAcceleration, mutualSphericalHarmonicsSettings, 0, 0 ) ), + boost::dynamic_pointer_cast< MutualSphericalHarmonicsGravitationalAccelerationModel >( + createMutualSphericalHarmonicsGravityAcceleration( + centralBody, bodyExertingAcceleration, nameOfCentralBody, + nameOfBodyExertingAcceleration, accelerationSettingsForCentralBodyAcceleration, 0, 1 ) ), + nameOfCentralBody ); + } + } + return accelerationModel; +} + +//! Function to create an aerodynamic acceleration model. +boost::shared_ptr< aerodynamics::AerodynamicAcceleration > createAerodynamicAcceleratioModel( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration ) +{ + // Check existence of required environment models + if( bodyUndergoingAcceleration->getAerodynamicCoefficientInterface( ) == NULL ) + { + throw std::runtime_error( "Error when making aerodynamic acceleration, body " + + nameOfBodyUndergoingAcceleration + + "has no aerodynamic coefficients." ); + } + + if( bodyExertingAcceleration->getAtmosphereModel( ) == NULL ) + { + throw std::runtime_error( "Error when making aerodynamic acceleration, central body " + + nameOfBodyExertingAcceleration + " has no atmosphere model."); + } + + if( bodyExertingAcceleration->getShapeModel( ) == NULL ) + { + throw std::runtime_error( "Error when making aerodynamic acceleration, central body " + + nameOfBodyExertingAcceleration + " has no shape model." ); + } + + // Retrieve flight conditions; create object if not yet extant. + boost::shared_ptr< FlightConditions > bodyFlightConditions = + bodyUndergoingAcceleration->getFlightConditions( ); + + if( bodyFlightConditions == NULL ) + { + bodyUndergoingAcceleration->setFlightConditions( + createFlightConditions( bodyUndergoingAcceleration, + bodyExertingAcceleration, + nameOfBodyUndergoingAcceleration, + nameOfBodyExertingAcceleration ) ); + bodyFlightConditions = bodyUndergoingAcceleration->getFlightConditions( ); + } + + // Retrieve frame in which aerodynamic coefficients are defined. + boost::shared_ptr< aerodynamics::AerodynamicCoefficientInterface > aerodynamicCoefficients = + bodyUndergoingAcceleration->getAerodynamicCoefficientInterface( ); + reference_frames::AerodynamicsReferenceFrames accelerationFrame; + if( aerodynamicCoefficients->getAreCoefficientsInAerodynamicFrame( ) ) + { + accelerationFrame = reference_frames::aerodynamic_frame; + } + else + { + accelerationFrame = reference_frames::body_frame; + } + + // Create function to transform from frame of aerodynamic coefficienrs to that of propagation. + boost::function< Eigen::Vector3d( const Eigen::Vector3d& ) > toPropagationFrameTransformation; + toPropagationFrameTransformation = + reference_frames::getAerodynamicForceTransformationFunction( + bodyFlightConditions->getAerodynamicAngleCalculator( ), + accelerationFrame, + boost::bind( &Body::getCurrentRotationToGlobalFrame, bodyExertingAcceleration ), + reference_frames::inertial_frame ); + + + boost::function< Eigen::Vector3d( ) > coefficientFunction = + boost::bind( &AerodynamicCoefficientInterface::getCurrentForceCoefficients, + aerodynamicCoefficients ); + boost::function< Eigen::Vector3d( ) > coefficientInPropagationFrameFunction = + boost::bind( &reference_frames::transformVectorFunctionFromVectorFunctions, + coefficientFunction, toPropagationFrameTransformation ); + + // Create acceleration model. + return boost::make_shared< AerodynamicAcceleration >( + coefficientInPropagationFrameFunction, + boost::bind( &FlightConditions::getCurrentDensity, bodyFlightConditions ), + boost::bind( &FlightConditions::getCurrentAirspeed, bodyFlightConditions ), + boost::bind( &Body::getBodyMass, bodyUndergoingAcceleration ), + boost::bind( &AerodynamicCoefficientInterface::getReferenceArea, + aerodynamicCoefficients ), + aerodynamicCoefficients->getAreCoefficientsInNegativeAxisDirection( ) ); +} + +//! Function to create a cannonball radiation pressure acceleration model. +boost::shared_ptr< CannonBallRadiationPressureAcceleration > +createCannonballRadiationPressureAcceleratioModel( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration ) +{ + // Retrieve radiation pressure interface + if( bodyUndergoingAcceleration->getRadiationPressureInterfaces( ).count( + nameOfBodyExertingAcceleration ) == 0 ) + { + throw std::runtime_error( + "Error when making radiation pressure, no radiation pressure interface found in " + + nameOfBodyUndergoingAcceleration + + " for body " + nameOfBodyExertingAcceleration ); + } + boost::shared_ptr< RadiationPressureInterface > radiationPressureInterface = + bodyUndergoingAcceleration->getRadiationPressureInterfaces( ).at( + nameOfBodyExertingAcceleration ); + + // Create acceleration model. + return boost::make_shared< CannonBallRadiationPressureAcceleration >( + boost::bind( &Body::getPosition, bodyExertingAcceleration ), + boost::bind( &Body::getPosition, bodyUndergoingAcceleration ), + boost::bind( &RadiationPressureInterface::getCurrentRadiationPressure, radiationPressureInterface ), + boost::bind( &RadiationPressureInterface::getRadiationPressureCoefficient, radiationPressureInterface ), + boost::bind( &RadiationPressureInterface::getArea, radiationPressureInterface ), + boost::bind( &Body::getBodyMass, bodyUndergoingAcceleration ) ); + +} + +//! Function to create a thrust acceleration model. +boost::shared_ptr< propulsion::ThrustAcceleration > +createThrustAcceleratioModel( + const boost::shared_ptr< AccelerationSettings > accelerationSettings, + const NamedBodyMap& bodyMap, + const std::string& nameOfBodyUndergoingThrust ) +{ + // Check input consistency + boost::shared_ptr< ThrustAccelerationSettings > thrustAccelerationSettings = + boost::dynamic_pointer_cast< ThrustAccelerationSettings >( accelerationSettings ); + if( thrustAccelerationSettings == NULL ) + { + throw std::runtime_error( "Error when creating thrust acceleration, input is inconsistent" ); + } + + std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > magnitudeUpdateSettings; + std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > directionUpdateSettings; + + // Create thrust direction model. + boost::shared_ptr< propulsion::BodyFixedForceDirectionGuidance > thrustDirectionGuidance = createThrustGuidanceModel( + thrustAccelerationSettings->thrustDirectionGuidanceSettings_, bodyMap, nameOfBodyUndergoingThrust, + getBodyFixedThrustDirection( thrustAccelerationSettings->thrustMagnitudeSettings_, bodyMap, + nameOfBodyUndergoingThrust ), magnitudeUpdateSettings ); + + // Create thrust magnitude model + boost::shared_ptr< propulsion::ThrustMagnitudeWrapper > thrustMagnitude = createThrustMagnitudeWrapper( + thrustAccelerationSettings->thrustMagnitudeSettings_, bodyMap, nameOfBodyUndergoingThrust, + directionUpdateSettings ); + + // Add required updates of environemt models. + std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > totalUpdateSettings; + propagators::addEnvironmentUpdates( totalUpdateSettings, magnitudeUpdateSettings ); + propagators::addEnvironmentUpdates( totalUpdateSettings, directionUpdateSettings ); + + // Set DependentOrientationCalculator for body if required. + if( !( thrustAccelerationSettings->thrustDirectionGuidanceSettings_->thrustDirectionType_ == + thrust_direction_from_existing_body_orientation ) ) + { + bodyMap.at( nameOfBodyUndergoingThrust )->setDependentOrientationCalculator( thrustDirectionGuidance ); + } + + // Create and return thrust acceleration object. + boost::function< void( const double ) > updateFunction = + boost::bind( &updateThrustMagnitudeAndDirection, thrustMagnitude, thrustDirectionGuidance, _1 ); + boost::function< void( const double ) > timeResetFunction = + boost::bind( &resetThrustMagnitudeAndDirectionTime, thrustMagnitude, thrustDirectionGuidance, _1 ); + return boost::make_shared< propulsion::ThrustAcceleration >( + boost::bind( &propulsion::ThrustMagnitudeWrapper::getCurrentThrustMagnitude, thrustMagnitude ), + boost::bind( &propulsion::BodyFixedForceDirectionGuidance ::getCurrentForceDirectionInPropagationFrame, thrustDirectionGuidance ), + boost::bind( &Body::getBodyMass, bodyMap.at( nameOfBodyUndergoingThrust ) ), + boost::bind( &propulsion::ThrustMagnitudeWrapper::getCurrentMassRate, thrustMagnitude ), + thrustAccelerationSettings->thrustMagnitudeSettings_->thrustOriginId_, + updateFunction, timeResetFunction, totalUpdateSettings ); +} + + +//! Function to create acceleration model object. +boost::shared_ptr< AccelerationModel< Eigen::Vector3d > > createAccelerationModel( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const boost::shared_ptr< AccelerationSettings > accelerationSettings, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const boost::shared_ptr< Body > centralBody, + const std::string& nameOfCentralBody, + const NamedBodyMap& bodyMap ) +{ + // Declare pointer to return object. + boost::shared_ptr< AccelerationModel< Eigen::Vector3d > > accelerationModelPointer; + + // Switch to call correct acceleration model type factory function. + switch( accelerationSettings->accelerationType_ ) + { + case central_gravity: + accelerationModelPointer = createGravitationalAccelerationModel( + bodyUndergoingAcceleration, bodyExertingAcceleration, accelerationSettings, + nameOfBodyUndergoingAcceleration, nameOfBodyExertingAcceleration, + centralBody, nameOfCentralBody ); + break; + case spherical_harmonic_gravity: + accelerationModelPointer = createGravitationalAccelerationModel( + bodyUndergoingAcceleration, bodyExertingAcceleration, accelerationSettings, + nameOfBodyUndergoingAcceleration, nameOfBodyExertingAcceleration, + centralBody, nameOfCentralBody ); + break; + case mutual_spherical_harmonic_gravity: + accelerationModelPointer = createGravitationalAccelerationModel( + bodyUndergoingAcceleration, bodyExertingAcceleration, accelerationSettings, + nameOfBodyUndergoingAcceleration, nameOfBodyExertingAcceleration, + centralBody, nameOfCentralBody ); + break; + case aerodynamic: + accelerationModelPointer = createAerodynamicAcceleratioModel( + bodyUndergoingAcceleration, + bodyExertingAcceleration, + nameOfBodyUndergoingAcceleration, + nameOfBodyExertingAcceleration ); + break; + case cannon_ball_radiation_pressure: + accelerationModelPointer = createCannonballRadiationPressureAcceleratioModel( + bodyUndergoingAcceleration, + bodyExertingAcceleration, + nameOfBodyUndergoingAcceleration, + nameOfBodyExertingAcceleration ); + break; + case thrust_acceleration: + accelerationModelPointer = createThrustAcceleratioModel( + accelerationSettings, bodyMap, + nameOfBodyUndergoingAcceleration ); + break; + default: + throw std::runtime_error( + std::string( "Error, acceleration model ") + + boost::lexical_cast< std::string >( accelerationSettings->accelerationType_ ) + + " not recognized when making acceleration model of" + + nameOfBodyExertingAcceleration + " on " + + nameOfBodyUndergoingAcceleration ); + break; + } + return accelerationModelPointer; +} + +//! Function to put SelectedAccelerationMap in correct order, to ensure correct model creation +SelectedAccelerationMap orderSelectedAccelerationMap( const SelectedAccelerationMap& selectedAccelerationsPerBody ) +{ + // Declare map of acceleration models acting on current body. + SelectedAccelerationMap orderedAccelerationsPerBody; + + // Iterate over all bodies which are undergoing acceleration + for( SelectedAccelerationMap::const_iterator bodyIterator = + selectedAccelerationsPerBody.begin( ); bodyIterator != selectedAccelerationsPerBody.end( ); + bodyIterator++ ) + { + // Retrieve name of body undergoing acceleration. + std::string bodyUndergoingAcceleration = bodyIterator->first; + + // Retrieve list of required acceleration model types and bodies exerting accelerationd on + // current body. + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > + accelerationsForBody = bodyIterator->second; + + // Iterate over all bodies exerting an acceleration + for( std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > >:: + iterator body2Iterator = accelerationsForBody.begin( ); + body2Iterator != accelerationsForBody.end( ); body2Iterator++ ) + { + // Retrieve name of body exerting acceleration. + std::string bodyExertingAcceleration = body2Iterator->first; + + // Retrieve list of accelerations due to current body. + std::vector< boost::shared_ptr< AccelerationSettings > > accelerationList = + body2Iterator->second; + + // Retrieve indices of all acceleration anf thrust models. + std::vector< int > aerodynamicAccelerationIndex; + std::vector< int > thrustAccelerationIndices; + for( unsigned int i = 0; i < accelerationList.size( ); i++ ) + { + if( accelerationList.at( i )->accelerationType_ == basic_astrodynamics::thrust_acceleration ) + { + thrustAccelerationIndices.push_back( i ); + } + else if( accelerationList.at( i )->accelerationType_ == basic_astrodynamics::aerodynamic ) + { + aerodynamicAccelerationIndex.push_back( i ); + } + } + + std::vector< boost::shared_ptr< AccelerationSettings > > orderedAccelerationList = accelerationList; + + // Put aerodynamic and thrust accelerations in correct order (ensure aerodynamic acceleration created first). + if( ( thrustAccelerationIndices.size( ) > 0 ) && ( aerodynamicAccelerationIndex.size( ) > 0 ) ) + { + if( aerodynamicAccelerationIndex.at( aerodynamicAccelerationIndex.size( ) - 1 ) > + thrustAccelerationIndices.at( 0 ) ) + { + if( ( aerodynamicAccelerationIndex.size( ) == 1 ) ) + { + std::iter_swap( orderedAccelerationList.begin( ) + aerodynamicAccelerationIndex.at( 0 ), + orderedAccelerationList.begin( ) + thrustAccelerationIndices.at( + thrustAccelerationIndices.size( ) - 1 ) ); + } + else + { + throw std::runtime_error( + "Error when ordering accelerations, cannot yet handle multple aerodynamic and thrust accelerations" ); + } + } + } + + orderedAccelerationsPerBody[ bodyUndergoingAcceleration ][ bodyExertingAcceleration ] = orderedAccelerationList; + } + } + + return orderedAccelerationsPerBody; +} + +} // namespace simulation_setup + +} // namespace tudat diff --git a/Tudat/SimulationSetup/PropagationSetup/createAccelerationModels.h b/Tudat/SimulationSetup/PropagationSetup/createAccelerationModels.h new file mode 100644 index 0000000000..ef5dfdfdba --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/createAccelerationModels.h @@ -0,0 +1,361 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_CREATEACCELERATIONMODELS_H +#define TUDAT_CREATEACCELERATIONMODELS_H + +#include +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" +#include "Tudat/Astrodynamics/Gravitation/centralGravityModel.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" +#include "Tudat/Astrodynamics/Aerodynamics/aerodynamicAcceleration.h" +#include "Tudat/SimulationSetup/PropagationSetup/accelerationSettings.h" +#include "Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.h" +#include "Tudat/Astrodynamics/Gravitation/thirdBodyPerturbation.h" + +namespace tudat +{ + +namespace simulation_setup +{ + +//! Function to create a direct (i.e. not third-body) gravitational acceleration (of any type) +/*! + * Function to create a direct (i.e. not third-body) gravitational acceleration of any type (i.e. point mass, + * spherical harmonic, mutual spherical harmonic). + * \param bodyUndergoingAcceleration Pointer to object of body that is being accelerated. + * \param bodyExertingAcceleration Pointer to object of body that is exerting the gravitational acceleration. + * \param nameOfBodyUndergoingAcceleration Name of body that is being accelerated. + * \param nameOfBodyExertingAcceleration Name of body that is exerting the gravitational acceleration. + * \param accelerationSettings Settings object for the gravitational acceleration. + * \param nameOfCentralBody Name of central body in frame centered at which acceleration is to + * be calculated. + * \param isCentralBody Boolean defining whether the body undergoing the acceleration is the central body for a + * third-body acceleration, of which the return object of this funciton is one of the sub-parts. Boolean is + * only used when creating mutual spherical harmonic acceleration, to ensure teh correct usage of the acceleration + * settings. + * \return Direct gravitational acceleration model of requested settings. + */ +boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > createDirectGravitationalAcceleration( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const boost::shared_ptr< AccelerationSettings > accelerationSettings, + const std::string& nameOfCentralBody = "", + const bool isCentralBody = 0 ); + +//! Function to create a third-body gravitational acceleration (of any type) +/*! + * Function to create a direct third-body gravitational acceleration of any type (i.e. point mass, + * spherical harmonic, mutual spherical harmonic). + * \param bodyUndergoingAcceleration Pointer to object of body that is being accelerated. + * \param bodyExertingAcceleration Pointer to object of body that is exerting the gravitational acceleration. + * \param centralBody Pointer to central body in frame centered at which acceleration is to be calculated. + * \param nameOfBodyUndergoingAcceleration Name of body that is being accelerated. + * \param nameOfBodyExertingAcceleration Name of body that is exerting the gravitational acceleration. + * \param nameOfCentralBody Name of central body in frame centered at which acceleration is to + * be calculated. + * \param accelerationSettings Settings object for the gravitational acceleration. + * \return Third-body gravitational acceleration model of requested settings. + */ +boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > createThirdBodyGravitationalAcceleration( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const boost::shared_ptr< Body > centralBody, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const std::string& nameOfCentralBody, + const boost::shared_ptr< AccelerationSettings > accelerationSettings ); + +//! Function to create gravitational acceleration (of any type) +/*! + * Function to create a third-body or direct gravitational acceleration of any type (i.e. point mass, + * spherical harmonic, mutual spherical harmonic). + * \param bodyUndergoingAcceleration Pointer to object of body that is being accelerated. + * \param bodyExertingAcceleration Pointer to object of body that is exerting the gravitational acceleration. + * \param accelerationSettings Settings object for the gravitational acceleration. + * \param centralBody Pointer to central body in frame centered at which acceleration is to be calculated. + * \param nameOfBodyUndergoingAcceleration Name of body that is being accelerated. + * \param nameOfBodyExertingAcceleration Name of body that is exerting the gravitational acceleration. + * \param nameOfCentralBody Name of central body in frame centered at which acceleration is to + * be calculated. + * \param accelerationSettings Settings object for the gravitational acceleration. + * \return Gravitational acceleration model of requested settings. + */ +boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > createGravitationalAccelerationModel( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const boost::shared_ptr< AccelerationSettings > accelerationSettings, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const boost::shared_ptr< Body > centralBody, + const std::string& nameOfCentralBody ); + +//! Function to create central gravity acceleration model. +/*! + * Function to create central gravity acceleration model from bodies exerting and undergoing + * acceleration. + * \param bodyUndergoingAcceleration Pointer to object of body that is being accelerated. + * \param bodyExertingAcceleration Pointer to object of body that is exerting the central gravity + * acceleration. + * \param nameOfBodyUndergoingAcceleration Name of body that is being accelerated. + * \param nameOfBodyExertingAcceleration Name of body that is exerting the central gravity + * acceleration. + * \param useCentralBodyFixedFrame Boolean setting whether the central attraction of body + * undergoing acceleration on body exerting acceleration is to be included in acceleration model. + * Should be set to true in case the body undergoing acceleration is a celestial body + * (with gravity field) and integration is performed in the frame centered at the body exerting + * acceleration. + * \return Central gravity acceleration model pointer. + */ +boost::shared_ptr< gravitation::CentralGravitationalAccelerationModel3d > +createCentralGravityAcceleratioModel( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const bool useCentralBodyFixedFrame ); + +//! Function to create spherical harmonic gravity acceleration model. +/*! + * Function to create spherical harmonic gravity acceleration model from bodies exerting and + * undergoing acceleration. + * \param bodyUndergoingAcceleration Pointer to object of body that is being accelerated. + * \param bodyExertingAcceleration Pointer to object of body that is exerting the spherical + * harmonic gravity acceleration. + * \param nameOfBodyUndergoingAcceleration Name of body that is being accelerated. + * \param nameOfBodyExertingAcceleration Name of body that is exerting the spherical harmonic + * gravity acceleration. + * \param accelerationSettings Settings for acceleration model that is to be created (should + * be of derived type associated with spherical harmonic acceleration. + * \param useCentralBodyFixedFrame Boolean setting whether the central attraction of body + * undergoing acceleration on body exerting acceleration is to be included in acceleration model. + * Should be set to true in case the body undergoing acceleration is a celestial body + * (with gravity field) and integration is performed in the frame centered at the body exerting + * acceleration. + * \return Spherical harmonic gravity acceleration model pointer. + */ +boost::shared_ptr< gravitation::SphericalHarmonicsGravitationalAccelerationModel > +createSphericalHarmonicsGravityAcceleration( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const boost::shared_ptr< AccelerationSettings > accelerationSettings, + const bool useCentralBodyFixedFrame ); + +//! Function to create mutual spherical harmonic gravity acceleration model. +/*! + * Function to create mutual spherical harmonic gravity acceleration model from bodies exerting and + * undergoing acceleration. + * \param bodyUndergoingAcceleration Pointer to object of body that is being accelerated. + * \param bodyExertingAcceleration Pointer to object of body that is exerting the mutual spherical + * harmonic gravity acceleration. + * \param nameOfBodyUndergoingAcceleration Name of body that is being accelerated. + * \param nameOfBodyExertingAcceleration Name of body that is exerting the mutual spherical harmonic + * gravity acceleration. + * \param accelerationSettings Settings for acceleration model that is to be created (should + * be of derived type associated with mutual spherical harmonic acceleration). + * \param useCentralBodyFixedFrame Boolean setting whether the central attraction of body + * undergoing acceleration on body exerting acceleration is to be included in acceleration model. + * Should be set to true in case the body undergoing acceleration is a celestial body + * (with gravity field) and integration is performed in the frame centered at the body exerting + * acceleration. + * \param acceleratedBodyIsCentralBody Boolean defining whether the body undergoing the acceleration is the central body + * for a third-body acceleration, of which the return object of this funciton is one of the sub-parts. + * \return Mutual spherical harmonic gravity acceleration model pointer. + */ +boost::shared_ptr< gravitation::MutualSphericalHarmonicsGravitationalAccelerationModel > +createMutualSphericalHarmonicsGravityAcceleration( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const boost::shared_ptr< AccelerationSettings > accelerationSettings, + const bool useCentralBodyFixedFrame, + const bool acceleratedBodyIsCentralBody ); + +//! Function to create a third body central gravity acceleration model. +/*! + * Function to create a third body central gravity acceleration model from bodies exerting and + * undergoing acceleration, as well as the central body, w.r.t. which the integration is to be + * performed. + * \param bodyUndergoingAcceleration Pointer to object of body that is being accelerated. + * \param bodyExertingAcceleration Pointer to object of body that is exerting the acceleration. + * \param centralBody Pointer to central body in frame centered at which acceleration is to be + * calculated. + * \param nameOfBodyUndergoingAcceleration Name of object of body that is being accelerated. + * \param nameOfBodyExertingAcceleration Name of object of body that is exerting the central + * gravity acceleration. + * \param nameOfCentralBody Name of central body in frame cenetered at which acceleration is to + * be calculated. + * \return Pointer to object for calculating central gravity acceleration between bodies. + */ +boost::shared_ptr< gravitation::ThirdBodyCentralGravityAcceleration > +createThirdBodyCentralGravityAccelerationModel( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const boost::shared_ptr< Body > centralBody, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const std::string& nameOfCentralBody ); + +//! Function to create a third body spheric harmonic gravity acceleration model. +/*! + * Function to create a third body spheric harmonic gravity acceleration model from bodies exerting and + * undergoing acceleration, as well as the central body, w.r.t. which the integration is to be + * performed. + * \param bodyUndergoingAcceleration Pointer to object of body that is being accelerated. + * \param bodyExertingAcceleration Pointer to object of body that is exerting the acceleration. + * \param centralBody Pointer to central body in frame centered at which acceleration is to be + * calculated. + * \param nameOfBodyUndergoingAcceleration Name of object of body that is being accelerated. + * \param nameOfBodyExertingAcceleration Name of object of body that is exerting the central + * gravity acceleration. + * \param nameOfCentralBody Name of central body in frame cenetered at which acceleration is to + * be calculated. + * \param accelerationSettings Settings for acceleration model that is to be created (should + * be of derived type associated with spherical harmonic acceleration). + * \return Pointer to object for calculating third-body spheric harmonic gravity acceleration between bodies. + */ +boost::shared_ptr< gravitation::ThirdBodySphericalHarmonicsGravitationalAccelerationModel > +createThirdBodySphericalHarmonicGravityAccelerationModel( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const boost::shared_ptr< Body > centralBody, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const std::string& nameOfCentralBody, + const boost::shared_ptr< AccelerationSettings > accelerationSettings ); + +//! Function to create a third body mutual spheric harmonic gravity acceleration model. +/*! + * Function to create a third body mutual spheric harmonic gravity acceleration model from bodies exerting and + * undergoing acceleration, as well as the central body, w.r.t. which the integration is to be + * performed. + * \param bodyUndergoingAcceleration Pointer to object of body that is being accelerated. + * \param bodyExertingAcceleration Pointer to object of body that is exerting the acceleration. + * \param centralBody Pointer to central body in frame centered at which acceleration is to be + * calculated. + * \param nameOfBodyUndergoingAcceleration Name of object of body that is being accelerated. + * \param nameOfBodyExertingAcceleration Name of object of body that is exerting the central + * gravity acceleration. + * \param nameOfCentralBody Name of central body in frame cenetered at which acceleration is to + * be calculated. + * \param accelerationSettings Settings for acceleration model that is to be created (should + * be of derived type associated with mutual spherical harmonic acceleration). + * \return Pointer to object for calculating third-body mutual spheric harmonic gravity acceleration between bodies. + */ +boost::shared_ptr< gravitation::ThirdBodyMutualSphericalHarmonicsGravitationalAccelerationModel > +createThirdBodyMutualSphericalHarmonicGravityAccelerationModel( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const boost::shared_ptr< Body > centralBody, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const std::string& nameOfCentralBody, + const boost::shared_ptr< AccelerationSettings > accelerationSettings ); + +//! Function to create an aerodynamic acceleration model. +/*! + * Function to create an aerodynamic acceleration model, automatically creates all required + * links to environment models, vehicle properies and frame conversions + * \param bodyUndergoingAcceleration Pointer to object of body that is being accelerated. + * \param bodyExertingAcceleration Pointer to object of body that is exerting the acceleration, + * i.e. body with the atmosphere through which the accelerated body is flying. + * \param nameOfBodyUndergoingAcceleration Name of object of body that is being accelerated. + * \param nameOfBodyExertingAcceleration Name of object of body that is exerting the acceleration. + * \return Pointer to object for calculating aerodynamic acceleration. + */ +boost::shared_ptr< aerodynamics::AerodynamicAcceleration > +createAerodynamicAcceleratioModel( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration ); + +//! Function to create a cannonball radiation pressure acceleration model. +/*! + * Function to create a cannonball radiation pressure automatically creates all required + * links to environment models, vehicle properies and frame conversions + * \param bodyUndergoingAcceleration Pointer to object of body that is being accelerated. + * \param bodyExertingAcceleration Pointer to object of body that is exerting the acceleration, + * i.e. body emitting the radiation. + * \param nameOfBodyUndergoingAcceleration Name of object of body that is being accelerated. + * \param nameOfBodyExertingAcceleration Name of object of body that is exerting the acceleration. + * \return Pointer to object for calculating cannonball radiation pressures acceleration. + */ +boost::shared_ptr< electro_magnetism::CannonBallRadiationPressureAcceleration > +createCannonballRadiationPressureAcceleratioModel( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration ); + +//! Function to create a thrust acceleration model. +/*! + * Function to create a thrust acceleration model. Creates all required + * links to environment models, vehicle properies and frame conversions. + * \param accelerationSettings Settings of thrust acceleration model. + * \param bodyMap List of pointers to bodies required for the creation of the acceleration model + * objects. + * \param nameOfBodyUndergoingThrust Name of body that is undergoing the thrust acceleration + * \return Pointer to object for calculating thrust acceleration. + */ +boost::shared_ptr< propulsion::ThrustAcceleration > +createThrustAcceleratioModel( + const boost::shared_ptr< AccelerationSettings > accelerationSettings, + const NamedBodyMap& bodyMap, + const std::string& nameOfBodyUndergoingThrust ); + +//! Function to create acceleration model object. +/*! + * Function to create acceleration model object. + * Type of requested model is checked and corresponding factory function is called. + * \param bodyUndergoingAcceleration Pointer to object of body that is being accelerated. + * \param bodyExertingAcceleration Pointer to object of body that is exerting acceleration, + * \param accelerationSettings Settings for acceleration model that is to be created. + * \param nameOfBodyUndergoingAcceleration Name of object of body that is being accelerated. + * \param nameOfBodyExertingAcceleration Name of object of body that is exerting the acceleration. + * \param centralBody Pointer to central body in frame centered at which acceleration is to be + * calculated (optional, only relevant for third body accelerations). + * \param nameOfCentralBody Name of central body in frame cenetered at which acceleration is to + * be calculated (optional, only relevant for third body accelerations). + * \param bodyMap List of pointers to bodies required for the creation of the acceleration model + * objects. + * \return Acceleration model pointer. + */ +boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > +createAccelerationModel( + const boost::shared_ptr< Body > bodyUndergoingAcceleration, + const boost::shared_ptr< Body > bodyExertingAcceleration, + const boost::shared_ptr< AccelerationSettings > accelerationSettings, + const std::string& nameOfBodyUndergoingAcceleration, + const std::string& nameOfBodyExertingAcceleration, + const boost::shared_ptr< Body > centralBody = boost::shared_ptr< Body >( ), + const std::string& nameOfCentralBody = "", + const NamedBodyMap& bodyMap = NamedBodyMap( ) ); + +//! Function to put SelectedAccelerationMap in correct order, to ensure correct model creation +/*! + * Function to put SelectedAccelerationMap in correct order, to ensure correct model creation + * \param selectedAccelerationPerBody List of acceleration settings per body. + * \return selectedAccelerationPerBody, put in order to ensure correct model creation. + */ +SelectedAccelerationMap orderSelectedAccelerationMap( const SelectedAccelerationMap& selectedAccelerationPerBody ); + +} // namespace simulation_setup + +} // namespace tudat + +#endif // TUDAT_CREATEACCELERATIONMODELS_H diff --git a/Tudat/Astrodynamics/Propagators/createEnvironmentUpdater.cpp b/Tudat/SimulationSetup/PropagationSetup/createEnvironmentUpdater.cpp similarity index 68% rename from Tudat/Astrodynamics/Propagators/createEnvironmentUpdater.cpp rename to Tudat/SimulationSetup/PropagationSetup/createEnvironmentUpdater.cpp index 92a723b74c..6f8b52927e 100644 --- a/Tudat/Astrodynamics/Propagators/createEnvironmentUpdater.cpp +++ b/Tudat/SimulationSetup/PropagationSetup/createEnvironmentUpdater.cpp @@ -9,7 +9,7 @@ */ #include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h" -#include "Tudat/Astrodynamics/Propagators/createEnvironmentUpdater.h" +#include "Tudat/SimulationSetup/PropagationSetup/createEnvironmentUpdater.h" namespace tudat { @@ -60,11 +60,12 @@ void checkValidityOfRequiredEnvironmentUpdates( } case body_rotational_state_update: { - if( bodyMap.at( updateIterator->second.at( i ) )-> - getRotationalEphemeris( ) == NULL ) + if( ( bodyMap.at( updateIterator->second.at( i ) )-> + getRotationalEphemeris( ) == NULL ) && + ( bodyMap.at( updateIterator->second.at( i ) )->getDependentOrientationCalculator( ) == NULL ) ) { throw std::runtime_error( - "Error when making environment model update settings, could not find rotational ephemeris of body " + "Error when making environment model update settings, could not find rotational ephemeris or dependent orientation calculator of body " + boost::lexical_cast< std::string>( updateIterator->second.at( i ) ) ); } break; @@ -123,49 +124,6 @@ void checkValidityOfRequiredEnvironmentUpdates( } } -//! Function to extend existing list of required environment update types -void addEnvironmentUpdates( std::map< propagators::EnvironmentModelsToUpdate, - std::vector< std::string > >& environmentUpdateList, - const std::map< propagators::EnvironmentModelsToUpdate, - std::vector< std::string > > updatesToAdd ) -{ - // Iterate over all environment update types. - for( std::map< propagators::EnvironmentModelsToUpdate, - std::vector< std::string > >::const_iterator - environmentUpdateIterator = updatesToAdd.begin( ); - environmentUpdateIterator != updatesToAdd.end( ); environmentUpdateIterator++ ) - { - bool addCurrentUpdate = 0; - - // Iterate over all updated bodies. - for( unsigned int i = 0; i < environmentUpdateIterator->second.size( ); i++ ) - { - addCurrentUpdate = 0; - - // Check if current update type exists. - if( environmentUpdateList.count( environmentUpdateIterator->first ) == 0 ) - { - addCurrentUpdate = 1; - } - // Check if current body exists for update type. - else if( std::find( environmentUpdateList.at( environmentUpdateIterator->first ).begin( ), - environmentUpdateList.at( environmentUpdateIterator->first ).end( ), - environmentUpdateIterator->second.at( i ) ) == - environmentUpdateList.at( environmentUpdateIterator->first ).end( ) ) - { - addCurrentUpdate = 1; - } - - // Add update type if required. - if( addCurrentUpdate ) - { - environmentUpdateList[ environmentUpdateIterator->first ].push_back( - environmentUpdateIterator->second.at( i ) ); - } - } - } -} - //! Get list of required environment model update settings from translational acceleration models. std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > createTranslationalEquationsOfMotionEnvironmentUpdaterSettings( @@ -219,8 +177,11 @@ createTranslationalEquationsOfMotionEnvironmentUpdaterSettings( if( thirdBodyAcceleration != NULL && translationalAccelerationModels.count( thirdBodyAcceleration->getCentralBodyName( ) ) == 0 ) { - singleAccelerationUpdateNeeds[ body_transational_state_update ].push_back( - thirdBodyAcceleration->getCentralBodyName( ) ); + if( translationalAccelerationModels.count( thirdBodyAcceleration->getCentralBodyName( ) ) == 0 ) + { + singleAccelerationUpdateNeeds[ body_transational_state_update ].push_back( + thirdBodyAcceleration->getCentralBodyName( ) ); + } } else if( thirdBodyAcceleration == NULL ) { @@ -250,6 +211,16 @@ createTranslationalEquationsOfMotionEnvironmentUpdaterSettings( singleAccelerationUpdateNeeds[ spherical_harmonic_gravity_field_update ]. push_back( accelerationModelIterator->first ); break; + case mutual_spherical_harmonic_gravity: + singleAccelerationUpdateNeeds[ body_rotational_state_update ].push_back( + accelerationModelIterator->first ); + singleAccelerationUpdateNeeds[ spherical_harmonic_gravity_field_update ].push_back( + accelerationModelIterator->first ); + singleAccelerationUpdateNeeds[ body_rotational_state_update ].push_back( + acceleratedBodyIterator->first ); + singleAccelerationUpdateNeeds[ spherical_harmonic_gravity_field_update ].push_back( + acceleratedBodyIterator->first ); + break; case third_body_spherical_harmonic_gravity: { singleAccelerationUpdateNeeds[ body_rotational_state_update ].push_back( @@ -277,7 +248,55 @@ createTranslationalEquationsOfMotionEnvironmentUpdaterSettings( } break; } + case third_body_mutual_spherical_harmonic_gravity: + { + singleAccelerationUpdateNeeds[ body_rotational_state_update ].push_back( + accelerationModelIterator->first ); + singleAccelerationUpdateNeeds[ spherical_harmonic_gravity_field_update ].push_back( + accelerationModelIterator->first ); + singleAccelerationUpdateNeeds[ body_rotational_state_update ].push_back( + acceleratedBodyIterator->first ); + singleAccelerationUpdateNeeds[ spherical_harmonic_gravity_field_update ].push_back( + acceleratedBodyIterator->first ); + + boost::shared_ptr< gravitation::ThirdBodyMutualSphericalHarmonicsGravitationalAccelerationModel > + thirdBodyAcceleration = boost::dynamic_pointer_cast< + gravitation::ThirdBodyMutualSphericalHarmonicsGravitationalAccelerationModel >( + accelerationModelIterator->second.at( i ) ); + if( thirdBodyAcceleration != NULL && translationalAccelerationModels.count( + thirdBodyAcceleration->getCentralBodyName( ) ) == 0 ) + { + singleAccelerationUpdateNeeds[ body_transational_state_update ].push_back( + thirdBodyAcceleration->getCentralBodyName( ) ); + singleAccelerationUpdateNeeds[ body_rotational_state_update ].push_back( + thirdBodyAcceleration->getCentralBodyName( ) ); + singleAccelerationUpdateNeeds[ spherical_harmonic_gravity_field_update ].push_back( + thirdBodyAcceleration->getCentralBodyName( ) ); + } + else if( thirdBodyAcceleration == NULL ) + { + throw std::runtime_error( + std::string( "Error, incompatible input (ThirdBodyMutualSphericalHarmonicsGravitational" ) + + std::string( "AccelerationModel) to createTranslationalEquationsOfMotion ") + + std::string( "EnvironmentUpdaterSettings" ) ); + } + break; + } + case thrust_acceleration: + { + std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > thrustModelUpdates = + boost::dynamic_pointer_cast< propulsion::ThrustAcceleration >( + accelerationModelIterator->second.at( i ) )->getRequiredModelUpdates( ); + addEnvironmentUpdates( singleAccelerationUpdateNeeds, thrustModelUpdates ); + + singleAccelerationUpdateNeeds[ body_mass_update ].push_back( + acceleratedBodyIterator->first ); + + break; + } default: + throw std::runtime_error( std::string( "Error when setting acceleration model update needs, model type not recognized: " ) + + boost::lexical_cast< std::string >( currentAccelerationModelType ) ); break; } @@ -295,10 +314,59 @@ createTranslationalEquationsOfMotionEnvironmentUpdaterSettings( return environmentModelsToUpdate; } +//! Get list of required environment model update settings from mass rate models. +std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > +createMassPropagationEnvironmentUpdaterSettings( + const std::map< std::string, std::vector< boost::shared_ptr< basic_astrodynamics::MassRateModel > > > massRateModels, + const simulation_setup::NamedBodyMap& bodyMap ) +{ + using namespace basic_astrodynamics; + using namespace propagators; + + std::map< propagators::EnvironmentModelsToUpdate, + std::vector< std::string > > environmentModelsToUpdate; + std::map< propagators::EnvironmentModelsToUpdate, + std::vector< std::string > > singleRateModelUpdateNeeds; + + // Iterate over all bodies with mass rate model. + for( std::map< std::string, std::vector< boost::shared_ptr< MassRateModel > > >::const_iterator massRateModelIterator = + massRateModels.begin( ); massRateModelIterator != massRateModels.end( ); massRateModelIterator++ ) + { + for( unsigned int i = 0; i < massRateModelIterator->second.size( ); i++ ) + { + singleRateModelUpdateNeeds.clear( ); + + // Identify mass rate type and set required environment update settings. + AvailableMassRateModels currentAccelerationModelType = + getMassRateModelType( massRateModelIterator->second.at( i ) ); + switch( currentAccelerationModelType ) + { + case custom_mass_rate_model: + break; + case from_thrust_mass_rate_model: + break; + default: + throw std::runtime_error( std::string( "Error when setting mass rate model update needs, model type not recognized: " ) + + boost::lexical_cast< std::string >( currentAccelerationModelType ) ); + + } + + // Check whether requested updates are possible. + checkValidityOfRequiredEnvironmentUpdates( singleRateModelUpdateNeeds, bodyMap ); + + // Add requested updates of current acceleration model to + // full list of environment updates. + addEnvironmentUpdates( environmentModelsToUpdate, singleRateModelUpdateNeeds ); + } + } + + return environmentModelsToUpdate; + +} //! Function to create 'brute-force' update settings, in which each environment model is updated. std::map< propagators::EnvironmentModelsToUpdate, - std::vector< std::string > > createFullEnvironmentUpdaterSettings( +std::vector< std::string > > createFullEnvironmentUpdaterSettings( const simulation_setup::NamedBodyMap& bodyMap ) { using namespace basic_astrodynamics; @@ -333,18 +401,17 @@ std::map< propagators::EnvironmentModelsToUpdate, = bodyIterator->second->getRadiationPressureInterfaces( ); // Add each interface update function to update list. - for( std::map< std::string, boost::shared_ptr< RadiationPressureInterface > > ::iterator - iterator = radiationPressureInterfaces.begin( ); + for( std::map< std::string, boost::shared_ptr< RadiationPressureInterface > >::iterator + iterator = radiationPressureInterfaces.begin( ); iterator != radiationPressureInterfaces.end( ); iterator++ ) { singleAccelerationUpdateNeeds[ radiation_pressure_interface_update ]. - push_back( bodyIterator->first ); + push_back( bodyIterator->first ); } - // If body has rotation model, update rotational state in each time step. - boost::shared_ptr< ephemerides::RotationalEphemeris > rotationalEphemeris = - bodyIterator->second->getRotationalEphemeris( ); - if( rotationalEphemeris != NULL ) + // If body has rotation model, update rotational state in each time step.; + if( ( bodyIterator->second->getRotationalEphemeris( ) != NULL ) || + ( bodyIterator->second->getDependentOrientationCalculator( ) != NULL ) ) { singleAccelerationUpdateNeeds[ body_rotational_state_update ]. push_back( bodyIterator->first ); diff --git a/Tudat/Astrodynamics/Propagators/createEnvironmentUpdater.h b/Tudat/SimulationSetup/PropagationSetup/createEnvironmentUpdater.h similarity index 66% rename from Tudat/Astrodynamics/Propagators/createEnvironmentUpdater.h rename to Tudat/SimulationSetup/PropagationSetup/createEnvironmentUpdater.h index cc1863c4a6..351f040ba7 100644 --- a/Tudat/Astrodynamics/Propagators/createEnvironmentUpdater.h +++ b/Tudat/SimulationSetup/PropagationSetup/createEnvironmentUpdater.h @@ -13,7 +13,7 @@ #include -#include "Tudat/Astrodynamics/Propagators/environmentUpdater.h" +#include "Tudat/SimulationSetup/PropagationSetup/environmentUpdater.h" #include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h" namespace tudat @@ -37,19 +37,6 @@ void checkValidityOfRequiredEnvironmentUpdates( requestedUpdates, const simulation_setup::NamedBodyMap& bodyMap ); -//! Function to extend existing list of required environment update types -/*! - * Function to extend existing list of required environment update types - * \param environmentUpdateList List of environment updates to extend - * (passed by reference and modified by function) - * \param updatesToAdd List of environment updates that are to be added to environmentUpdateList - */ -void addEnvironmentUpdates( - std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > >& - environmentUpdateList, - const std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > - updatesToAdd ); - //! Get list of required environment model update settings from translational acceleration models. /*! * Get list of required environment model update settings from translational acceleration models. @@ -62,6 +49,18 @@ createTranslationalEquationsOfMotionEnvironmentUpdaterSettings( const basic_astrodynamics::AccelerationMap& translationalAccelerationModels, const simulation_setup::NamedBodyMap& bodyMap ); +//! Get list of required environment model update settings from mass rate models. +/*! + * Get list of required environment model update settings from mass rate models. + * \param massRateModels List of mass rate models used in simulation. + * \param bodyMap List of body objects used in the simulations. + * \return List of required environment model update settings. + */ +std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > +createMassPropagationEnvironmentUpdaterSettings( + const std::map< std::string, std::vector< boost::shared_ptr< basic_astrodynamics::MassRateModel > > > massRateModels, + const simulation_setup::NamedBodyMap& bodyMap ); + //! Get list of required environment model update settings from a list of propagation settings. /*! * Get list of required environment model update settings from a list of propagation settings. @@ -82,6 +81,42 @@ std::map< propagators::EnvironmentModelsToUpdate, // Check dynamics type switch( propagatorSettings->stateType_ ) { + case hybrid: + { + // Cast to derived type + boost::shared_ptr< MultiTypePropagatorSettings< StateScalarType > > multiTypePropagatorSettings = + boost::dynamic_pointer_cast< MultiTypePropagatorSettings< StateScalarType > >( propagatorSettings ); + + // Iterate over all propagation settings in hybrid model + std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > singleAccelerationUpdateNeeds; + + for( typename std::map< IntegratedStateType, + std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > >::const_iterator + typeIterator = multiTypePropagatorSettings->propagatorSettingsMap_.begin( ); + typeIterator != multiTypePropagatorSettings->propagatorSettingsMap_.end( ); typeIterator++ ) + { + for( unsigned int i = 0; i < typeIterator->second.size( ); i++ ) + { + // Create current propagation model from settings list (must not be hybrid). + if( typeIterator->first != hybrid ) + { + singleAccelerationUpdateNeeds = createEnvironmentUpdaterSettings< StateScalarType >( + typeIterator->second.at( i ), bodyMap ); + + // Add single model environment model update settings to full list + checkValidityOfRequiredEnvironmentUpdates( singleAccelerationUpdateNeeds, bodyMap ); + addEnvironmentUpdates( environmentModelsToUpdate, singleAccelerationUpdateNeeds ); + } + else + { + throw std::runtime_error( + "Error when making environment updater type list, cannot handle hybrid propagator inside hybrid propagator" ); + } + } + } + break; + } + // Retrieve environment model settings for translational dynamics case transational_state: { environmentModelsToUpdate = createTranslationalEquationsOfMotionEnvironmentUpdaterSettings( @@ -91,6 +126,15 @@ std::map< propagators::EnvironmentModelsToUpdate, bodyMap ); break; } + // Retrieve environment model settings for mass rate model + case body_mass_state: + { + environmentModelsToUpdate = createMassPropagationEnvironmentUpdaterSettings( + boost::dynamic_pointer_cast< + MassPropagatorSettings< StateScalarType > >( + propagatorSettings )->massRateModels_, bodyMap ); + break; + } default: { throw std::runtime_error( "Error, cannot create environment updates for type " + diff --git a/Tudat/SimulationSetup/PropagationSetup/createMassRateModels.cpp b/Tudat/SimulationSetup/PropagationSetup/createMassRateModels.cpp new file mode 100644 index 0000000000..48adb1665e --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/createMassRateModels.cpp @@ -0,0 +1,149 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/SimulationSetup/PropagationSetup/createMassRateModels.h" + +namespace tudat +{ + +namespace simulation_setup +{ + + +//! Function to create a mass rate model +boost::shared_ptr< basic_astrodynamics::MassRateModel > +createMassRateModel( + const std::string& bodyWithMassRate, + const boost::shared_ptr< MassRateModelSettings > massRateModelSettings, + const NamedBodyMap& bodyMap, + const basic_astrodynamics::AccelerationMap& accelerationModels ) +{ + boost::shared_ptr< basic_astrodynamics::MassRateModel > massRateModel; + + // Check type of mass rate model. + switch( massRateModelSettings->massRateType_ ) + { + case basic_astrodynamics::custom_mass_rate_model: + { + // Check input consistency + boost::shared_ptr< CustomMassRateModelSettings > customMassRateModelSettings = + boost::dynamic_pointer_cast< CustomMassRateModelSettings >( massRateModelSettings ); + if( customMassRateModelSettings == NULL ) + { + throw std::runtime_error( "Error when making cusom mass rate model, input is inconsistent" ); + } + else + { + massRateModel = boost::make_shared< basic_astrodynamics::CustomMassRateModel >( + customMassRateModelSettings->massRateFunction_ ); + } + break; + } + case basic_astrodynamics::from_thrust_mass_rate_model: + { + // Check input consistency + boost::shared_ptr< FromThrustMassModelSettings > fromThrustMassModelSettings = + boost::dynamic_pointer_cast< FromThrustMassModelSettings >( massRateModelSettings ); + if( fromThrustMassModelSettings == NULL ) + { + throw std::runtime_error( "Error when making from-engine mass rate model, input is inconsistent" ); + } + else + { + std::vector< boost::shared_ptr< basic_astrodynamics::AccelerationModel3d > > + thrustAccelerations; + if( accelerationModels.count( bodyWithMassRate ) != 0 ) + { + if( accelerationModels.at( bodyWithMassRate ).count( bodyWithMassRate ) != 0 ) + { + thrustAccelerations = basic_astrodynamics::getAccelerationModelsOfType( + accelerationModels.at( bodyWithMassRate ).at( bodyWithMassRate ), + basic_astrodynamics::thrust_acceleration ); + } + } + + if( thrustAccelerations.size( ) == 0 ) + { + std::cerr<<"Warning when making from-thrust mass-rate model, no thrust model is found; no thust is used"< > + explicitThrustAccelerations; + + if( fromThrustMassModelSettings->useAllThrustModels_ == 0 ) + { + // Retrieve thrust models with the correct id (should be 1) + for( unsigned int i = 0; i < thrustAccelerations.size( ); i++ ) + { + if( boost::dynamic_pointer_cast< propulsion::ThrustAcceleration >( + thrustAccelerations.at( i ) )->getAssociatedThroustSource( ) == + fromThrustMassModelSettings->associatedThroustSource_ ) + { + explicitThrustAccelerations.push_back( boost::dynamic_pointer_cast< propulsion::ThrustAcceleration >( + thrustAccelerations.at( i ) ) ); + } + } + + if( explicitThrustAccelerations.size( ) != 1 ) + { + std::cerr<<"Warning when making from-thrust mass-rate model, did not find exactly 1 thrust model with correct identifier"<( + thrustAccelerations.at( i ) ) ); + } + } + + // Create mass rate model + massRateModel = boost::make_shared< propulsion::FromThrustMassRateModel >( + explicitThrustAccelerations ); + } + break; + } + default: + throw std::runtime_error( "Error when making mass rate model, type not recognized" ); + + } + return massRateModel; +} + + +//! Function to create a list of mass rate models for a list of bodies. +std::map< std::string, std::vector< boost::shared_ptr< basic_astrodynamics::MassRateModel > > > createMassRateModelsMap( + const NamedBodyMap& bodyMap, + const std::map< std::string, std::vector< boost::shared_ptr< MassRateModelSettings > > >& massRateModelSettings, + const basic_astrodynamics::AccelerationMap& accelerationModels ) +{ + // Iterate over all bodies + std::map< std::string, std::vector< boost::shared_ptr< basic_astrodynamics::MassRateModel > > > massRateModels; + for( std::map< std::string, std::vector< boost::shared_ptr< MassRateModelSettings > > >::const_iterator settingsIterator = + massRateModelSettings.begin( ); settingsIterator != massRateModelSettings.end( ); settingsIterator++) + { + // Iterate over all mass model settings for current body. + for( unsigned int i = 0; i < settingsIterator->second.size( ); i++ ) + { + massRateModels[ settingsIterator->first ].push_back( + createMassRateModel( settingsIterator->first, settingsIterator->second.at( i ), bodyMap, + accelerationModels ) ); + } + } + return massRateModels; + +} + +} // namespace simulation_setup + +} // namespace tudat + diff --git a/Tudat/SimulationSetup/PropagationSetup/createMassRateModels.h b/Tudat/SimulationSetup/PropagationSetup/createMassRateModels.h new file mode 100644 index 0000000000..579a54cb6d --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/createMassRateModels.h @@ -0,0 +1,146 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_CREATEMASSRATEMODELS_H +#define TUDAT_CREATEMASSRATEMODELS_H + + +#include +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/massRateModel.h" +#include "Tudat/Astrodynamics/Propulsion/massRateFromThrust.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" + +namespace tudat +{ + +namespace simulation_setup +{ + +//! Class for providing settings for a mass rate model. +/*! + * Class for providing settings for a mass rate model, that defines the models to be used to numerically propagate the + * mass of a body during a simulation. If any additional information (besides the type of the mass rate model) is required, + * these must be implemented in a derived class (dedicated for each mass rate model type). + */ +class MassRateModelSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param massRateType Type of mass rate model that is to be used. + */ + MassRateModelSettings( + const basic_astrodynamics::AvailableMassRateModels massRateType ): + massRateType_( massRateType ){ } + + //! Destructor. + virtual ~MassRateModelSettings( ){ } + + //! Type of mass rate model that is to be used. + basic_astrodynamics::AvailableMassRateModels massRateType_; + +}; + +//! Class defining the settings for a custom (i.e. predefined function of time) mass rate model. +class CustomMassRateModelSettings: public MassRateModelSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param massRateFunction Function returning the mass rate as a function of time. + */ + CustomMassRateModelSettings( + const boost::function< double( const double ) > massRateFunction ): + MassRateModelSettings( basic_astrodynamics::custom_mass_rate_model ), + massRateFunction_( massRateFunction ){ } + + //! Destructor. + ~CustomMassRateModelSettings( ){ } + + //! Function returning the mass rate as a function of time. + boost::function< double( const double ) > massRateFunction_; + +}; + + +//! Class defining the settings of a thrust model where the thrust is directly retrieved from (a) model(s) of an engine. +class FromThrustMassModelSettings: public MassRateModelSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param useAllThrustModels Boolean denoting whether all engines of the associated body are to be combined into a + * single thrust model. + * \param associatedThroustSource Name of engine model from which thrust is to be derived (must be empty if + * useAllThrustModels is set to true) + */ + FromThrustMassModelSettings( + const bool useAllThrustModels = 1, + const std::string& associatedThroustSource = "" ): + MassRateModelSettings( basic_astrodynamics::from_thrust_mass_rate_model ), + associatedThroustSource_( associatedThroustSource ), useAllThrustModels_( useAllThrustModels ){ } + + //! Destructor + ~FromThrustMassModelSettings( ){ } + + //! Name of engine model from which thrust is to be derived + std::string associatedThroustSource_; + + //! Boolean denoting whether all engines of the associated body are to be combined into a single thrust model. + bool useAllThrustModels_; + +}; + +//! Function to create a mass rate model +/*! + * Function to create a mass rate model, from specific settings and the full set of environment models. + * \param bodyWithMassRate Name of body for which a mass rate model is to be created. + * \param massRateModelSettings Settings for the mass rate model that is to be created. + * \param bodyMap List of pointers to body objects; defines the full simulation environment. + * \param accelerationModels List of acceleration models that are used during numerical propagation (empty by default). + * \return Mass rate model that is to be used during numerical propagation. + */ +boost::shared_ptr< basic_astrodynamics::MassRateModel > createMassRateModel( + const std::string& bodyWithMassRate, + const boost::shared_ptr< MassRateModelSettings > massRateModelSettings, + const NamedBodyMap& bodyMap, + const basic_astrodynamics::AccelerationMap& accelerationModels = basic_astrodynamics::AccelerationMap( ) ); + + +//! Function to create a list of mass rate models for a list of bodies. +/*! + * Function to create a list of mass rate models for a list of bodies, from specific settings and the full set of + * environment models. + * \param bodyMap List of pointers to body objects; defines the full simulation environment. + * \param massRateModelSettings Settings for the mass rate models that are to be created (key is body id). + * \param bodyMap List of pointers to body objects; defines the full simulation environment. + * \param accelerationModels List of acceleration models that are used during numerical propagation (empty by default). + * \return Mass rate models that are to be used during numerical propagation (key is body id).. + */ +std::map< std::string, std::vector< boost::shared_ptr< basic_astrodynamics::MassRateModel > > > createMassRateModelsMap( + const NamedBodyMap& bodyMap, + const std::map< std::string, std::vector< boost::shared_ptr< MassRateModelSettings > > >& massRateModelSettings, + const basic_astrodynamics::AccelerationMap& accelerationModels = basic_astrodynamics::AccelerationMap( ) ); + +} // namespace simulation_setup + +} // namespace tudat + +#endif // TUDAT_CREATEMASSRATEMODELS_H diff --git a/Tudat/SimulationSetup/PropagationSetup/createNumericalSimulator.cpp b/Tudat/SimulationSetup/PropagationSetup/createNumericalSimulator.cpp new file mode 100644 index 0000000000..46c603f6ae --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/createNumericalSimulator.cpp @@ -0,0 +1,146 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + + +#include "Tudat/SimulationSetup/PropagationSetup/createNumericalSimulator.h" +#include "Tudat/Astrodynamics/Ephemerides/frameManager.h" + +namespace tudat +{ + +namespace simulation_setup +{ + +//! Function to create a set of acceleration models from a map of bodies and acceleration model types. +basic_astrodynamics::AccelerationMap createAccelerationModelsMap( + const NamedBodyMap& bodyMap, + const SelectedAccelerationMap& selectedAccelerationPerBody, + const std::map< std::string, std::string >& centralBodies ) +{ + // Declare return map. + basic_astrodynamics::AccelerationMap accelerationModelMap; + + // Put selectedAccelerationPerBody in correct order + SelectedAccelerationMap orderedAccelerationPerBody = + orderSelectedAccelerationMap( selectedAccelerationPerBody ); + + // Iterate over all bodies which are undergoing acceleration + for( SelectedAccelerationMap::const_iterator bodyIterator = + orderedAccelerationPerBody.begin( ); bodyIterator != orderedAccelerationPerBody.end( ); + bodyIterator++ ) + { + boost::shared_ptr< Body > currentCentralBody; + + // Retrieve name of body undergoing acceleration. + std::string bodyUndergoingAcceleration = bodyIterator->first; + + // Retrieve name of current central body. + std::string currentCentralBodyName = centralBodies.at( bodyUndergoingAcceleration ); + + if( !ephemerides::isFrameInertial( currentCentralBodyName ) ) + { + if( bodyMap.count( currentCentralBodyName ) == 0 ) + { + throw std::runtime_error( + std::string( "Error, could not find non-inertial central body ") + + currentCentralBodyName + " of " + bodyUndergoingAcceleration + + " when making acceleration model." ); + } + else + { + currentCentralBody = bodyMap.at( currentCentralBodyName ); + } + } + + // Check if body undergoing acceleration is included in bodyMap + if( bodyMap.count( bodyUndergoingAcceleration ) == 0 ) + { + throw std::runtime_error( + std::string( "Error when making acceleration models, requested forces" ) + + "acting on body " + bodyUndergoingAcceleration + + ", but no such body found in map of bodies" ); + } + + // Declare map of acceleration models acting on current body. + basic_astrodynamics::SingleBodyAccelerationMap mapOfAccelerationsForBody; + + // Retrieve list of required acceleration model types and bodies exerting accelerationd on + // current body. + std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > + accelerationsForBody = bodyIterator->second; + + // Iterate over all bodies exerting an acceleration + for( std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > >:: + iterator body2Iterator = accelerationsForBody.begin( ); + body2Iterator != accelerationsForBody.end( ); body2Iterator++ ) + { + // Retrieve name of body exerting acceleration. + std::string bodyExertingAcceleration = body2Iterator->first; + + // Check if body exerting acceleration is included in bodyMap + if( bodyMap.count( bodyExertingAcceleration ) == 0 ) + { + throw std::runtime_error( + std::string( "Error when making acceleration models, requested forces ") + + "acting on body " + bodyUndergoingAcceleration + " due to body " + + bodyExertingAcceleration + + ", but no such body found in map of bodies" ); + } + + // Retrieve list of accelerations due to current body. + std::vector< boost::shared_ptr< AccelerationSettings > > accelerationList = + body2Iterator->second; + + for( unsigned int i = 0; i < accelerationList.size( ); i++ ) + { + // Create acceleration model. + mapOfAccelerationsForBody[ bodyExertingAcceleration ].push_back( + createAccelerationModel( bodyMap.at( bodyUndergoingAcceleration ), + bodyMap.at( bodyExertingAcceleration ), + accelerationList.at( i ), + bodyUndergoingAcceleration, + bodyExertingAcceleration, + currentCentralBody, + currentCentralBodyName, + bodyMap ) ); + } + } + + // Put acceleration models on current body in return map. + accelerationModelMap[ bodyUndergoingAcceleration ] = mapOfAccelerationsForBody; + } + + return accelerationModelMap; +} + +//! Function to create acceleration models from a map of bodies and acceleration model types. +basic_astrodynamics::AccelerationMap createAccelerationModelsMap( + const NamedBodyMap& bodyMap, + const SelectedAccelerationMap& selectedAccelerationPerBody, + const std::vector< std::string >& propagatedBodies, + const std::vector< std::string >& centralBodies ) +{ + if( centralBodies.size( ) != propagatedBodies.size( ) ) + { + throw std::runtime_error( "Error, number of propagated bodies must equal number of central bodies" ); + } + + std::map< std::string, std::string > centralBodyMap; + for( unsigned int i = 0; i < propagatedBodies.size( ); i++ ) + { + centralBodyMap[ propagatedBodies.at( i ) ] = centralBodies.at( i ); + } + + return createAccelerationModelsMap( bodyMap, selectedAccelerationPerBody, centralBodyMap ); +} + +} + +} diff --git a/Tudat/SimulationSetup/PropagationSetup/createNumericalSimulator.h b/Tudat/SimulationSetup/PropagationSetup/createNumericalSimulator.h new file mode 100644 index 0000000000..e7da8d9bee --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/createNumericalSimulator.h @@ -0,0 +1,101 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + + +#ifndef TUDAT_CREATENUMERICALSIMULATOR_H +#define TUDAT_CREATENUMERICALSIMULATOR_H + +#include "Tudat/SimulationSetup/PropagationSetup/accelerationSettings.h" +#include "Tudat/SimulationSetup/PropagationSetup/createAccelerationModels.h" +#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h" +#include "Tudat/SimulationSetup/PropagationSetup/variationalEquationsSolver.h" + +namespace tudat +{ + +namespace simulation_setup +{ + + +//! Function to create acceleration models from a map of bodies and acceleration model types. +/*! + * Function to create acceleration models from a map of bodies and acceleration model types. + * The return type can be used to identify both the body undergoing and exerting acceleration. + * \param bodyMap List of pointers to bodies required for the creation of the acceleration model + * objects. + * \param selectedAccelerationPerBody List identifying which bodies exert which type of + * acceleration(s) on which bodies. + * \param centralBodies Map of central bodies for each body undergoing acceleration. + * \return List of acceleration model objects, in form of AccelerationMap. + */ +basic_astrodynamics::AccelerationMap createAccelerationModelsMap( + const NamedBodyMap& bodyMap, + const SelectedAccelerationMap& selectedAccelerationPerBody, + const std::map< std::string, std::string >& centralBodies ); + +//! Function to create acceleration models from a map of bodies and acceleration model types. +/*! + * Function to create acceleration models from a map of bodies and acceleration model types. + * The return type can be used to identify both the body undergoing and exerting acceleration. + * \param bodyMap List of pointers to bodies required for the creation of the acceleration model + * objects. + * \param selectedAccelerationPerBody List identifying which bodies exert which type of + * acceleration(s) on which bodies. + * \param propagatedBodies List of bodies that are to be propagated + * \param centralBodies List of central bodies for each body undergoing acceleration (in same order as propagatedBodies). + * \return List of acceleration model objects, in form of AccelerationMap. + */ +basic_astrodynamics::AccelerationMap createAccelerationModelsMap( + const NamedBodyMap& bodyMap, + const SelectedAccelerationMap& selectedAccelerationPerBody, + const std::vector< std::string >& propagatedBodies, + const std::vector< std::string >& centralBodies ); + + + +template< typename StateScalarType = double, typename TimeType = double > +boost::shared_ptr< propagators::SingleArcDynamicsSimulator< StateScalarType, TimeType > > createSingleArcDynamicsSimulator( + const simulation_setup::NamedBodyMap& bodyMap, + const boost::shared_ptr< numerical_integrators::IntegratorSettings< TimeType > > integratorSettings, + const boost::shared_ptr< propagators::PropagatorSettings< StateScalarType > > propagatorSettings, + const bool areEquationsOfMotionToBeIntegrated = true, + const bool clearNumericalSolutions = true, + const bool setIntegratedResult = true ) +{ + return boost::make_shared< propagators::SingleArcDynamicsSimulator< StateScalarType, TimeType > >( + bodyMap, + integratorSettings, propagatorSettings, areEquationsOfMotionToBeIntegrated, clearNumericalSolutions, + setIntegratedResult ); +} + +template< typename StateScalarType = double, typename TimeType = double, typename ParameterType = double > +boost::shared_ptr< propagators::SingleArcVariationalEquationsSolver< StateScalarType, TimeType, ParameterType > > +createSingleArcVariationalEquationsSolver( + const simulation_setup::NamedBodyMap& bodyMap, + const boost::shared_ptr< numerical_integrators::IntegratorSettings< TimeType > > integratorSettings, + const boost::shared_ptr< propagators::PropagatorSettings< StateScalarType > > propagatorSettings, + const boost::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > parametersToEstimate, + const bool integrateDynamicalAndVariationalEquationsConcurrently = 1, + const boost::shared_ptr< numerical_integrators::IntegratorSettings< double > > variationalOnlyIntegratorSettings + = boost::shared_ptr< numerical_integrators::IntegratorSettings< double > >( ), + const bool clearNumericalSolution = 1, + const bool integrateEquationsOnCreation = 1 ) +{ + return boost::make_shared< propagators::SingleArcVariationalEquationsSolver< StateScalarType, TimeType, ParameterType > >( + bodyMap, integratorSettings, propagatorSettings, parametersToEstimate, + integrateDynamicalAndVariationalEquationsConcurrently, variationalOnlyIntegratorSettings, + clearNumericalSolution, integrateEquationsOnCreation ); +} + +} // namespace simulation_setup + +} // namespace tudat + +#endif // TUDAT_CREATENUMERICALSIMULATOR_H diff --git a/Tudat/SimulationSetup/PropagationSetup/createStateDerivativeModel.cpp b/Tudat/SimulationSetup/PropagationSetup/createStateDerivativeModel.cpp new file mode 100644 index 0000000000..8b13789179 --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/createStateDerivativeModel.cpp @@ -0,0 +1 @@ + diff --git a/Tudat/Astrodynamics/Propagators/createStateDerivativeModel.h b/Tudat/SimulationSetup/PropagationSetup/createStateDerivativeModel.h similarity index 58% rename from Tudat/Astrodynamics/Propagators/createStateDerivativeModel.h rename to Tudat/SimulationSetup/PropagationSetup/createStateDerivativeModel.h index cf962a2cac..6b6c1f4813 100644 --- a/Tudat/Astrodynamics/Propagators/createStateDerivativeModel.h +++ b/Tudat/SimulationSetup/PropagationSetup/createStateDerivativeModel.h @@ -11,10 +11,15 @@ #ifndef TUDAT_CREATESTATEDERIVATIVEMODEL_H #define TUDAT_CREATESTATEDERIVATIVEMODEL_H +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h" #include "Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h" -#include "Tudat/Astrodynamics/Propagators/propagationSettings.h" +#include "Tudat/SimulationSetup/PropagationSetup/propagationSettings.h" #include "Tudat/Astrodynamics/Propagators/nBodyCowellStateDerivative.h" -#include "Tudat/SimulationSetup/body.h" +#include "Tudat/Astrodynamics/Propagators/nBodyEnckeStateDerivative.h" +#include "Tudat/Astrodynamics/Propagators/bodyMassStateDerivative.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" namespace tudat { @@ -94,14 +99,16 @@ boost::shared_ptr< CentralBodyData< StateScalarType, TimeType > > createCentralB * propagation settings and environment. * \param translationPropagatorSettings Settings for the translational dynamics model. * \param bodyMap List of body objects in the environment + * \param propagationStartTime Time from which numerical propagation starts. * \return Translational state derivative model (instance of derived class of NBodyStateDerivative) */ template< typename StateScalarType = double, typename TimeType = double > boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > - createTranslationalStateDerivativeModel( +createTranslationalStateDerivativeModel( const boost::shared_ptr< TranslationalStatePropagatorSettings< StateScalarType > > - translationPropagatorSettings, - const simulation_setup::NamedBodyMap& bodyMap ) + translationPropagatorSettings, + const simulation_setup::NamedBodyMap& bodyMap, + const TimeType propagationStartTime ) { // Create object for frame origin transformations. @@ -124,6 +131,33 @@ boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > translationPropagatorSettings->bodiesToIntegrate_ ); break; } + case encke: + { + // Calculate initial Kepler elements for Encke propagator + std::vector< Eigen::Matrix< StateScalarType, 6, 1 > > initialKeplerElements; + initialKeplerElements.resize( translationPropagatorSettings->bodiesToIntegrate_.size( ) ); + std::vector< std::string > centralBodies = translationPropagatorSettings->centralBodies_; + + for( unsigned int i = 0; i < translationPropagatorSettings->bodiesToIntegrate_.size( ); i++ ) + { + if( bodyMap.count( centralBodies[ i ] ) == 0 ) + { + std::string errorMessage = "Error when creating Encke propagator, did not find central body " + + boost::lexical_cast< std::string >( centralBodies[ i ] ); + throw std::runtime_error( errorMessage ); + } + initialKeplerElements[ i ] = orbital_element_conversions::convertCartesianToKeplerianElements< StateScalarType >( + translationPropagatorSettings->getInitialStates( ).segment( i * 6, 6 ), static_cast< StateScalarType >( + bodyMap.at( centralBodies[ i ] )->getGravityFieldModel( )->getGravitationalParameter( ) ) ); + } + + // Create Encke state derivative object. + stateDerivativeModel = boost::make_shared< NBodyEnckeStateDerivative< StateScalarType, TimeType > > + ( translationPropagatorSettings->accelerationsMap_, centralBodyData, translationPropagatorSettings->bodiesToIntegrate_, + initialKeplerElements, propagationStartTime ); + + break; + } default: throw std::runtime_error( "Error, did not recognize translational state propagation type: " + @@ -132,18 +166,37 @@ boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > return stateDerivativeModel; } +//! Function to create a mass state derivative model. +/*! + * Function to create a mass state derivative model from propagation settings and environment. + * \param massPropagatorSettings Settings for the mass dynamics model. + * \param bodyMap List of body objects in the environment + * \return Mass state derivative model. + */ +template< typename StateScalarType = double, typename TimeType = double > +boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > createBodyMassStateDerivativeModel( + const boost::shared_ptr< MassPropagatorSettings< StateScalarType > > massPropagatorSettings, + const simulation_setup::NamedBodyMap& bodyMap ) +{ + return boost::make_shared< propagators::BodyMassStateDerivative< StateScalarType, TimeType > >( + massPropagatorSettings->massRateModels_, + massPropagatorSettings->bodiesWithMassToPropagate_ ); +} + //! Function to create a state derivative model. /*! * Function to create a state derivative model from propagation settings and the environment. * \param propagatorSettings Settings for the dynamical model. * \param bodyMap List of body objects in the environment + * \param propagationStartTime Time from which numerical propagation starts. * \return State derivative model (instance of required derived class of SingleStateTypeDerivative) */ template< typename StateScalarType = double, typename TimeType = double > boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > - createStateDerivativeModel( +createStateDerivativeModel( const boost::shared_ptr< PropagatorSettings< StateScalarType > > propagatorSettings, - const simulation_setup::NamedBodyMap& bodyMap ) + const simulation_setup::NamedBodyMap& bodyMap, + const TimeType propagationStartTime ) { boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > stateDerivativeModel; @@ -155,9 +208,9 @@ boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > { // Check input consistency. boost::shared_ptr< TranslationalStatePropagatorSettings< StateScalarType > > - translationPropagatorSettings = + translationPropagatorSettings = boost::dynamic_pointer_cast< - TranslationalStatePropagatorSettings< StateScalarType > >( propagatorSettings ); + TranslationalStatePropagatorSettings< StateScalarType > >( propagatorSettings ); if( translationPropagatorSettings == NULL ) { throw std::runtime_error( @@ -166,7 +219,24 @@ boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > else { stateDerivativeModel = createTranslationalStateDerivativeModel< StateScalarType, TimeType >( - translationPropagatorSettings, bodyMap ); + translationPropagatorSettings, bodyMap, propagationStartTime ); + } + break; + } + case body_mass_state: + { + // Check input consistency. + boost::shared_ptr< MassPropagatorSettings< StateScalarType > > massPropagatorSettings = + boost::dynamic_pointer_cast< MassPropagatorSettings< StateScalarType > >( propagatorSettings ); + if( massPropagatorSettings == NULL ) + { + throw std::runtime_error( + "Error, expected mass propagation settings when making state derivative model" ); + } + else + { + stateDerivativeModel = createBodyMassStateDerivativeModel< StateScalarType, TimeType >( + massPropagatorSettings, bodyMap ); } break; } @@ -184,15 +254,17 @@ boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > * Function to create a list of state derivative models from * propagation settings and the environment. * \param propagatorSettings Settings for the dynamical model. - * \param bodyMap List of body objects in the environment + * \param bodyMap List of body objects in the environment. + * \param propagationStartTime Time from which numerical propagation starts. * \return List of state derivative models (instances of required * derived class of SingleStateTypeDerivative) */ template< typename StateScalarType = double, typename TimeType = double > std::vector< boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > > - createStateDerivativeModels( +createStateDerivativeModels( const boost::shared_ptr< PropagatorSettings< StateScalarType > > propagatorSettings, - const simulation_setup::NamedBodyMap& bodyMap ) + const simulation_setup::NamedBodyMap& bodyMap, + const TimeType propagationStartTime ) { std::vector< boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > > stateDerivativeModels; @@ -200,15 +272,46 @@ std::vector< boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, Time // Check type of state derivative model and call associated create function. switch( propagatorSettings->stateType_ ) { + // If hybrid, call create function separately for each entry. + case hybrid: + { + boost::shared_ptr< MultiTypePropagatorSettings< StateScalarType > > multiTypePropagatorSettings = + boost::dynamic_pointer_cast< MultiTypePropagatorSettings< StateScalarType > >( propagatorSettings ); + + // Iterate over all propagation settings + for( typename std::map< IntegratedStateType, + std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > >::iterator + propagatorIterator = multiTypePropagatorSettings->propagatorSettingsMap_.begin( ); + propagatorIterator != multiTypePropagatorSettings->propagatorSettingsMap_.end( ); propagatorIterator++ ) + { + for( unsigned int i = 0; i < propagatorIterator->second.size( ); i++ ) + { + // Call create function for current propagation settings. + if( propagatorIterator->first != hybrid ) + { + stateDerivativeModels.push_back( createStateDerivativeModel< StateScalarType, TimeType >( + propagatorIterator->second.at( i ), bodyMap, propagationStartTime ) ); + } + else + { + throw std::runtime_error( + "Error when making state derivative model, cannot process nested hybrid propagators" ); + } + } + } + break; + } + // If not hybrid, call create function for single object directly. default: stateDerivativeModels.push_back( createStateDerivativeModel< StateScalarType, TimeType >( - propagatorSettings, bodyMap ) ); + propagatorSettings, bodyMap, propagationStartTime ) ); } return stateDerivativeModels; } } // namespace propagators + } // namespace tudat #endif // TUDAT_CREATESTATEDERIVATIVEMODEL_H diff --git a/Tudat/SimulationSetup/PropagationSetup/createThrustModelGuidance.cpp b/Tudat/SimulationSetup/PropagationSetup/createThrustModelGuidance.cpp new file mode 100644 index 0000000000..f1705487ba --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/createThrustModelGuidance.cpp @@ -0,0 +1,411 @@ +#include "Tudat/SimulationSetup/PropagationSetup/createThrustModelGuidance.h" +#include "Tudat/Basics/utilities.h" + +namespace tudat +{ + +namespace simulation_setup +{ + +//! Function to create the object determining the direction of the thrust acceleration. +boost::shared_ptr< propulsion::BodyFixedForceDirectionGuidance > createThrustGuidanceModel( + const boost::shared_ptr< ThrustDirectionGuidanceSettings > thrustDirectionGuidanceSettings, + const NamedBodyMap& bodyMap, + const std::string& nameOfBodyWithGuidance, + const boost::function< Eigen::Vector3d( ) > bodyFixedThrustOrientation, + std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > >& magnitudeUpdateSettings ) +{ + boost::shared_ptr< propulsion::BodyFixedForceDirectionGuidance > thrustGuidance; + + // Determine thrust direction type + switch( thrustDirectionGuidanceSettings->thrustDirectionType_ ) + { + case colinear_with_state_segment_thrust_direction: + { + // Check input consistency + boost::shared_ptr< ThrustDirectionFromStateGuidanceSettings > thrustDirectionFromStateGuidanceSettings = + boost::dynamic_pointer_cast< ThrustDirectionFromStateGuidanceSettings >( thrustDirectionGuidanceSettings ); + if( thrustDirectionFromStateGuidanceSettings == NULL ) + { + throw std::runtime_error( "Error when thrust colinear with state, input is inconsistent" ); + } + else + { + // Retrieve state function of body for which thrust is to be computed. + boost::function< basic_mathematics::Vector6d( ) > bodyStateFunction = + boost::bind( &Body::getState, bodyMap.at( nameOfBodyWithGuidance ) ); + boost::function< basic_mathematics::Vector6d( ) > centralBodyStateFunction; + + // Retrieve state function of central body (or set to zero if inertial) + if( thrustDirectionFromStateGuidanceSettings->relativeBody_ != "SSB" && + thrustDirectionFromStateGuidanceSettings->relativeBody_ != "" ) + { + centralBodyStateFunction = boost::bind( &Body::getState, bodyMap.at( + thrustDirectionFromStateGuidanceSettings->relativeBody_ ) ); + magnitudeUpdateSettings[ propagators::body_transational_state_update ].push_back( + thrustDirectionFromStateGuidanceSettings->relativeBody_ ); + } + else + { + centralBodyStateFunction = boost::lambda::constant( basic_mathematics::Vector6d::Zero( ) ); + } + + // Define relative state function + boost::function< void( basic_mathematics::Vector6d& ) > stateFunction = + boost::bind( + &ephemerides::getRelativeState, _1, bodyStateFunction, centralBodyStateFunction ); + boost::function< Eigen::Vector3d( const double ) > thrustDirectionFunction; + + // Create force direction function. + if( thrustDirectionFromStateGuidanceSettings->isColinearWithVelocity_ ) + { + thrustDirectionFunction = + boost::bind( &propulsion::getForceDirectionColinearWithVelocity, stateFunction, _1, + thrustDirectionFromStateGuidanceSettings->directionIsOppositeToVector_ ); + } + else + { + thrustDirectionFunction = + boost::bind( &propulsion::getForceDirectionColinearWithPosition, stateFunction, _1, + thrustDirectionFromStateGuidanceSettings->directionIsOppositeToVector_ ); + } + + // Create direction guidance + thrustGuidance = boost::make_shared< propulsion::DirectionBasedForceGuidance >( + thrustDirectionFunction, thrustDirectionFromStateGuidanceSettings->relativeBody_, + bodyFixedThrustOrientation ); + } + break; + } + case thrust_direction_from_existing_body_orientation: + { + boost::shared_ptr< Body > bodyWithGuidance = bodyMap.at( nameOfBodyWithGuidance ); + + boost::function< Eigen::Quaterniond( const double ) > rotationFunction; + + // Retrieve existing body rotation model and set associated update settings. + if( bodyWithGuidance->getFlightConditions( ) != NULL ) + { + rotationFunction = boost::bind( + &simulation_setup::Body::getCurrentRotationToGlobalFrame, + bodyWithGuidance ); + + magnitudeUpdateSettings[ propagators::vehicle_flight_conditions_update ].push_back( nameOfBodyWithGuidance ); + magnitudeUpdateSettings[ propagators::body_rotational_state_update ].push_back( nameOfBodyWithGuidance ); + magnitudeUpdateSettings[ propagators::body_rotational_state_update ].push_back( + thrustDirectionGuidanceSettings->relativeBody_ ); + magnitudeUpdateSettings[ propagators::body_transational_state_update ].push_back( + thrustDirectionGuidanceSettings->relativeBody_); + } + else if( bodyWithGuidance->getRotationalEphemeris( ) != NULL ) + { + rotationFunction = boost::bind( + &simulation_setup::Body::getCurrentRotationToGlobalFrame, + bodyWithGuidance ); + magnitudeUpdateSettings[ propagators::body_rotational_state_update ].push_back( nameOfBodyWithGuidance ); + } + else + { + throw std::runtime_error( "Error, requested thrust orientation from existing model, but no such model found" ); + } + + thrustGuidance = boost::make_shared< propulsion::OrientationBasedForceGuidance >( + rotationFunction, bodyFixedThrustOrientation ); + + break; + } + case custom_thrust_direction: + { + // Check input consistency + boost::shared_ptr< CustomThrustDirectionSettings > customThrustGuidanceSettings = + boost::dynamic_pointer_cast< CustomThrustDirectionSettings >( thrustDirectionGuidanceSettings ); + if( customThrustGuidanceSettings == NULL ) + { + throw std::runtime_error( "Error when getting thrust guidance with custom_thrust_direction, input is inconsistent" ); + } + else + { + // Create direction guidance + boost::function< Eigen::Vector3d( const double ) > thrustDirectionFunction = + customThrustGuidanceSettings->thrustDirectionFunction_; + thrustGuidance = boost::make_shared< propulsion::DirectionBasedForceGuidance >( + thrustDirectionFunction, "", bodyFixedThrustOrientation ); + } + + break; + } + case custom_thrust_orientation: + { + // Check input consistency + boost::shared_ptr< CustomThrustOrientationSettings > customThrustOrientationSettings = + boost::dynamic_pointer_cast< CustomThrustOrientationSettings >( thrustDirectionGuidanceSettings ); + if( customThrustOrientationSettings == NULL ) + { + throw std::runtime_error( "Error when getting thrust guidance with custom_thrust_orientation, input is inconsistent" ); + } + else + { + // Create direction guidance + thrustGuidance = boost::make_shared< propulsion::OrientationBasedForceGuidance >( + customThrustOrientationSettings->thrustOrientationFunction_, + bodyFixedThrustOrientation ); + magnitudeUpdateSettings[ propagators::body_rotational_state_update ].push_back( nameOfBodyWithGuidance ); + } + break; + } + default: + throw std::runtime_error( "Error, could not find thrust guidance type when creating thrust guidance." ); + } + return thrustGuidance; +} + +//! Function to retrieve the effective thrust direction from a set of thrust sources. +Eigen::Vector3d getCombinedThrustDirection( + const std::vector< boost::function< Eigen::Vector3d( )> >& thrustDirections, + const std::vector< boost::function< double( )> >& thrustMagnitudes ) +{ + Eigen::Vector3d thrustDirection = Eigen::Vector3d::Zero( ); + double totalThrust = 0.0; + + for( unsigned int i = 0; i < thrustDirections.size( ); i++ ) + { + thrustDirection += thrustMagnitudes.at( i )( ) * thrustDirections.at( i )( ); + totalThrust += thrustMagnitudes.at( i )( ); + } + return thrustDirection / totalThrust; +} + +//! Function to create a function that returns the thrust direction in the body-fixed frame. +boost::function< Eigen::Vector3d( ) > getBodyFixedThrustDirection( + const boost::shared_ptr< ThrustEngineSettings > thrustMagnitudeSettings, + const NamedBodyMap& bodyMap, + const std::string bodyName ) +{ + boost::function< Eigen::Vector3d( ) > thrustDirectionFunction; + + // Identify magnitude settings type + switch( thrustMagnitudeSettings->thrustMagnitudeGuidanceType_ ) + { + case constant_thrust_magnitude: + { + // Check input consistency + boost::shared_ptr< ConstantThrustEngineSettings > constantThrustMagnitudeSettings = + boost::dynamic_pointer_cast< ConstantThrustEngineSettings >( thrustMagnitudeSettings ); + if( constantThrustMagnitudeSettings == NULL ) + { + throw std::runtime_error( "Error when creating body-fixed thrust direction of type constant_thrust_magnitude, input is inconsistent" ); + } + else + { + thrustDirectionFunction = boost::lambda::constant( constantThrustMagnitudeSettings->bodyFixedThrustDirection_ ); + } + break; + } + case from_engine_properties_thrust_magnitude: + { + // Check input consistency + boost::shared_ptr< FromBodyThrustEngineSettings > fromEngineThrustMagnitudeSettings = + boost::dynamic_pointer_cast< FromBodyThrustEngineSettings >( thrustMagnitudeSettings ); + if( fromEngineThrustMagnitudeSettings == NULL ) + { + throw std::runtime_error( "Error when creating body-fixed thrust direction of type from_engine_properties_thrust_magnitude, input is inconsistent" ); + } + if( bodyMap.at( bodyName )->getVehicleSystems( ) == NULL ) + { + throw std::runtime_error( "Error when creating body-fixed thrust direction of type from_engine_properties_thrust_magnitude, no vehicle systems found" ); + + } + + // Retrieve single engine + if( fromEngineThrustMagnitudeSettings->useAllEngines_ == false ) + { + // Check if engine model exists + if( ( bodyMap.at( bodyName )->getVehicleSystems( )->getEngineModels( ).count( + thrustMagnitudeSettings->thrustOriginId_ ) == 0 ) ) + { + throw std::runtime_error( "Error when creating body-fixed thrust direction of type from_engine_properties_thrust_magnitude, no engine of right ID found" ); + } + else + { + thrustDirectionFunction = + boost::bind( &system_models::EngineModel::getBodyFixedThrustDirection, + bodyMap.at( bodyName )->getVehicleSystems( )->getEngineModels( ).at( + thrustMagnitudeSettings->thrustOriginId_ ) ); + } + + } + // Retrieve mean thrust direction from all engines + else + { + // Print warning if there are no engines (zero thrust) + if( ( bodyMap.at( bodyName )->getVehicleSystems( )->getEngineModels( ).size( ) == 0 ) ) + { + std::cerr<<"Error when creating body-fixed thrust direction of type from_engine_properties_thrust_magnitude; no engines found: returning 0 thrust"< > thrustDirections; + std::vector< boost::function< double( )> > thrustMagnitudes; + + std::map< std::string, boost::shared_ptr< system_models::EngineModel > > engineModels = + bodyMap.at( bodyName )->getVehicleSystems( )->getEngineModels( ); + + for( std::map< std::string, boost::shared_ptr< system_models::EngineModel > >::const_iterator engineIterator = + engineModels.begin( ); engineIterator != engineModels.end( ); engineIterator++ ) + { + thrustDirections.push_back( + boost::bind( &system_models::EngineModel::getBodyFixedThrustDirection, engineIterator->second ) ); + thrustMagnitudes.push_back( + boost::bind( &system_models::EngineModel::getCurrentThrust, engineIterator->second ) ); + } + + // Create effective thrust direction function. + thrustDirectionFunction = boost::bind( + &getCombinedThrustDirection, thrustDirections, thrustMagnitudes ); + + } + break; + + } + case thrust_magnitude_from_time_function: + { + // Check input consistency + boost::shared_ptr< FromFunctionThrustEngineSettings > fromFunctionThrustMagnitudeSettings = + boost::dynamic_pointer_cast< FromFunctionThrustEngineSettings >( thrustMagnitudeSettings ); + if( fromFunctionThrustMagnitudeSettings == NULL ) + { + throw std::runtime_error( "Error when creating body-fixed thrust direction of type thrust_magnitude_from_time_function, input is inconsistent" ); + } + else + { + thrustDirectionFunction = boost::lambda::constant( fromFunctionThrustMagnitudeSettings->bodyFixedThrustDirection_ ); + } + break; + + } + default: + throw std::runtime_error( "Error when creating body-fixed thrust direction, type not identified" ); + } + return thrustDirectionFunction; +} + +//! Function to create a wrapper object that computes the thrust magnitude +boost::shared_ptr< propulsion::ThrustMagnitudeWrapper > createThrustMagnitudeWrapper( + const boost::shared_ptr< ThrustEngineSettings > thrustMagnitudeSettings, + const NamedBodyMap& bodyMap, + const std::string& nameOfBodyWithGuidance, + std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > >& magnitudeUpdateSettings ) +{ + boost::shared_ptr< propulsion::ThrustMagnitudeWrapper > thrustMagnitudeWrapper; + + // Identify magnitude settings type + switch( thrustMagnitudeSettings->thrustMagnitudeGuidanceType_ ) + { + case constant_thrust_magnitude: + { + // Check input consistency + boost::shared_ptr< ConstantThrustEngineSettings > constantThrustMagnitudeSettings = + boost::dynamic_pointer_cast< ConstantThrustEngineSettings >( thrustMagnitudeSettings ); + if( constantThrustMagnitudeSettings == NULL ) + { + throw std::runtime_error( "Error when creating constant thrust magnitude wrapper, input is inconsistent" ); + } + + thrustMagnitudeWrapper = boost::make_shared< propulsion::CustomThrustMagnitudeWrapper >( + boost::lambda::constant( constantThrustMagnitudeSettings->thrustMagnitude_ ), + boost::lambda::constant( constantThrustMagnitudeSettings->specificImpulse_ ) ); + break; + + } + case from_engine_properties_thrust_magnitude: + { + // Check input consistency + boost::shared_ptr< FromBodyThrustEngineSettings > fromEngineThrustMagnitudeSettings = + boost::dynamic_pointer_cast< FromBodyThrustEngineSettings >( thrustMagnitudeSettings ); + if( fromEngineThrustMagnitudeSettings == NULL ) + { + throw std::runtime_error( "Error when creating from-engine thrust magnitude wrapper, input is inconsistent" ); + } + if( bodyMap.at( nameOfBodyWithGuidance )->getVehicleSystems( ) == NULL ) + { + throw std::runtime_error( "Error when creating from-engine thrust magnitude wrapper, no vehicle systems found" ); + + } + + // Retrieve single engine thrust + if( fromEngineThrustMagnitudeSettings->useAllEngines_ == false ) + { + if( ( bodyMap.at( nameOfBodyWithGuidance )->getVehicleSystems( )->getEngineModels( ).count( + thrustMagnitudeSettings->thrustOriginId_ ) == 0 ) ) + { + throw std::runtime_error( "Error when creating from-engine thrust magnitude wrapper, no engine of right ID found" ); + } + else + { + thrustMagnitudeWrapper = boost::make_shared< propulsion::ThrustMagnitudeFromEngineWrapper >( + bodyMap.at( nameOfBodyWithGuidance )->getVehicleSystems( )->getEngineModels( ).at( + thrustMagnitudeSettings->thrustOriginId_ ) ); + } + } + // Retrieve total engine thrust + else + { + if( ( bodyMap.at( nameOfBodyWithGuidance )->getVehicleSystems( )->getEngineModels( ).size( ) == 0 ) ) + { + std::cerr<<"Error when creating from-engine thrust magnitude wrapper for all engines; no engines found: returning 0 thrust"<( + utilities::createVectorFromMapValues< boost::shared_ptr< system_models::EngineModel >, std::string >( + bodyMap.at( nameOfBodyWithGuidance )->getVehicleSystems( )->getEngineModels( ) )); + } + break; + + } + case thrust_magnitude_from_time_function: + { + // Check input consistency + boost::shared_ptr< FromFunctionThrustEngineSettings > fromFunctionThrustMagnitudeSettings = + boost::dynamic_pointer_cast< FromFunctionThrustEngineSettings >( thrustMagnitudeSettings ); + if( fromFunctionThrustMagnitudeSettings == NULL ) + { + throw std::runtime_error( "Error when creating from-function thrust magnitude wrapper, input is inconsistent" ); + } + thrustMagnitudeWrapper = boost::make_shared< propulsion::CustomThrustMagnitudeWrapper >( + fromFunctionThrustMagnitudeSettings->thrustMagnitudeFunction_, + fromFunctionThrustMagnitudeSettings->specificImpulseFunction_, + fromFunctionThrustMagnitudeSettings->isEngineOnFunction_ ); + break; + + } + default: + throw std::runtime_error( "Error when creating thrust magnitude wrapper, type not identified" ); + } + + return thrustMagnitudeWrapper; +} + +//! Function to update the thrust magnitude and direction to current time. +void updateThrustMagnitudeAndDirection( + const boost::shared_ptr< propulsion::ThrustMagnitudeWrapper > thrustMagnitudeWrapper, + const boost::shared_ptr< propulsion::BodyFixedForceDirectionGuidance > thrustDirectionGuidance, + const double currentTime ) +{ + thrustMagnitudeWrapper->update( currentTime ); + thrustDirectionGuidance->updateCalculator( currentTime ); +} + +//! Function to reset the current time variable of the thrust magnitude and direction wrappers +void resetThrustMagnitudeAndDirectionTime( + const boost::shared_ptr< propulsion::ThrustMagnitudeWrapper > thrustMagnitudeWrapper, + const boost::shared_ptr< propulsion::BodyFixedForceDirectionGuidance > thrustDirectionGuidance, + const double currentTime ) +{ + thrustMagnitudeWrapper->resetCurrentTime( currentTime ); + thrustDirectionGuidance->resetCurrentTime( currentTime ); +} + + +} + +} + diff --git a/Tudat/SimulationSetup/PropagationSetup/createThrustModelGuidance.h b/Tudat/SimulationSetup/PropagationSetup/createThrustModelGuidance.h new file mode 100644 index 0000000000..0c1f110d86 --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/createThrustModelGuidance.h @@ -0,0 +1,101 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_CREATETHRUSTMODELGUIDANCE_H +#define TUDAT_CREATETHRUSTMODELGUIDANCE_H + +#include "Tudat/Astrodynamics/SystemModels/engineModel.h" +#include "Tudat/Astrodynamics/Propulsion/thrustGuidance.h" +#include "Tudat/Astrodynamics/Propagators/environmentUpdateTypes.h" +#include "Tudat/Astrodynamics/Propulsion/thrustMagnitudeWrapper.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createFlightConditions.h" +#include "Tudat/SimulationSetup/PropagationSetup/thrustSettings.h" +#include "Tudat/Astrodynamics/Ephemerides/ephemeris.h" +#include "Tudat/Astrodynamics/SystemModels/engineModel.h" + +namespace tudat +{ + +namespace simulation_setup +{ + + +//! Function to retrieve the effective thrust direction from a set of thrust sources. +/*! + * Function to retrieve the effective thrust direction from a set of thrust sources. + * \param thrustDirections List of functions returning thrust directions. + * \param thrustMagnitudes List of functions returning thrust magnitude. + * \return Effective thrust direction. + */ +Eigen::Vector3d getCombinedThrustDirection( + const std::vector< boost::function< Eigen::Vector3d( )> >& thrustDirections, + const std::vector< boost::function< double( )> >& thrustMagnitudes ); + +//! Function to create a function that returns the thrust direction in the body-fixed frame. +/*! + * Function to create a function that returns the thrust direction in the body-fixed frame. + * \param thrustMagnitudeSettings Settings for the thrust magnitude + * \param bodyMap List of body objects that comprises the environment + * \param bodyName Name of body for which thrust is to be created. + * \return Function that returns the thrust direction in the body-fixed frame. + */ +boost::function< Eigen::Vector3d( ) > getBodyFixedThrustDirection( + const boost::shared_ptr< ThrustEngineSettings > thrustMagnitudeSettings, + const NamedBodyMap& bodyMap, + const std::string bodyName ); + +//! Function to create a wrapper object that computes the thrust magnitude +/*! + * Function to create a wrapper object that computes the thrust magnitude + * \param thrustMagnitudeSettings Settings for the thrust magnitude + * \param bodyMap List of body objects that comprises the environment + * \param nameOfBodyWithGuidance Name of body for which thrust is to be created. + * \param magnitudeUpdateSettings Environment update settings that are required to compute the thrust direction (updated + * by function as needed). + * \return Object used during propagation to compute the thrust direction + */ +boost::shared_ptr< propulsion::ThrustMagnitudeWrapper > createThrustMagnitudeWrapper( + const boost::shared_ptr< ThrustEngineSettings > thrustMagnitudeSettings, + const NamedBodyMap& bodyMap, + const std::string& nameOfBodyWithGuidance, + std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > >& magnitudeUpdateSettings ); + +//! Function to update the thrust magnitude and direction to current time. +/*! + * Function to update the thrust magnitude and direction to current time. + * \param thrustMagnitudeWrapper Object used during propagation to compute the thrust magnitude + * \param thrustDirectionGuidance Object used during propagation to compute the body-fixed thrust direction + * \param currentTime Time to which objects are to be updated. + */ +void updateThrustMagnitudeAndDirection( + const boost::shared_ptr< propulsion::ThrustMagnitudeWrapper > thrustMagnitudeWrapper, + const boost::shared_ptr< propulsion::BodyFixedForceDirectionGuidance > thrustDirectionGuidance, + const double currentTime ); + +//! Function to reset the current time variable of the thrust magnitude and direction wrappers +/*! +* Function to reset the current time variable of the thrust magnitude and direction wrappers. This function does not +* update the actual thrust direction and guidance; it is typically used to reset the current time to NaN, thereby signalling +* the need to recompute the magnitude/direction upon next call to update functions +* \param thrustMagnitudeWrapper Object used during propagation to compute the thrust magnitude +* \param thrustDirectionGuidance Object used during propagation to compute the body-fixed thrust direction +* \param currentTime New current time variable that is to be set. +*/ +void resetThrustMagnitudeAndDirectionTime( + const boost::shared_ptr< propulsion::ThrustMagnitudeWrapper > thrustMagnitudeWrapper, + const boost::shared_ptr< propulsion::BodyFixedForceDirectionGuidance > thrustDirectionGuidance, + const double currentTime = TUDAT_NAN ); + +} // namespace simulation_setup + +} // namespace tudat + +#endif // TUDAT_CREATETHRUSTMODELGUIDANCE_H diff --git a/Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.cpp b/Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.cpp new file mode 100644 index 0000000000..9e4bfaebe5 --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.cpp @@ -0,0 +1,29 @@ +#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h" + +namespace tudat +{ + +namespace propagators +{ + +boost::shared_ptr< ephemerides::ReferenceFrameManager > createFrameManager( + const simulation_setup::NamedBodyMap& bodyMap ) +{ + // Get ephemerides from bodies + std::map< std::string, boost::shared_ptr< ephemerides::Ephemeris > > ephemerides; + for( simulation_setup::NamedBodyMap::const_iterator bodyIterator = bodyMap.begin( ); + bodyIterator != bodyMap.end( ); bodyIterator++ ) + { + if( bodyIterator->second->getEphemeris( ) != NULL ) + { + ephemerides[ bodyIterator->first ] = bodyIterator->second->getEphemeris( ); + } + } + return boost::make_shared< ephemerides::ReferenceFrameManager >( + ephemerides ); +} + +} + +} + diff --git a/Tudat/Astrodynamics/Propagators/dynamicsSimulator.h b/Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h similarity index 58% rename from Tudat/Astrodynamics/Propagators/dynamicsSimulator.h rename to Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h index 806906d2b2..aafb0d7436 100644 --- a/Tudat/Astrodynamics/Propagators/dynamicsSimulator.h +++ b/Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h @@ -11,6 +11,9 @@ #ifndef TUDAT_DYNAMICSSIMULATOR_H #define TUDAT_DYNAMICSSIMULATOR_H +#include +#include + #include #include @@ -18,7 +21,7 @@ #include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaVariableStepSizeIntegrator.h" #include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h" #include "Tudat/Mathematics/Interpolators/cubicSplineInterpolator.h" - +#include "Tudat/Basics/utilities.h" #include "Tudat/Astrodynamics/Propagators/nBodyStateDerivative.h" #include "Tudat/Astrodynamics/Ephemerides/frameManager.h" #include "Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h" @@ -26,11 +29,12 @@ #include "Tudat/Astrodynamics/Ephemerides/tabulatedEphemeris.h" #include "Tudat/Astrodynamics/Ephemerides/compositeEphemeris.h" #include "Tudat/Astrodynamics/Propagators/nBodyCowellStateDerivative.h" -#include "Tudat/Astrodynamics/Propagators/propagationSettings.h" -#include "Tudat/Astrodynamics/Propagators/setNumericallyIntegratedStates.h" +#include "Tudat/SimulationSetup/PropagationSetup/propagationSettings.h" +#include "Tudat/SimulationSetup/PropagationSetup/setNumericallyIntegratedStates.h" #include "Tudat/Astrodynamics/Propagators/integrateEquations.h" -#include "Tudat/Astrodynamics/Propagators/createStateDerivativeModel.h" -#include "Tudat/Astrodynamics/Propagators/createEnvironmentUpdater.h" +#include "Tudat/SimulationSetup/PropagationSetup/createStateDerivativeModel.h" +#include "Tudat/SimulationSetup/PropagationSetup/createEnvironmentUpdater.h" +#include "Tudat/SimulationSetup/PropagationSetup/propagationTermination.h" #include "Tudat/Astrodynamics/Propagators/dynamicsStateDerivativeModel.h" #include "Tudat/Mathematics/Interpolators/lagrangeInterpolator.h" @@ -40,17 +44,15 @@ namespace tudat namespace propagators { -//! Function to get the states of a set of bodies +//! Function to get the states of a set of bodies, w.r.t. some set of central bodies, at the requested time. /*! -* Function to get the states of a set of bodies, w.r.t. some set of -* central bodies, at the requested time. +* Function to get the states of a set of bodies, w.r.t. some set of central bodies, at the requested time. * \param bodiesToIntegrate List of bodies for which to retrieve state. * \param centralBodies Origins w.r.t. which to retrieve states of bodiesToIntegrate. * \param bodyMap List of bodies to use in simulations. * \param initialTime Time at which to retrieve states. * \param frameManager OBject with which to calculate frame origin translations. -* \return Initial state vector (with 6 Cartesian elements per body, in -* order of bodiesToIntegrate vector). +* \return Initial state vector (with 6 Cartesian elements per body, in order of bodiesToIntegrate vector). */ template< typename TimeType = double, typename StateScalarType = double > Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > getInitialStatesOfBodies( @@ -62,8 +64,7 @@ Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > getInitialStatesOfBodies( { // Set initial states of bodies to integrate. Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > systemInitialState = - Eigen::Matrix< StateScalarType, - Eigen::Dynamic, 1 >::Zero( bodiesToIntegrate.size( ) * 6, 1 ); + Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >::Zero( bodiesToIntegrate.size( ) * 6, 1 ); boost::shared_ptr< ephemerides::Ephemeris > ephemerisOfCurrentBody; // Iterate over all bodies. @@ -72,35 +73,34 @@ Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > getInitialStatesOfBodies( ephemerisOfCurrentBody = bodyMap.at( bodiesToIntegrate.at( i ) )->getEphemeris( ); // Get body initial state from ephemeris - systemInitialState.segment( i * 6 , 6 ) - = ephemerisOfCurrentBody->getTemplatedStateFromEphemeris< + systemInitialState.segment( i * 6 , 6 ) = ephemerisOfCurrentBody->getTemplatedStateFromEphemeris< StateScalarType, TimeType >( initialTime ); // Correct initial state if integration origin and ephemeris origin are not equal. if( centralBodies.at( i ) != ephemerisOfCurrentBody->getReferenceFrameOrigin( ) ) { - boost::shared_ptr< ephemerides::Ephemeris > correctionEphemeris - = frameManager->getEphemeris( ephemerisOfCurrentBody->getReferenceFrameOrigin( ), - centralBodies.at( i ) ); - systemInitialState.segment( i * 6 , 6 ) - -= correctionEphemeris->getTemplatedStateFromEphemeris< + boost::shared_ptr< ephemerides::Ephemeris > correctionEphemeris = + frameManager->getEphemeris( ephemerisOfCurrentBody->getReferenceFrameOrigin( ), centralBodies.at( i ) ); + systemInitialState.segment( i * 6 , 6 ) -= correctionEphemeris->getTemplatedStateFromEphemeris< StateScalarType, TimeType >( initialTime ); } } return systemInitialState; } -//! Function to get the states of a set of bodies + +boost::shared_ptr< ephemerides::ReferenceFrameManager > createFrameManager( + const simulation_setup::NamedBodyMap& bodyMap ); + +//! Function to get the states of a set of bodies, w.r.t. some set of central bodies, at the requested time. /*! -* Function to get the states of a set of bodies, w.r.t. some set of -* central bodies, at the requested time, creates +* Function to get the states of a set of bodies, w.r.t. some set of central bodies, at the requested time, creates * frameManager from input data. * \param bodiesToIntegrate List of bodies for which to retrieve state. * \param centralBodies Origins w.r.t. which to retrieve states of bodiesToIntegrate. * \param bodyMap List of bodies to use in simulations. * \param initialTime Time at which to retrieve states. -* \return Initial state vector (with 6 Cartesian elements per body, in -* order of bodiesToIntegrate vector). +* \return Initial state vector (with 6 Cartesian elements per body, in order of bodiesToIntegrate vector). */ template< typename TimeType = double, typename StateScalarType = double > Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > getInitialStatesOfBodies( @@ -111,15 +111,13 @@ Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > getInitialStatesOfBodies( { // Create ReferenceFrameManager and call overloaded function. return getInitialStatesOfBodies( bodiesToIntegrate, centralBodies, bodyMap, initialTime, - boost::make_shared< ephemerides::ReferenceFrameManager > - ( bodyMap ) ); + createFrameManager( bodyMap ) ); } -//! Function to get the states of bodyToIntegrate +//! Function to get the states of single body, w.r.t. some central body, at the requested time. /*! -* Function to get the states of single, w.r.t. some set of central -* body, at the requested time., creates -* frameManager from input data. +* Function to get the states of single body, w.r.t. some central body, at the requested time. This function creates +* frameManager from input data to perform all required conversions. * \param bodyToIntegrate Bodies for which to retrieve state * \param centralBody Origins w.r.t. which to retrieve state of bodyToIntegrate. * \param bodyMap List of bodies to use in simulations. @@ -134,17 +132,15 @@ Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > getInitialStateOfBody( const TimeType initialTime ) { return getInitialStatesOfBodies< TimeType, StateScalarType >( - boost::assign::list_of( bodyToIntegrate ), boost::assign::list_of( centralBody ), - bodyMap, initialTime ); + boost::assign::list_of( bodyToIntegrate ), boost::assign::list_of( centralBody ), bodyMap, initialTime ); } //! Base class for performing full numerical integration of a dynamical system. /*! - * Base class for performing full numerical integration of a - * dynamical system. Governing equations are set once, + * Base class for performing full numerical integration of a dynamical system. Governing equations are set once, * but can be re-integrated for different initial conditions using the same instance of the class. * Derived classes define the specific kind of integration that is performed - * (single-arc/multi-arc; dynamics/variational equations, etc.) + * (single-arc/multi-arc/etc.) */ template< typename StateScalarType = double, typename TimeType = double > class DynamicsSimulator @@ -153,33 +149,29 @@ class DynamicsSimulator //! Constructor of simulator. /*! - * Constructor of simulator, constructs integrator and object for - * calculating each time step of integration. + * Constructor of simulator, constructs integrator and object for calculating each time step of integration. * \param bodyMap Map of bodies (with names) of all bodies in integration. * \param integratorSettings Settings for numerical integrator. * \param propagatorSettings Settings for propagator. - * \param clearNumericalSolutions Boolean to determine whether to - * clear the raw numerical solution member variables (default true) - * after propagation and resetting ephemerides. - * \param setIntegratedResult Boolean to determine whether to (default true). - * automatically use the integrated results to set ephemerides. + * \param clearNumericalSolutions Boolean to determine whether to clear the raw numerical solution member variables + * after propagation and resetting ephemerides (default true). + * \param setIntegratedResult Boolean to determine whether to automatically use the integrated results to set + * ephemerides (default true). */ DynamicsSimulator( const simulation_setup::NamedBodyMap& bodyMap, - const boost::shared_ptr< numerical_integrators::IntegratorSettings< TimeType > > - integratorSettings, + const boost::shared_ptr< numerical_integrators::IntegratorSettings< TimeType > > integratorSettings, const boost::shared_ptr< PropagatorSettings< StateScalarType > > propagatorSettings, const bool clearNumericalSolutions = true, const bool setIntegratedResult = true ): bodyMap_( bodyMap ), integratorSettings_( integratorSettings ), - propagatorSettings_( propagatorSettings ), - clearNumericalSolutions_( clearNumericalSolutions ), + propagatorSettings_( propagatorSettings ), clearNumericalSolutions_( clearNumericalSolutions ), setIntegratedResult_( setIntegratedResult ) { - frameManager_ = boost::make_shared< ephemerides::ReferenceFrameManager >( bodyMap ); - if( setIntegratedResult ) + if( setIntegratedResult_ ) { + frameManager_ = createFrameManager( bodyMap ); integratedStateProcessors_ = createIntegratedStateProcessors< TimeType, StateScalarType >( propagatorSettings_, bodyMap_, frameManager_ ); } @@ -188,10 +180,35 @@ class DynamicsSimulator propagatorSettings_, bodyMap_ ); dynamicsStateDerivative_ = boost::make_shared< DynamicsStateDerivativeModel< TimeType, StateScalarType > >( createStateDerivativeModels< StateScalarType, TimeType >( - propagatorSettings_, bodyMap_ ), environmentUpdater_ ); + propagatorSettings_, bodyMap_, integratorSettings_->initialTime_ ), + boost::bind( &EnvironmentUpdater< StateScalarType, TimeType >::updateEnvironment, + environmentUpdater_, _1, _2, _3 ) ); + propagationTerminationCondition_ = createPropagationTerminationConditions( + propagatorSettings->getTerminationSettings( ), bodyMap_, integratorSettings->initialTimeStep_ ); + + if( propagatorSettings_->getDependentVariablesToSave( ) != NULL ) + { + std::pair< boost::function< Eigen::VectorXd( ) >, std::map< int, std::string > > dependentVariableData = + createDependentVariableListFunction< TimeType, StateScalarType >( + propagatorSettings_->getDependentVariablesToSave( ), bodyMap_, + dynamicsStateDerivative_->getStateDerivativeModels( ) ); + dependentVariablesFunctions_ = dependentVariableData.first; + dependentVariableIds_ = dependentVariableData.second; + + if( propagatorSettings_->getDependentVariablesToSave( )->printDependentVariableTypes_ ) + { + std::cout<<"Dependent variables being saved, output vectors contain: "<::computeStateDerivative, + boost::bind( &DynamicsStateDerivativeModel< TimeType, StateScalarType >::computeStateDerivative, + dynamicsStateDerivative_, _1, _2 ); + doubleStateDerivativeFunction_ = + boost::bind( &DynamicsStateDerivativeModel< TimeType, StateScalarType >::computeStateDoubleDerivative, dynamicsStateDerivative_, _1, _2 ); } @@ -200,26 +217,22 @@ class DynamicsSimulator //! This function numerically (re-)integrates the equations of motion. /*! - * This function numerically (re-)integrates the equations of motion, using the settings set - * through the constructor and a new initial state vector provided here. The raw results are - * set in the equationsOfMotionNumericalSolution_ - * \param initialGlobalStates Initial state vector that is to be used for numerical - * integration. Note that this state should be in the correct frame (i.e. corresponding to - * centralBodies in propagatorSettings_), but not in the propagator-specific form (i.e Encke, - * Gauss, etc. for translational dynamics) - * \sa SingleStateTypeDerivative::convertToOutputSolution + * This function numerically (re-)integrates the equations of motion, using the settings set through the constructor + * and a new initial state vector provided here. The raw results are set in the equationsOfMotionNumericalSolution_ + * \param initialGlobalStates Initial state vector that is to be used for numerical integration. + * Note that this state should be in the correct frame (i.e. corresponding to centralBodies in propagatorSettings_), + * but not in the propagator-specific form (i.e Encke, Gauss, etc. for translational dynamics) + * \sa SingleStateTypeDerivative::convertToOutputSolution */ virtual void integrateEquationsOfMotion( - const Eigen::Matrix< StateScalarType, - Eigen::Dynamic, Eigen::Dynamic >& initialGlobalStates ) = 0; + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& initialGlobalStates ) = 0; //! Function to get the settings for the numerical integrator. /*! * Function to get the settings for the numerical integrator. * \return The settings for the numerical integrator. */ - boost::shared_ptr< numerical_integrators::IntegratorSettings< TimeType > > - getIntegratorSettings( ) + boost::shared_ptr< numerical_integrators::IntegratorSettings< TimeType > > getIntegratorSettings( ) { return integratorSettings_; } @@ -236,6 +249,18 @@ class DynamicsSimulator return stateDerivativeFunction_; } + //! Function to get the function that performs a single state derivative function evaluation with double precision. + /*! + * Function to get the function that performs a single state derivative function evaluation with double precision, + * regardless of template arguments. + * \return Function that performs a single state derivative function evaluation with double precision. + */ + boost::function< Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > + ( const double, const Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic >& ) > getDoubleStateDerivativeFunction( ) + { + return doubleStateDerivativeFunction_; + } + //! Function to get the settings for the propagator. /*! * Function to get the settings for the propagator. @@ -246,11 +271,10 @@ class DynamicsSimulator return propagatorSettings_; } - //! Function to get the object that updates the environment + //! Function to get the object that updates the environment. /*! - * Function to get the object responsible for updating the environment based on the current - * state and time. - * \return Object resposible for updating the environment based on the current state and time. + * Function to get the object responsible for updating the environment based on the current state and time. + * \return Object responsible for updating the environment based on the current state and time. */ boost::shared_ptr< EnvironmentUpdater< StateScalarType, TimeType > > getEnvironmentUpdater( ) { @@ -259,13 +283,10 @@ class DynamicsSimulator //! Function to get the object that updates and returns state derivative /*! - * Function to get the object that updates current environment and returns state derivative from - * single function call. - * \return Object that updates current environment and returns state derivative from single - * function call + * Function to get the object that updates current environment and returns state derivative from single function call + * \return Object that updates current environment and returns state derivative from single function call */ - boost::shared_ptr< DynamicsStateDerivativeModel< TimeType, StateScalarType > > - getDynamicsStateDerivative( ) + boost::shared_ptr< DynamicsStateDerivativeModel< TimeType, StateScalarType > > getDynamicsStateDerivative( ) { return dynamicsStateDerivative_; } @@ -280,47 +301,52 @@ class DynamicsSimulator return bodyMap_; } + boost::shared_ptr< PropagationTerminationCondition > getPropagationTerminationCondition( ) + { + return propagationTerminationCondition_; + } + protected: //! This function updates the environment with the numerical solution of the propagation. /*! - * This function updates the environment with the numerical solution of the propagation. For - * instance, it sets the propagated translational dynamics solution as the new input for the - * Ephemeris object of the body that was propagated. This function is pure virtual and must be - * implemented in the derived class. + * This function updates the environment with the numerical solution of the propagation. For instance, it sets + * the propagated translational dynamics solution as the new input for the Ephemeris object of the body that was + * propagated. This function is pure virtual and must be implemented in the derived class. */ virtual void processNumericalEquationsOfMotionSolution( ) = 0; - //! List of object (per dynamics type) that process the integrated numerical solution by - //! updating the environment + //! List of object (per dynamics type) that process the integrated numerical solution by updating the environment std::map< IntegratedStateType, std::vector< boost::shared_ptr< IntegratedStateProcessor< TimeType, StateScalarType > > > > integratedStateProcessors_; //! Object responsible for updating the environment based on the current state and time. /*! - - * Object responsible for updating the environment based on the current state and time. Calling - * the updateEnvironment function automatically updates all dependent variables that are needed - * to calulate the state derivative. + * Object responsible for updating the environment based on the current state and time. Calling the updateEnvironment + * function automatically updates all dependent variables that are needed to calulate the state derivative. */ boost::shared_ptr< EnvironmentUpdater< StateScalarType, TimeType > > environmentUpdater_; - //! Interface object that updates current environment and returns state derivative from single - //! function call. - boost::shared_ptr< DynamicsStateDerivativeModel< TimeType, StateScalarType > > - dynamicsStateDerivative_; + //! Interface object that updates current environment and returns state derivative from single function call. + boost::shared_ptr< DynamicsStateDerivativeModel< TimeType, StateScalarType > > dynamicsStateDerivative_; //! Function that performs a single state derivative function evaluation. /*! - * Function that performs a single state derivative function evaluation, will typically be set - * to DynamicsStateDerivativeModel< TimeType, StateScalarType >::computeStateDerivative - * function. Calling this function will first update the environment (using - * environmentUpdater_) and then calculate the full system state derivative. + * Function that performs a single state derivative function evaluation, will typically be set to + * DynamicsStateDerivativeModel< TimeType, StateScalarType >::computeStateDerivative function. + * Calling this function will first update the environment (using environmentUpdater_) and then calculate the + * full system state derivative. */ boost::function< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > - ( const TimeType, const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& ) > - stateDerivativeFunction_; + ( const TimeType, const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& ) > stateDerivativeFunction_; + //! Function that performs a single state derivative function evaluation with double precision. + /*! + * Function that performs a single state derivative function evaluation with double precision + * \sa stateDerivativeFunction_ + */ + boost::function< Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > + ( const double, const Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic >& ) > doubleStateDerivativeFunction_; //! Map of bodies (with names) of all bodies in integration. simulation_setup::NamedBodyMap bodyMap_; @@ -331,11 +357,20 @@ class DynamicsSimulator //! Settings for propagator. boost::shared_ptr< PropagatorSettings< StateScalarType > > propagatorSettings_; + //! Object defining when the propagation is to be terminated. + boost::shared_ptr< PropagationTerminationCondition > propagationTerminationCondition_; + + //! Function returning dependent variables (during numerical propagation) + boost::function< Eigen::VectorXd( ) > dependentVariablesFunctions_; + + //! Map listing starting entry of dependent variables in output vector, along with associated ID. + std::map< int, std::string > dependentVariableIds_; + //! Object for retrieving ephemerides for transformation of reference frame (origins) boost::shared_ptr< ephemerides::ReferenceFrameManager > frameManager_; - //! Boolean to determine whether to clear the raw numerical solution member variables after - //! propagation and resetting ephemerides. + //! Boolean to determine whether to clear the raw numerical solution member variables after propagation and + //! resetting ephemerides. bool clearNumericalSolutions_; //! Boolean to determine whether to automatically use the integrated results to set ephemerides. @@ -344,13 +379,12 @@ class DynamicsSimulator }; -//! Class for performing full numerical integration of a dynamical system in a single arc.. +//! Class for performing full numerical integration of a dynamical system in a single arc. /*! - * Class for performing full numerical integration of a dynamical system in a single arc, i.e. the - * equations of motion have a single initial time, and are propagated once for the full prescribed - * time interval. This is in contrast to multi-arc dynamics, where the time interval si cut into - * pieces. In this class, the governing equations are set once, but can be re-integrated for - * different initial conditions using the same instance of the class. + * Class for performing full numerical integration of a dynamical system in a single arc, i.e. the equations of motion + * have a single initial time, and are propagated once for the full prescribed time interval. This is in contrast to + * multi-arc dynamics, where the time interval si cut into pieces. In this class, the governing equations are set once, + * but can be re-integrated for different initial conditions using the same instance of the class. */ template< typename StateScalarType = double, typename TimeType = double > class SingleArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, TimeType > @@ -366,33 +400,32 @@ class SingleArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim using DynamicsSimulator< StateScalarType, TimeType >::integratorSettings_; using DynamicsSimulator< StateScalarType, TimeType >::propagatorSettings_; using DynamicsSimulator< StateScalarType, TimeType >::integratedStateProcessors_; + using DynamicsSimulator< StateScalarType, TimeType >::propagationTerminationCondition_; + using DynamicsSimulator< StateScalarType, TimeType >::dependentVariablesFunctions_; //! Constructor of simulator. /*! - * Constructor of simulator, constructs integrator and object for calculating each time step of - * integration. + * Constructor of simulator, constructs integrator and object for calculating each time step of integration. * \param bodyMap Map of bodies (with names) of all bodies in integration. * \param integratorSettings Settings for numerical integrator. * \param propagatorSettings Settings for propagator. - * \param areEquationsOfMotionToBeIntegrated Boolean to denote whether equations of motion - * should be integrated immediately at the end of the contructor or not. - * \param clearNumericalSolutions Boolean to determine whether to clear the raw numerical - * solution member variables after propagation and resetting ephemerides. - * \param setIntegratedResult Boolean to determine whether to automatically use the integrated - * results to set ephemerides. + * \param areEquationsOfMotionToBeIntegrated Boolean to denote whether equations of motion should be integrated + * immediately at the end of the contructor or not (default true). + * \param clearNumericalSolutions Boolean to determine whether to clear the raw numerical solution member variables + * after propagation and resetting ephemerides (default true). + * \param setIntegratedResult Boolean to determine whether to automatically use the integrated results to set + * ephemerides (default true). */ SingleArcDynamicsSimulator( const simulation_setup::NamedBodyMap& bodyMap, - const boost::shared_ptr< numerical_integrators::IntegratorSettings< TimeType > > - integratorSettings, + const boost::shared_ptr< numerical_integrators::IntegratorSettings< TimeType > > integratorSettings, const boost::shared_ptr< PropagatorSettings< StateScalarType > > propagatorSettings, const bool areEquationsOfMotionToBeIntegrated = true, const bool clearNumericalSolutions = true, const bool setIntegratedResult = true ): DynamicsSimulator< StateScalarType, TimeType >( - bodyMap, integratorSettings, propagatorSettings, clearNumericalSolutions, - setIntegratedResult ) + bodyMap, integratorSettings, propagatorSettings, clearNumericalSolutions, setIntegratedResult ) { // Integrate equations of motion if required. if( areEquationsOfMotionToBeIntegrated ) @@ -407,13 +440,11 @@ class SingleArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim //! This function numerically (re-)integrates the equations of motion. /*! - * This function numerically (re-)integrates the equations of motion, using the settings set - * through the constructor and a new initial state vector provided here. The raw results are - * set in the equationsOfMotionNumericalSolution_ - * \param initialStates Initial state vector that is to be used for numerical integration. Note - * that this state should be in the correct frame (i.e. corresponding to centralBodies in - * propagatorSettings_), but not in the propagator- specific form (i.e Encke, Gauss, etc. for - * translational dynamics) + * This function numerically (re-)integrates the equations of motion, using the settings set through the constructor + * and a new initial state vector provided here. The raw results are set in the equationsOfMotionNumericalSolution_ + * \param initialStates Initial state vector that is to be used for numerical integration. Note that this state should + * be in the correct frame (i.e. corresponding to centralBodies in propagatorSettings_), but not in the propagator- + * specific form (i.e Encke, Gauss, etc. for translational dynamics) * \sa SingleStateTypeDerivative::convertToOutputSolution */ void integrateEquationsOfMotion( @@ -422,13 +453,18 @@ class SingleArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim equationsOfMotionNumericalSolution_.clear( ); - dynamicsStateDerivative_->setPropagationSettings( std::vector< IntegratedStateType >( ), 1 ); + dynamicsStateDerivative_->setPropagationSettings( std::vector< IntegratedStateType >( ), 1, 0 ); // Integrate equations of motion numerically. - equationsOfMotionNumericalSolution_ = - integrateEquations< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >, TimeType >( - stateDerivativeFunction_, dynamicsStateDerivative_->convertFromOutputSolution( - initialStates, integratorSettings_->initialTime_ ), integratorSettings_ ); + integrateEquations< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >, TimeType >( + stateDerivativeFunction_, equationsOfMotionNumericalSolution_, + dynamicsStateDerivative_->convertFromOutputSolution( + initialStates, integratorSettings_->initialTime_ ), integratorSettings_, + boost::bind( &PropagationTerminationCondition::checkStopCondition, + propagationTerminationCondition_, _1 ), + dependentVariableHistory_, + dependentVariablesFunctions_, + propagatorSettings_->getPrintInterval( ) ); equationsOfMotionNumericalSolution_ = dynamicsStateDerivative_-> convertNumericalStateSolutionsToOutputSolutions( equationsOfMotionNumericalSolution_ ); @@ -443,16 +479,26 @@ class SingleArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim * Function to return the map of state history of numerically integrated bodies. * \return Map of state history of numerically integrated bodies. */ - std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > - getEquationsOfMotionNumericalSolution( ) + std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > getEquationsOfMotionNumericalSolution( ) { return equationsOfMotionNumericalSolution_; } + //! Function to return the map of dependent variable history that was saved during numerical propagation. + /*! + * Function to return the map of dependent variable history that was saved during numerical propagation. + * \return Map of dependent variable history that was saved during numerical propagation. + */ + std::map< TimeType, Eigen::VectorXd > getDependentVariableHistory( ) + { + return dependentVariableHistory_; + } + + //! Function to reset the environment from an externally generated state history. /*! - * Function to reset the environment from an externally generated state history, the order of - * the entries in the state vectors are proscribed by propagatorSettings + * Function to reset the environment from an externally generated state history, the order of the entries in the + * state vectors are proscribed by propagatorSettings * \param equationsOfMotionNumericalSolution Externally generated state history. */ void manuallySetAndProcessRawNumericalEquationsOfMotionSolution( @@ -468,9 +514,9 @@ class SingleArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim //! This function updates the environment with the numerical solution of the propagation. /*! - * This function updates the environment with the numerical solution of the propagation. It - * sets the propagated translational dynamics solution as the new input for the Ephemeris - * object of the body that was propagated. + * This function updates the environment with the numerical solution of the propagation. It sets + * the propagated translational dynamics solution as the new input for the Ephemeris object of the body that was + * propagated. */ void processNumericalEquationsOfMotionSolution( ) { @@ -484,7 +530,7 @@ class SingleArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim equationsOfMotionNumericalSolution_.clear( ); } - for( std::map< std::string, boost::shared_ptr< simulation_setup::Body > >::const_iterator + for( simulation_setup::NamedBodyMap::const_iterator bodyIterator = bodyMap_.begin( ); bodyIterator != bodyMap_.end( ); bodyIterator++ ) { @@ -494,15 +540,16 @@ class SingleArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim //! Map of state history of numerically integrated bodies. /*! - * Map of state history of numerically integrated bodies, i.e. the result of the numerical - * integration, transformed into the 'conventional form' (\sa - * SingleStateTypeDerivative::convertToOutputSolution). Key of map denotes time, values are - * concatenated vectors of integrated body states (order defined by propagatorSettings_). - * + * Map of state history of numerically integrated bodies, i.e. the result of the numerical integration, transformed + * into the 'conventional form' (\sa SingleStateTypeDerivative::convertToOutputSolution). Key of map denotes time, + * values are concatenated vectors of integrated body states (order defined by propagatorSettings_). * NOTE: this map is empty if clearNumericalSolutions_ is set to true. */ - std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > - equationsOfMotionNumericalSolution_; + std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > equationsOfMotionNumericalSolution_; + + //! Map of dependent variable history that was saved during numerical propagation. + std::map< TimeType, Eigen::VectorXd > dependentVariableHistory_; + }; } // namespace propagators diff --git a/Tudat/Astrodynamics/Propagators/environmentUpdater.h b/Tudat/SimulationSetup/PropagationSetup/environmentUpdater.h similarity index 56% rename from Tudat/Astrodynamics/Propagators/environmentUpdater.h rename to Tudat/SimulationSetup/PropagationSetup/environmentUpdater.h index 52a7f02302..d45a5b8fc2 100644 --- a/Tudat/Astrodynamics/Propagators/environmentUpdater.h +++ b/Tudat/SimulationSetup/PropagationSetup/environmentUpdater.h @@ -17,10 +17,14 @@ #include #include +#include +#include +#include -#include "Tudat/SimulationSetup/body.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" #include "Tudat/Astrodynamics/Gravitation/timeDependentSphericalHarmonicsGravityField.h" -#include "Tudat/Astrodynamics/Propagators/propagationSettings.h" +#include "Tudat/SimulationSetup/PropagationSetup/propagationSettings.h" +#include "Tudat/Astrodynamics/Propagators/environmentUpdateTypes.h" namespace tudat { @@ -28,16 +32,6 @@ namespace tudat namespace propagators { -//! Enum defining types of environment model updates that can be done. -enum EnvironmentModelsToUpdate -{ - body_transational_state_update = 0, - body_rotational_state_update = 1, - body_mass_update = 2, - spherical_harmonic_gravity_field_update = 3, - vehicle_flight_conditions_update = 4, - radiation_pressure_interface_update = 5 -}; //! Class used to update the environment during numerical integration. /*! @@ -96,7 +90,7 @@ class EnvironmentUpdater */ void updateEnvironment( const TimeType currentTime, - const std::map< IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& + const std::unordered_map< IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& integratedStatesToSet, const std::vector< IntegratedStateType >& setIntegratedStatesFromEnvironment = std::vector< IntegratedStateType >( ) ) @@ -110,46 +104,22 @@ class EnvironmentUpdater " " + boost::lexical_cast< std::string >( integratedStates_.size( ) ) ); } + for( unsigned int i = 0; i < resetFunctionVector_.size( ); i++ ) + { + resetFunctionVector_.at( i ).get< 2 >( )( ); + } + // Set integrated state variables in environment. setIntegratedStatesInEnvironment( integratedStatesToSet ); // Set current state from environment for override settings setIntegratedStatesFromEnvironment setStatesFromEnvironment( setIntegratedStatesFromEnvironment, currentTime ); - // Set states of bodies which are not numerically integrated . - for( outerCurrentStateFromEnvironmentIterator_ = currentStateFromEnvironmentList_.begin( ); - outerCurrentStateFromEnvironmentIterator_ != currentStateFromEnvironmentList_.end( ); - outerCurrentStateFromEnvironmentIterator_++ ) - { - for( currentStateFromEnvironmentIterator_ = outerCurrentStateFromEnvironmentIterator_->second.begin( ); - currentStateFromEnvironmentIterator_ != outerCurrentStateFromEnvironmentIterator_->second.end( ); - currentStateFromEnvironmentIterator_++ ) - { - currentStateFromEnvironmentIterator_->second( currentTime ); - } - } - - // Evaluate update functions (dependent variables of state and time) determined by setUpdateFunctions - for( updateFunctionIterator = updateFunctionList_.begin( ); - updateFunctionIterator != updateFunctionList_.end( ); - updateFunctionIterator++ ) - { - for( unsigned int i = 0; i < updateFunctionIterator->second.size( ); i++ ) - { - updateFunctionIterator->second.at( i ).second( ); - } - } - // Evaluate time-dependent update functions (dependent variables of state and time) // determined by setUpdateFunctions - for( updateTimeIterator = updateTimeFunctionList_.begin( ); - updateTimeIterator != updateTimeFunctionList_.end( ); - updateTimeIterator++ ) + for( unsigned int i = 0; i < updateFunctionVector_.size( ); i++ ) { - for( unsigned int i = 0; i < updateTimeIterator->second.size( ); i++ ) - { - updateTimeIterator->second.at( i ).second( static_cast< double >( currentTime ) ); - } + updateFunctionVector_.at( i ).get< 2 >( )( currentTime ); } } @@ -164,35 +134,43 @@ class EnvironmentUpdater * \param integratedStatesToSet Integrated states which are to be set in environment. */ void setIntegratedStatesInEnvironment( - const std::map< IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& + const std::unordered_map< IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& integratedStatesToSet ) { // Iterate over state types and set states in environment - for( typename std::map< IntegratedStateType, - Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >::const_iterator - integratedStateIterator = integratedStatesToSet.begin( ); - integratedStateIterator != integratedStatesToSet.end( ); - integratedStateIterator++ ) + for( integratedStateIterator_ = integratedStatesToSet.begin( ); + integratedStateIterator_ != integratedStatesToSet.end( ); + integratedStateIterator_++ ) { - - switch( integratedStateIterator->first ) + switch( integratedStateIterator_->first ) { case transational_state: { // Set translational states for bodies provided as input. - std::vector< std::pair< std::string, std::string > > bodiesWithIntegratedStates = - integratedStates_.at( transational_state ); - for( unsigned int i = 0; i < bodiesWithIntegratedStates.size( ); i++ ) + for( unsigned int i = 0; i < integratedStates_[ transational_state ].size( ); i++ ) { - bodyList_[ bodiesWithIntegratedStates[ i ].first ] - ->template setTemplatedState< StateScalarType >( - integratedStateIterator->second.segment( i * 6, 6 ) ); + bodyList_[ integratedStates_[ transational_state ][ i ].first ]->template + setTemplatedState< StateScalarType >( + integratedStateIterator_->second.segment( i * 6, 6 ) ); } break; }; + case body_mass_state: + { + // Set mass for bodies provided as input. + std::vector< std::pair< std::string, std::string > > bodiesWithIntegratedMass = + integratedStates_.at( body_mass_state ); + + for( unsigned int i = 0; i < bodiesWithIntegratedMass.size( ); i++ ) + { + bodyList_[ bodiesWithIntegratedMass[ i ].first ] + ->setConstantBodyMass( integratedStateIterator_->second( i ) ); + } + break; + }; default: throw std::runtime_error( "Error, could not find integrated state settings for " + - boost::lexical_cast< std::string >( integratedStateIterator->first ) ); + boost::lexical_cast< std::string >( integratedStateIterator_->first ) ); } } } @@ -217,7 +195,7 @@ class EnvironmentUpdater { // Iterate over all integrated translational states. std::vector< std::pair< std::string, std::string > > bodiesWithIntegratedStates = - integratedStates_.at( transational_state ); + integratedStates_[ transational_state ]; for( unsigned int i = 0; i < bodiesWithIntegratedStates.size( ); i++ ) { bodyList_[ bodiesWithIntegratedStates[ i ].first ]-> @@ -226,6 +204,19 @@ class EnvironmentUpdater } break; } + case body_mass_state: + { + // Iterate over all integrated masses. + std::vector< std::pair< std::string, std::string > > bodiesWithIntegratedStates = + integratedStates_.at( body_mass_state ); + for( unsigned int i = 0; i < bodiesWithIntegratedStates.size( ); i++ ) + { + bodyList_[ bodiesWithIntegratedStates[ i ].first ]-> + updateMass( currentTime ); + + } + break; + } default: throw std::runtime_error( "Error, could not find state settings for " + boost::lexical_cast< std::string >( statesToSet.at( i ) ) ); @@ -233,6 +224,124 @@ class EnvironmentUpdater } } + //! Function to set the order in which the updateFunctionVector_ is to be updated. + /*! + * Function to set the order in which the updateFunctionVector_ is to be updated. Order is determined recursively in + * this function, the variable iterationNumber keeps track of the number of nested calls to the function. + * \param iterationNumber Number of subsequent calls to this funtion + */ + void setUpdateFunctionOrder( const int iterationNumber = 0 ) + { + bool rerunUpdateOrder = 0; + + // Iterate over all update functions + for( unsigned int i = 0; i < updateFunctionVector_.size( ); i++ ) + { + // Check if environment model is rotational state. + if( updateFunctionVector_.at( i ).get< 0 >( ) == body_rotational_state_update ) + { + // Check id body has no rotational ephemeris (i.e. if rotation comes from iterationNumber ). + if( bodyList_.at( updateFunctionVector_.at( i ).get< 1 >( ) )->getRotationalEphemeris( ) == NULL ) + { + // Check if DependentOrientationCalculator is an AerodynamicAngleCalculator. + boost::shared_ptr< reference_frames::DependentOrientationCalculator > dependentOrientationCalculator = + bodyList_.at( updateFunctionVector_.at( i ).get< 1 >( ) )->getDependentOrientationCalculator( ); + boost::shared_ptr< reference_frames::AerodynamicAngleCalculator > aerodynamicAngleCalculator = + boost::dynamic_pointer_cast< reference_frames::AerodynamicAngleCalculator >( + dependentOrientationCalculator ); + + // Check if properties of AerodynamicAngleCalculator are such that a different update order is warranted. + if( boost::dynamic_pointer_cast< reference_frames::AerodynamicAngleCalculator >( + dependentOrientationCalculator ) != NULL ) + { + unsigned int translationalUpdateIndex = 0; + unsigned int rotationalUpdateIndex = 0; + bool translationalUpdateIndexSet = false; + bool rotationalUpdateIndexSet = false; + + // Check if the state or orientation of the central body of AerodynamicAngleCalculator is updated. + for( unsigned int j = 0; j < updateFunctionVector_.size( ); j++ ) + { + if( ( updateFunctionVector_.at( j ).get< 0 >( ) == body_transational_state_update ) && + ( updateFunctionVector_.at( j ).get< 1 >( ) == + aerodynamicAngleCalculator->getCentralBodyName( ) ) ) + { + translationalUpdateIndex = j; + translationalUpdateIndexSet = true; + } + + if( ( updateFunctionVector_.at( j ).get< 0 >( ) == body_rotational_state_update ) && + ( updateFunctionVector_.at( j ).get< 1 >( ) == + aerodynamicAngleCalculator->getCentralBodyName( ) ) ) + { + rotationalUpdateIndex = j; + rotationalUpdateIndexSet = true; + } + } + + // If required updates are not found, throw error. + if( !translationalUpdateIndexSet || !rotationalUpdateIndexSet ) + { + throw std::runtime_error( + "Error when finding update order for AerodynamicAngleCalculator, did not find required updates for central body" ); + } + + // Check if rotational and translational state of central body are updated before + // AerodynamicAngleCalculator. + if( ( i < rotationalUpdateIndex ) || ( i < translationalUpdateIndex ) ) + { + if( i > rotationalUpdateIndex ) + { + std::iter_swap( updateFunctionVector_.begin( ) + i, + updateFunctionVector_.begin( ) + translationalUpdateIndex ); + rerunUpdateOrder = 1; + break; + } + else if( i > translationalUpdateIndex ) + { + std::iter_swap( updateFunctionVector_.begin( ) + i, + updateFunctionVector_.begin( ) + rotationalUpdateIndex ); + rerunUpdateOrder = 1; + break; + } + else if( translationalUpdateIndex < rotationalUpdateIndex ) + { + std::iter_swap( updateFunctionVector_.begin( ) + i, + updateFunctionVector_.begin( ) + translationalUpdateIndex ); + std::iter_swap( updateFunctionVector_.begin( ) + translationalUpdateIndex, + updateFunctionVector_.begin( ) + rotationalUpdateIndex ); + rerunUpdateOrder = 1; + break; + } + else + { + + std::iter_swap( updateFunctionVector_.begin( ) + i, + updateFunctionVector_.begin( ) + rotationalUpdateIndex ); + std::iter_swap( updateFunctionVector_.begin( ) + rotationalUpdateIndex, + updateFunctionVector_.begin( ) + translationalUpdateIndex ); + rerunUpdateOrder = 1; + break; + } + } + } + } + } + } + + // Define escape condition in case of infinite loop. + if( iterationNumber > 10000 ) + { + throw std::runtime_error( "Error when finding update order; stuck in infinite loop" ); + } + + // Rerun function if needed. + if( rerunUpdateOrder ) + { + setUpdateFunctionOrder( iterationNumber + 1 ); + } + } + //! Function to set the update functions for the environment from the required update settings. /*! * Function to set the update functions for the environment from the required update settings. @@ -241,6 +350,9 @@ class EnvironmentUpdater void setUpdateFunctions( const std::map< EnvironmentModelsToUpdate, std::vector< std::string > >& updateSettings ) { + std::map< EnvironmentModelsToUpdate, + std::vector< std::pair< std::string, boost::function< void( const double ) > > > > updateTimeFunctionList; + // Iterate over all required updates and set associated update function in lists for( std::map< EnvironmentModelsToUpdate, std::vector< std::string > >::const_iterator updateIterator = @@ -269,7 +381,7 @@ class EnvironmentUpdater { bool addUpdate = 1; - // Check if translational state is propagated + // Check if mass is propagated if( integratedStates_.count( transational_state ) > 0 ) { // Check if current body is propagated @@ -294,25 +406,33 @@ class EnvironmentUpdater ::setTemplatedStateFromEphemeris< StateScalarType, TimeType >, bodyList_.at( currentBodies.at( i ) ), _1 ); - currentStateFromEnvironmentList_[ body_transational_state_update ].insert( + updateTimeFunctionList[ body_transational_state_update ].push_back( std::make_pair( currentBodies.at( i ), stateSetFunction ) ); } break; } case body_rotational_state_update: { - boost::shared_ptr< ephemerides::RotationalEphemeris > rotationalEphemeris = - bodyList_.at( currentBodies.at( i ) )->getRotationalEphemeris( ); - // Check if rotational ephemeris exists - if( rotationalEphemeris != NULL ) + if( ( bodyList_.at( currentBodies.at( i ) )->getRotationalEphemeris( ) != NULL ) || + ( bodyList_.at( currentBodies.at( i ) )->getDependentOrientationCalculator( ) != NULL ) ) { boost::function< void( const TimeType ) > rotationalStateSetFunction = boost::bind( &simulation_setup::Body ::setCurrentRotationalStateToLocalFrameFromEphemeris, bodyList_.at( currentBodies.at( i ) ), _1 ); - currentStateFromEnvironmentList_[ body_rotational_state_update ].insert( + updateTimeFunctionList[ body_rotational_state_update ].push_back( std::make_pair( currentBodies.at( i ), rotationalStateSetFunction ) ); + + if( bodyList_.at( currentBodies.at( i ) )->getRotationalEphemeris( ) == NULL ) + { + resetFunctionVector_.push_back( + boost::make_tuple( + body_rotational_state_update, currentBodies.at( i ), + boost::bind( &reference_frames::DependentOrientationCalculator:: + resetCurrentTime, bodyList_.at( currentBodies.at( i ) )-> + getDependentOrientationCalculator( ), TUDAT_NAN ) ) ); + } } else { @@ -325,11 +445,32 @@ class EnvironmentUpdater } case body_mass_update: + { + bool addUpdate = 1; + + // Check if translational state is propagated + if( integratedStates_.count( body_mass_state ) > 0 ) + { + // Check if current body is propagated + std::pair< std::string, std::string > bodyToCheck + = std::make_pair( currentBodies.at( i ), "" ); + std::vector< std::pair< std::string, std::string > > integratedBodyMasses + = integratedStates_.at( body_mass_state ); + if( std::find( integratedBodyMasses.begin( ), + integratedBodyMasses.end( ), + bodyToCheck ) != integratedBodyMasses.end( ) ) + { + addUpdate = 0; + } + } + + if( addUpdate ) { - updateTimeFunctionList_[ body_mass_update ].push_back( - std::make_pair( currentBodies.at( i ), - boost::bind( &simulation_setup::Body::updateMass, - bodyList_.at( currentBodies.at( i ) ), _1 ) ) ); + updateTimeFunctionList[ body_mass_update ].push_back( + std::make_pair( currentBodies.at( i ), + boost::bind( &simulation_setup::Body::updateMass, + bodyList_.at( currentBodies.at( i ) ), _1 ) ) ); + } break; } case spherical_harmonic_gravity_field_update: @@ -338,16 +479,16 @@ class EnvironmentUpdater // Check if body has time-dependent sh field boost::shared_ptr< gravitation::TimeDependentSphericalHarmonicsGravityField > gravityField = boost::dynamic_pointer_cast - < gravitation::TimeDependentSphericalHarmonicsGravityField > - ( bodyList_.at( currentBodies.at( i ) )->getGravityFieldModel( ) ); + < gravitation::TimeDependentSphericalHarmonicsGravityField > + ( bodyList_.at( currentBodies.at( i ) )->getGravityFieldModel( ) ); if( gravityField != NULL ) { - updateTimeFunctionList_[ spherical_harmonic_gravity_field_update ].push_back( + updateTimeFunctionList[ spherical_harmonic_gravity_field_update ].push_back( std::make_pair( currentBodies.at( i ), boost::bind( &gravitation - ::TimeDependentSphericalHarmonicsGravityField - ::update, + ::TimeDependentSphericalHarmonicsGravityField + ::update, gravityField, _1 ) ) ); } // If no sh field at all, throw eeror. @@ -367,12 +508,19 @@ class EnvironmentUpdater { // If vehicle has flight conditions, add flight conditions update // function to update list. - updateTimeFunctionList_[ vehicle_flight_conditions_update ].push_back( + updateTimeFunctionList[ vehicle_flight_conditions_update ].push_back( std::make_pair( currentBodies.at( i ), boost::bind( &aerodynamics::FlightConditions::updateConditions, bodyList_.at( currentBodies.at( i ) ) ->getFlightConditions( ), _1 ) ) ); + + resetFunctionVector_.push_back( + boost::make_tuple( + vehicle_flight_conditions_update, currentBodies.at( i ), + boost::bind( &aerodynamics::FlightConditions:: + resetCurrentTime, bodyList_.at( currentBodies.at( i ) )-> + getFlightConditions( ), TUDAT_NAN ) ) ); } else { @@ -399,7 +547,7 @@ class EnvironmentUpdater } else if( radiationPressureInterfaces.size( ) > 1 ) { - std::cerr<<"Request radiation pressure update of "< > > >::iterator updateTimeIterator = updateTimeFunctionList.begin( ); + updateTimeIterator != updateTimeFunctionList.end( ); updateTimeIterator++ ) + { + for( unsigned int i = 0; i < updateTimeIterator->second.size( ); i++ ) + { + updateFunctionVector_.push_back( + boost::make_tuple( updateTimeIterator->first, updateTimeIterator->second.at( i ).first, + updateTimeIterator->second.at( i ).second ) ); + } + } + + // Set update order of functions. + setUpdateFunctionOrder( ); } //! List of body objects, this list encompasses all environment object in the simulation. @@ -440,40 +604,20 @@ class EnvironmentUpdater std::map< IntegratedStateType, std::vector< std::pair< std::string, std::string > > > integratedStates_; - //! List of function to call for updating the state (i.e. translational, rotational, etc.) from - //! the environment. - std::map< EnvironmentModelsToUpdate, - std::multimap< std::string, boost::function< void( const TimeType ) > > > - currentStateFromEnvironmentList_; - - //! Predefined iterator for computational efficiency. - typename std::map< EnvironmentModelsToUpdate, - std::multimap< std::string, boost::function< void( const TimeType ) > > >::iterator - outerCurrentStateFromEnvironmentIterator_; - - //! Predefined iterator for computational efficiency. - typename std::multimap< std::string, - boost::function< void( const TimeType ) > >::iterator - currentStateFromEnvironmentIterator_; - - //! List of time-independent functions to call to update the environment. - std::map< EnvironmentModelsToUpdate, - std::vector< std::pair< std::string, boost::function< void( ) > > > > updateFunctionList_; - - //! Predefined iterator for computational efficiency. - std::map< EnvironmentModelsToUpdate, - std::vector< std::pair< std::string, boost::function< void( ) > > > >::iterator - updateFunctionIterator; - - //! List of time-dependent functions to call to update the environment. - std::map< EnvironmentModelsToUpdate, - std::vector< std::pair< std::string, boost::function< void( const double ) > > > > - updateTimeFunctionList_; - - //! Predefined iterator for computational efficiency. - std::map< EnvironmentModelsToUpdate, - std::vector< std::pair< std::string, boost::function< void( const double ) > > > >::iterator - updateTimeIterator; + //! List of time-dependent functions to call to update the environment. + std::vector< boost::tuple< EnvironmentModelsToUpdate, std::string, boost::function< void( const double ) > > > + updateFunctionVector_; + + //! List of time-dependent functions to call to reset the time of the environment (to NaN signal recomputation for next + //! time step). + std::vector< boost::tuple< EnvironmentModelsToUpdate, std::string, boost::function< void( ) > > > resetFunctionVector_; + + + + + //! Predefined state history iterator for computational efficiency. + typename std::unordered_map< IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >::const_iterator + integratedStateIterator_; }; diff --git a/Tudat/SimulationSetup/PropagationSetup/propagationOutput.cpp b/Tudat/SimulationSetup/PropagationSetup/propagationOutput.cpp new file mode 100644 index 0000000000..b6fa7dbd00 --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/propagationOutput.cpp @@ -0,0 +1,173 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/Astrodynamics/Aerodynamics/aerodynamics.h" +#include "Tudat/SimulationSetup/PropagationSetup/propagationOutput.h" + +namespace tudat +{ + +namespace propagators +{ + +//! Get the vector representation of a quaternion. +Eigen::VectorXd getVectorRepresentationForRotation( + const boost::function< Eigen::Quaterniond( ) > rotationFunction ) +{ + Eigen::Matrix3d currentRotationMatrix = rotationFunction( ).toRotationMatrix( ); + + Eigen::VectorXd vectorRepresentation = Eigen::VectorXd( 9 ); + for( unsigned int i = 0; i < 3; i++ ) + { + for( unsigned int j = 0; j < 3; j++ ) + { + vectorRepresentation( i * 3 + j ) = currentRotationMatrix( i, j ); + } + } + return vectorRepresentation; +} + +//! Get the 3x3 matrix representation from a vector with 9 entries +Eigen::Matrix3d getMatrixFromVectorRotationRepresentation( + const Eigen::VectorXd vectorRepresentation ) +{ + if( vectorRepresentation.rows( ) != 9 ) + { + throw std::runtime_error( "Error when putting vector in matrix representation, size is incompatible" ); + } + Eigen::Matrix3d currentRotationMatrix; + for( unsigned int i = 0; i < 3; i++ ) + { + for( unsigned int j = 0; j < 3; j++ ) + { + currentRotationMatrix( i, j ) = vectorRepresentation( i * 3 + j ); + } + } + return currentRotationMatrix; +} + +//! Get the quaternion formulation of an orthonormal matrix, from input of a vector with 9 entries corresponding to matrix +//! entries. +Eigen::Quaterniond getQuaternionFromVectorRotationRepresentation( + const Eigen::VectorXd vectorRepresentation ) +{ + return Eigen::Quaterniond( getMatrixFromVectorRotationRepresentation( vectorRepresentation ) ); +} + +//! Function to evaluate a set of double and vector-returning functions and concatenate the results. +Eigen::VectorXd evaluateListOfFunctions( + const std::vector< boost::function< double( ) > >& doubleFunctionList, + const std::vector< std::pair< boost::function< Eigen::VectorXd( ) >, int > > vectorFunctionList, + const int totalSize) +{ + Eigen::VectorXd variableList = Eigen::VectorXd::Zero( totalSize ); + int currentIndex = 0; + + for( unsigned int i = 0; i < doubleFunctionList.size( ); i++ ) + { + variableList( i ) = doubleFunctionList.at( i )( ); + currentIndex++; + } + + for( unsigned int i = 0; i < vectorFunctionList.size( ); i++ ) + { + variableList.segment( currentIndex, vectorFunctionList.at( i ).second ) = + vectorFunctionList.at( i ).first( ); + currentIndex += vectorFunctionList.at( i ).second; + } + + // Check consistency with input + if( currentIndex != totalSize ) + { + std::string errorMessage = "Error when evaluating lists of functions, sizes are inconsistent: " + + boost::lexical_cast< std::string >( currentIndex ) + " and " + + boost::lexical_cast< std::string >( totalSize ); + throw std::runtime_error( errorMessage ); + } + + return variableList; +} + +//! Funtion to get the size of a dependent variable +int getDependentVariableSize( + const PropagationDependentVariables dependentVariableSettings ) +{ + int variableSize = -1; + switch( dependentVariableSettings ) + { + case mach_number_dependent_variable: + variableSize = 1; + break; + case altitude_dependent_variable: + variableSize = 1; + break; + case airspeed_dependent_variable: + variableSize = 1; + break; + case local_density_dependent_variable: + variableSize = 1; + break; + case relative_speed_dependent_variable: + variableSize = 1; + break; + case relative_position_dependent_variable: + variableSize = 3; + break; + case relative_distance_dependent_variable: + variableSize = 1; + break; + case relative_velocity_dependent_variable: + variableSize = 3; + break; + case radiation_pressure_dependent_variable: + variableSize = 1; + break; + case total_acceleration_norm_dependent_variable: + variableSize = 1; + break; + case single_acceleration_norm_dependent_variable: + variableSize = 1; + break; + case total_acceleration_dependent_variable: + variableSize = 3; + break; + case single_acceleration_dependent_variable: + variableSize = 3; + break; + case aerodynamic_force_coefficients_dependent_variable: + variableSize = 3; + break; + case aerodynamic_moment_coefficients_dependent_variable: + variableSize = 3; + break; + case rotation_matrix_to_body_fixed_frame_variable: + variableSize = 9; + break; + case intermediate_aerodynamic_rotation_matrix_variable: + variableSize = 9; + break; + case relative_body_aerodynamic_orientation_angle_variable: + variableSize = 1; + break; + case body_fixed_airspeed_based_velocity_variable: + variableSize = 3; + break; + default: + std::string errorMessage = "Error, did not recognize dependent variable size of type: " + + boost::lexical_cast< std::string >( dependentVariableSettings ); + throw std::runtime_error( errorMessage ); + } + return variableSize; +} + + +} // namespace propagators + +} // namespace tudat diff --git a/Tudat/SimulationSetup/PropagationSetup/propagationOutput.h b/Tudat/SimulationSetup/PropagationSetup/propagationOutput.h new file mode 100644 index 0000000000..7e468e106e --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/propagationOutput.h @@ -0,0 +1,597 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_PROPAGATIONOUTPUT_H +#define TUDAT_PROPAGATIONOUTPUT_H + +#include + +#include "Tudat/Astrodynamics/Aerodynamics/aerodynamics.h" +#include "Tudat/Astrodynamics/Propagators/dynamicsStateDerivativeModel.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" +#include "Tudat/SimulationSetup/PropagationSetup/propagationOutputSettings.h" + +namespace tudat +{ + +namespace propagators +{ + +//! Function to evaluate a function with two input variables (by reference) from function pointers +/*! + * Function to evaluate a function with two input variables (by reference) from function pointers that return these + * two input variables. + * \param functionToEvaluate Function that is to be evaluated with input from function pointers. + * \param firstInput Function returning first input to functionToEvaluate. + * \param secondInput Function returning second input to functionToEvaluate. + * \return Output from functionToEvaluate, using functions firstInput and secondInput as input. + */ +template< typename OutputType, typename InputType > +OutputType evaluateReferenceFunction( + const boost::function< OutputType( const InputType&, const InputType& ) > functionToEvaluate, + const boost::function< InputType( ) > firstInput, + const boost::function< InputType( ) > secondInput ) +{ + return functionToEvaluate( firstInput( ), secondInput( ) ); +} + +//! Function to evaluate a function with two input variables from function pointers +/*! + * Function to evaluate a function with two input variables from function pointers that return these + * two input variables. + * \param functionToEvaluate Function that is to be evaluated with input from function pointers. + * \param firstInput Function returning first input to functionToEvaluate. + * \param secondInput Function returning second input to functionToEvaluate. + * \return Output from functionToEvaluate, using functions firstInput and secondInput as input. + */ +template< typename OutputType, typename InputType > +OutputType evaluateFunction( + const boost::function< OutputType( const InputType, const InputType ) > functionToEvaluate, + const boost::function< InputType( ) > firstInput, + const boost::function< InputType( ) > secondInput ) +{ + return functionToEvaluate( firstInput( ), secondInput( ) ); +} + +//! Funtion to get the size of a dependent variable +/*! + * Funtion to get the size (i.e. number of values in variable: one for altitude, three for position, etc.) + * of a dependent variable + * \param dependentVariableSettings Dependent variable type for which size is to be determined. + * \return Size of requested dependent variable. + */ +int getDependentVariableSize( + const PropagationDependentVariables dependentVariableSettings ); + +//! Get the vector representation of a quaternion. +/*! + * Get the vector representation of a quaternion. Quaternion is converted to a rotation matrix, which is then put into + * a vector representation. + * \param rotationFunction Function returning the quaternion that is to be put inot vector rerpesentation + * \return Column vector consisting of transpose of concatenated rows of matrix representation of rotationFunction output. + */ +Eigen::VectorXd getVectorRepresentationForRotation( + const boost::function< Eigen::Quaterniond( ) > rotationFunction ); + +//! Get the 3x3 matrix representation from a vector with 9 entries +/*! + * Get the matrix representation from a vector with 9 entries. The vector is the transpose of the concatenated rows of + * the associated matrix. + * \param vectorRepresentation Vector representation of 3x3 matrix (transpose of the concatenated rows of + * the associated matrix) + * \return Matrix from 3x3 vector representation. + */ +Eigen::Matrix3d getMatrixFromVectorRotationRepresentation( + const Eigen::VectorXd vectorRepresentation ); + +//! Get the quaternion formulation of an orthonormal matrix, from input of a vector with 9 entries corresponding to matrix +//! entries. +/*! + * et the quaternion formulation of an orthonormal matrix, from input of a vector with 9 entries corresponding to matrix + * entries. The input vector is the transpose of the concatenated rows of the associated (orthonormal) matrix. + * \param vectorRepresentation Vector representation of 3x3 matrix (transpose of the concatenated rows of + * the associated matrix) + * \return Quaternion representation of orthonormal matrix obtained from 3x3 vector representation. + */ +Eigen::Quaterniond getQuaternionFromVectorRotationRepresentation( + const Eigen::VectorXd vectorRepresentation ); + +//! Function to create a function returning a requested dependent variable value (of type double). +/*! + * Function to create a function returning a requested dependent variable value (of type double), retrieved from + * environment and/or state derivative models. + * \param dependentVariableSettings Settings for dependent variable that is to be returned by function created here. + * \param bodyMap List of bodies to use in simulations (containing full environment). + * \param stateDerivativeModels List of state derivative models used in simulations (sorted by dynamics type as key) + * \return Function returning requested dependent variable. NOTE: The environment and state derivative models need to + * be updated to current state and independent variable before computation is performed. + */ +template< typename TimeType = double, typename StateScalarType = double > +boost::function< double( ) > getDoubleDependentVariableFunction( + const boost::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings, + const simulation_setup::NamedBodyMap& bodyMap, + const std::unordered_map< IntegratedStateType, + std::vector< boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > > > stateDerivativeModels = + std::unordered_map< IntegratedStateType, + std::vector< boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > > >( ) ) +{ + boost::function< double( ) > variableFunction; + + // Retrieve base information on dependent variable + PropagationDependentVariables dependentVariable = dependentVariableSettings->variableType_; + const std::string& bodyWithProperty = dependentVariableSettings->associatedBody_; + const std::string& secondaryBody = dependentVariableSettings->secondaryBody_; + + // Check dependent variable type and create function accordingly. + switch( dependentVariable ) + { + case mach_number_dependent_variable: + { + if( bodyMap.at( bodyWithProperty )->getFlightConditions( ) == NULL ) + { + std::string errorMessage = "Error, no flight conditions available when requesting Mach number output of " + + bodyWithProperty + "w.r.t." + secondaryBody; + throw std::runtime_error( errorMessage ); + } + + boost::function< double( const double, const double ) > functionToEvaluate = + boost::bind( &aerodynamics::computeMachNumber, _1, _2 ); + + // Retrieve functions for airspeed and speed of sound. + boost::function< double( ) > firstInput = + boost::bind( &aerodynamics::FlightConditions::getCurrentAirspeed, + bodyMap.at( bodyWithProperty )->getFlightConditions( ) ); + boost::function< double( ) > secondInput = + boost::bind( &aerodynamics::FlightConditions::getCurrentSpeedOfSound, + bodyMap.at( bodyWithProperty )->getFlightConditions( ) ); + + + variableFunction = boost::bind( &evaluateFunction< double, double >, + functionToEvaluate, firstInput, secondInput ); + break; + } + case altitude_dependent_variable: + if( bodyMap.at( bodyWithProperty )->getFlightConditions( ) == NULL ) + { + + } + variableFunction = boost::bind( &aerodynamics::FlightConditions::getCurrentAltitude, + bodyMap.at( bodyWithProperty )->getFlightConditions( ) ); + break; + case airspeed_dependent_variable: + if( bodyMap.at( bodyWithProperty )->getFlightConditions( ) == NULL ) + { + std::string errorMessage = "Error, no flight conditions available when requesting airspeed output of " + + bodyWithProperty + "w.r.t." + secondaryBody; + throw std::runtime_error( errorMessage ); + } + variableFunction = boost::bind( &aerodynamics::FlightConditions::getCurrentAirspeed, + bodyMap.at( bodyWithProperty )->getFlightConditions( ) ); + break; + case local_density_dependent_variable: + if( bodyMap.at( bodyWithProperty )->getFlightConditions( ) == NULL ) + { + std::string errorMessage = "Error, no flight conditions available when requesting density output of " + + bodyWithProperty + "w.r.t." + secondaryBody; + throw std::runtime_error( errorMessage ); + } + variableFunction = boost::bind( &aerodynamics::FlightConditions::getCurrentDensity, + bodyMap.at( bodyWithProperty )->getFlightConditions( ) ); + break; + case radiation_pressure_dependent_variable: + if( bodyMap.at( bodyWithProperty )->getRadiationPressureInterfaces( ).count( secondaryBody ) == 0 ) + { + std::string errorMessage = "Error, no radiation pressure interfaces when requesting radiation pressure output of " + + bodyWithProperty + "w.r.t." + secondaryBody; + throw std::runtime_error( errorMessage ); + } + variableFunction = boost::bind( &electro_magnetism::RadiationPressureInterface::getCurrentRadiationPressure, + bodyMap.at( bodyWithProperty )->getRadiationPressureInterfaces( ).at( secondaryBody ) ); + break; + case relative_distance_dependent_variable: + { + // Retrieve functions for positions of two bodies. + boost::function< double( const Eigen::Vector3d&, const Eigen::Vector3d& ) > functionToEvaluate = + boost::bind( &linear_algebra::computeNormOfVectorDifference, _1, _2 ); + boost::function< Eigen::Vector3d( ) > firstInput = + boost::bind( &simulation_setup::Body::getPosition, bodyMap.at( bodyWithProperty ) ); + boost::function< Eigen::Vector3d( ) > secondInput = + boost::bind( &simulation_setup::Body::getPosition, bodyMap.at( secondaryBody ) ); + + variableFunction = boost::bind( + &evaluateReferenceFunction< double, Eigen::Vector3d >, functionToEvaluate, firstInput, secondInput ); + break; + } + case relative_speed_dependent_variable: + { + // Retrieve functions for velicoty of two bodies. + boost::function< double( const Eigen::Vector3d&, const Eigen::Vector3d& ) > functionToEvaluate = + boost::bind( &linear_algebra::computeNormOfVectorDifference, _1, _2 ); + boost::function< Eigen::Vector3d( ) > firstInput = + boost::bind( &simulation_setup::Body::getVelocity, bodyMap.at( bodyWithProperty ) ); + boost::function< Eigen::Vector3d( ) > secondInput = + boost::bind( &simulation_setup::Body::getVelocity, bodyMap.at( secondaryBody ) ); + + variableFunction = boost::bind( + &evaluateReferenceFunction< double, Eigen::Vector3d >, functionToEvaluate, firstInput, secondInput ); + + break; + } + case single_acceleration_norm_dependent_variable: + { + // Check input consistency + boost::shared_ptr< SingleAccelerationDependentVariableSaveSettings > accelerationDependentVariableSettings = + boost::dynamic_pointer_cast< SingleAccelerationDependentVariableSaveSettings >( + dependentVariableSettings ); + if( accelerationDependentVariableSettings == NULL ) + { + std::string errorMessage = "Error, inconsistent inout when creating dependent variable function of type single_acceleration_norm_dependent_variable"; + throw std::runtime_error( errorMessage ); + } + else + { + // Retrieve list of suitable acceleration models (size should be one to avoid ambiguities) + std::vector< boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > > + listOfSuitableAccelerationModels = getAccelerationBetweenBodies( + accelerationDependentVariableSettings->associatedBody_, + accelerationDependentVariableSettings->secondaryBody_, + stateDerivativeModels, accelerationDependentVariableSettings->accelerationModeType_ ); + if( listOfSuitableAccelerationModels.size( ) != 1 ) + { + std::string errorMessage = "Error when getting acceleration between bodies " + + accelerationDependentVariableSettings->associatedBody_ + " and " + + accelerationDependentVariableSettings->secondaryBody_ + " of type " + + boost::lexical_cast< std::string >( + accelerationDependentVariableSettings->accelerationModeType_ ) + + ", no such acceleration found"; + throw std::runtime_error( errorMessage ); + } + else + { + boost::function< Eigen::Vector3d( ) > vectorFunction = + boost::bind( &basic_astrodynamics::AccelerationModel3d::getAcceleration, + listOfSuitableAccelerationModels.at( 0 ) ); + variableFunction = boost::bind( &linear_algebra::getVectorNormFromFunction, vectorFunction ); + } + } + break; + } + case total_acceleration_norm_dependent_variable: + { + // Retrieve model responsible for computing accelerations of requested bodies. + boost::shared_ptr< NBodyStateDerivative< StateScalarType, TimeType > > nBodyModel = + getTranslationalStateDerivativeModelForBody( bodyWithProperty, stateDerivativeModels ); + boost::function< Eigen::Vector3d( ) > vectorFunction = + boost::bind( &NBodyStateDerivative< StateScalarType, TimeType >::getTotalAccelerationForBody, + nBodyModel, bodyWithProperty ); + variableFunction = boost::bind( &linear_algebra::getVectorNormFromFunction, vectorFunction ); + + break; + } + case relative_body_aerodynamic_orientation_angle_variable: + { + if( bodyMap.at( bodyWithProperty )->getFlightConditions( ) == NULL ) + { + std::string errorMessage = "Error when flight conditions for relative_body_aerodynamic_orientation_angle_variable output " + + bodyWithProperty + " has no flight conditions"; + throw std::runtime_error( errorMessage ); + } + + // Check input consistency. + boost::shared_ptr< BodyAerodynamicAngleVariableSaveSettings > bodyAerodynamicAngleVariableSaveSettings = + boost::dynamic_pointer_cast< BodyAerodynamicAngleVariableSaveSettings >( dependentVariableSettings ); + if( bodyAerodynamicAngleVariableSaveSettings == NULL ) + { + std::string errorMessage= "Error, inconsistent inout when creating dependent variable function of type relative_body_aerodynamic_orientation_angle_variable"; + throw std::runtime_error( errorMessage ); + } + + variableFunction = boost::bind( &reference_frames::AerodynamicAngleCalculator::getAerodynamicAngle, + bodyMap.at( bodyWithProperty )->getFlightConditions( )->getAerodynamicAngleCalculator( ), + bodyAerodynamicAngleVariableSaveSettings->angle_ ); + break; + } + default: + std::string errorMessage = + "Error, did not recognize double dependent variable type when making variable function: " + + boost::lexical_cast< std::string >( dependentVariableSettings->variableType_ ); + throw std::runtime_error( errorMessage ); + } + return variableFunction; +} + +//! Function to create a function returning a requested dependent variable value (of type VectorXd). +/*! + * Function to create a function returning a requested dependent variable value (of type VectorXd), retrieved from + * environment and/or state derivative models. + * \param dependentVariableSettings Settings for dependent variable that is to be returned by function created here. + * \param bodyMap List of bodies to use in simulations (containing full environment). + * \param stateDerivativeModels List of state derivative models used in simulations (sorted by dynamics type as key) + * \return Function returning requested dependent variable. NOTE: The environment and state derivative models need to + * be updated to current state and independent variable before computation is performed. + */ +template< typename TimeType = double, typename StateScalarType = double > +std::pair< boost::function< Eigen::VectorXd( ) >, int > getVectorDependentVariableFunction( + const boost::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings, + const simulation_setup::NamedBodyMap& bodyMap, + const std::unordered_map< IntegratedStateType, + std::vector< boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > > > stateDerivativeModels = + std::unordered_map< IntegratedStateType, + std::vector< boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > > >( ) ) +{ + boost::function< Eigen::VectorXd( ) > variableFunction; + int parameterSize; + + // Retrieve base information on dependent variable + PropagationDependentVariables dependentVariable = dependentVariableSettings->variableType_; + const std::string& bodyWithProperty = dependentVariableSettings->associatedBody_; + const std::string& secondaryBody = dependentVariableSettings->secondaryBody_; + + // Check dependent variable type and create function accordingly. + switch( dependentVariable ) + { + case relative_position_dependent_variable: + { + // Retrieve functions for positions of two bodies. + boost::function< Eigen::Vector3d( const Eigen::Vector3d&, const Eigen::Vector3d& ) > functionToEvaluate = + boost::bind( &linear_algebra::computeVectorDifference, _1, _2 ); + boost::function< Eigen::Vector3d( ) > firstInput = + boost::bind( &simulation_setup::Body::getPosition, bodyMap.at( bodyWithProperty ) ); + boost::function< Eigen::Vector3d( ) > secondInput = + boost::bind( &simulation_setup::Body::getPosition, bodyMap.at( secondaryBody ) ); + + variableFunction = boost::bind( + &evaluateReferenceFunction< Eigen::Vector3d, Eigen::Vector3d >, + functionToEvaluate, firstInput, secondInput ); + parameterSize = 3; + break; + } + case relative_velocity_dependent_variable: + { + // Retrieve functions for velocities of two bodies. + boost::function< Eigen::Vector3d( const Eigen::Vector3d&, const Eigen::Vector3d& ) > functionToEvaluate = + boost::bind( &linear_algebra::computeVectorDifference, _1, _2 ); + boost::function< Eigen::Vector3d( ) > firstInput = + boost::bind( &simulation_setup::Body::getVelocity, bodyMap.at( bodyWithProperty ) ); + boost::function< Eigen::Vector3d( ) > secondInput = + boost::bind( &simulation_setup::Body::getVelocity, bodyMap.at( secondaryBody ) ); + + variableFunction = boost::bind( + &evaluateReferenceFunction< Eigen::Vector3d, Eigen::Vector3d >, + functionToEvaluate, firstInput, secondInput ); + parameterSize = 3; + + + break; + } + case total_acceleration_dependent_variable: + { + // Retrieve model responsible for computing accelerations of requested bodies. + boost::shared_ptr< NBodyStateDerivative< StateScalarType, TimeType > > nBodyModel = + getTranslationalStateDerivativeModelForBody( bodyWithProperty, stateDerivativeModels ); + variableFunction = + boost::bind( &NBodyStateDerivative< StateScalarType, TimeType >::getTotalAccelerationForBody, nBodyModel, + bodyWithProperty ); + parameterSize = 3; + + + break; + } + case single_acceleration_dependent_variable: + { + // Check input consistency. + boost::shared_ptr< SingleAccelerationDependentVariableSaveSettings > accelerationDependentVariableSettings = + boost::dynamic_pointer_cast< SingleAccelerationDependentVariableSaveSettings >( dependentVariableSettings ); + if( accelerationDependentVariableSettings == NULL ) + { + std::string errorMessage= "Error, inconsistent inout when creating dependent variable function of type single_acceleration_dependent_variable"; + throw std::runtime_error( errorMessage ); + } + else + { + // Retrieve list of suitable acceleration models (size should be one to avoid ambiguities) + std::vector< boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > > + listOfSuitableAccelerationModels = getAccelerationBetweenBodies( + accelerationDependentVariableSettings->associatedBody_, + accelerationDependentVariableSettings->secondaryBody_, + stateDerivativeModels, accelerationDependentVariableSettings->accelerationModeType_ ); + if( listOfSuitableAccelerationModels.size( ) != 1 ) + { + std::string errorMessage = "Error when getting acceleration between bodies " + + accelerationDependentVariableSettings->associatedBody_ + " and " + + accelerationDependentVariableSettings->secondaryBody_ + " of type " + + boost::lexical_cast< std::string >( + accelerationDependentVariableSettings->accelerationModeType_ ) + + ", no such acceleration found"; + throw std::runtime_error( errorMessage ); + } + else + { + //boost::function< Eigen::Vector3d( ) > vectorFunction = + variableFunction = boost::bind( &basic_astrodynamics::AccelerationModel3d::getAcceleration, + listOfSuitableAccelerationModels.at( 0 ) ); + parameterSize = 3; + } + } + break; + } + case aerodynamic_force_coefficients_dependent_variable: + { + if( bodyMap.at( bodyWithProperty )->getFlightConditions( ) == NULL ) + { + std::string errorMessage = "Error, no flight conditions available when requesting density output of aerodynamic force coefficients " + + bodyWithProperty + "w.r.t." + secondaryBody; + throw std::runtime_error( errorMessage ); + } + + variableFunction = boost::bind( + &aerodynamics::AerodynamicCoefficientInterface::getCurrentForceCoefficients, + bodyMap.at( bodyWithProperty )->getFlightConditions( )->getAerodynamicCoefficientInterface( ) ); + parameterSize = 3; + + break; + } + case aerodynamic_moment_coefficients_dependent_variable: + { + if( bodyMap.at( bodyWithProperty )->getFlightConditions( ) == NULL ) + { + + std::string errorMessage = "Error, no flight conditions available when requesting density output of aerodynamic moment coefficients " + + bodyWithProperty + "w.r.t." + secondaryBody; + throw std::runtime_error( errorMessage ); + } + + variableFunction = boost::bind( + &aerodynamics::AerodynamicCoefficientInterface::getCurrentMomentCoefficients, + bodyMap.at( bodyWithProperty )->getFlightConditions( )->getAerodynamicCoefficientInterface( ) ); + parameterSize = 3; + + break; + } + case rotation_matrix_to_body_fixed_frame_variable: + { + boost::function< Eigen::Quaterniond( ) > rotationFunction = + boost::bind( &simulation_setup::Body::getCurrentRotationToLocalFrame, bodyMap.at( bodyWithProperty ) ); + variableFunction = boost::bind( &getVectorRepresentationForRotation, rotationFunction ); + parameterSize = 9; + break; + } + case intermediate_aerodynamic_rotation_matrix_variable: + { + if( bodyMap.at( bodyWithProperty )->getFlightConditions( ) == NULL ) + { + std::string errorMessage= "Error, no flight conditions when creating dependent variable function of type intermediate_aerodynamic_rotation_matrix_variable"; + throw std::runtime_error( errorMessage ); + } + + // Check input consistency. + boost::shared_ptr< IntermediateAerodynamicRotationVariableSaveSettings > + intermediateAerodynamicRotationVariableSaveSettings = + boost::dynamic_pointer_cast< IntermediateAerodynamicRotationVariableSaveSettings >( + dependentVariableSettings ); + if( intermediateAerodynamicRotationVariableSaveSettings == NULL ) + { + std::string errorMessage= "Error, inconsistent inout when creating dependent variable function of type intermediate_aerodynamic_rotation_matrix_variable"; + throw std::runtime_error( errorMessage ); + } + + boost::function< Eigen::Quaterniond( ) > rotationFunction = + boost::bind( &reference_frames::AerodynamicAngleCalculator::getRotationQuaternionBetweenFrames, + bodyMap.at( bodyWithProperty )->getFlightConditions( )->getAerodynamicAngleCalculator( ), + intermediateAerodynamicRotationVariableSaveSettings->baseFrame_, + intermediateAerodynamicRotationVariableSaveSettings->targetFrame_ ); + + variableFunction = boost::bind( &getVectorRepresentationForRotation, rotationFunction ); + parameterSize = 9; + break; + } + case body_fixed_airspeed_based_velocity_variable: + { + if( bodyMap.at( bodyWithProperty )->getFlightConditions( ) == NULL ) + { + std::string errorMessage= "Error, no flight conditions when creating dependent variable function of type current_airpeed_based_velocity_variable_variable"; + throw std::runtime_error( errorMessage ); + } + + variableFunction = boost::bind( &aerodynamics::FlightConditions::getCurrentAirspeedBasedVelocity, + bodyMap.at( bodyWithProperty )->getFlightConditions( ) ); + parameterSize = 3; + break; + } + default: + std::string errorMessage = + "Error, did not recognize vector dependent variable type when making variable function: " + + boost::lexical_cast< std::string >( dependentVariableSettings->variableType_ ); + throw std::runtime_error( errorMessage ); + } + return std::make_pair( variableFunction, parameterSize ); +} + +//! Function to evaluate a set of double and vector-returning functions and concatenate the results. +/*! + * Function to evaluate a set of double and vector-returning functions and concatenate the results. Results of double + * function list are put in return vector first, followed by those in vector function list. + * \param doubleFunctionList List of functions returning double variables + * \param vectorFunctionList List of functions returning vector variables (pairs denote function and return vector size) + * \param totalSize Total size of concatenated vector (used as input for efficiency. + * \return Concatenated results from input functions. + */ +Eigen::VectorXd evaluateListOfFunctions( + const std::vector< boost::function< double( ) > >& doubleFunctionList, + const std::vector< std::pair< boost::function< Eigen::VectorXd( ) >, int > > vectorFunctionList, + const int totalSize ); + +//! Function to create a function that evaluates a list of dependent variables and concatenates the results. +/*! + * Function to create a function that evaluates a list of dependent variables and concatenates the results. + * Dependent variables functions are created inside this function from a list of settings on their required + * types/properties. + * \param saveSettings Object containing types and other properties of dependent variables. + * \param bodyMap List of bodies to use in simulations (containing full environment). + * \param stateDerivativeModels List of state derivative models used in simulations (sorted by dynamics type as key) + * \return Pair with function returning requested dependent variable values, and list variable names with start entries. + * NOTE: The environment and state derivative models need to + * be updated to current state and independent variable before computation is performed. + */ +template< typename TimeType = double, typename StateScalarType = double > +std::pair< boost::function< Eigen::VectorXd( ) >, std::map< int, std::string > > createDependentVariableListFunction( + const boost::shared_ptr< DependentVariableSaveSettings > saveSettings, + const simulation_setup::NamedBodyMap& bodyMap, + const std::unordered_map< IntegratedStateType, + std::vector< boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > > >& stateDerivativeModels = + std::unordered_map< IntegratedStateType, + std::vector< boost::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > > >( ) ) +{ + // Retrieve list of save settings + std::vector< boost::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables = + saveSettings->dependentVariables_; + + // create list of double and vector parameters + std::vector< boost::function< double( ) > > doubleFunctionList; + std::vector< std::pair< boost::function< Eigen::VectorXd( ) >, int > > vectorFunctionList; + int totalVariableSize = 0; + + std::map< int, std::string > dependentVariableId; + for( unsigned int i = 0; i < dependentVariables.size( ); i++ ) + { + // Create double parameter + if( getDependentVariableSize( dependentVariables.at( i )->variableType_ ) == 1 ) + { + doubleFunctionList.push_back( getDoubleDependentVariableFunction( + dependentVariables.at( i ), + bodyMap, stateDerivativeModels ) ); + dependentVariableId[ totalVariableSize ] = getDependentVariableId( + dependentVariables.at( i ) ); + totalVariableSize++; + } + // Create vector parameter + else + { + vectorFunctionList.push_back( getVectorDependentVariableFunction( + dependentVariables.at( i ), + bodyMap, stateDerivativeModels ) ); + dependentVariableId[ totalVariableSize ] = getDependentVariableId( + dependentVariables.at( i ) ); + totalVariableSize += vectorFunctionList.at( vectorFunctionList.size( ) - 1 ).second; + } + } + + // Create function conatenating function results. + return std::make_pair( + boost::bind( &evaluateListOfFunctions, doubleFunctionList, vectorFunctionList, totalVariableSize ), + dependentVariableId ); +} + + +} // namespace propagators + +} // namespace tudat + +#endif // TUDAT_PROPAGATIONOUTPUT_H diff --git a/Tudat/SimulationSetup/PropagationSetup/propagationOutputSettings.cpp b/Tudat/SimulationSetup/PropagationSetup/propagationOutputSettings.cpp new file mode 100644 index 0000000000..a2a38d0b38 --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/propagationOutputSettings.cpp @@ -0,0 +1,168 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/SimulationSetup/PropagationSetup/propagationOutputSettings.h" + +namespace tudat +{ + +namespace propagators +{ + +//! Function to get a string representing a 'named identification' of a dependent variable type +std::string getDependentVariableName( const PropagationDependentVariables propagationDependentVariables ) +{ + std::string variableName = ""; + switch( propagationDependentVariables ) + { + case mach_number_dependent_variable: + variableName = "Mach number "; + break; + case altitude_dependent_variable: + variableName = "Altitude "; + break; + case airspeed_dependent_variable: + variableName = "Airspeed "; + break; + case local_density_dependent_variable: + variableName = "Density "; + break; + case relative_speed_dependent_variable: + variableName = "Relative speed "; + break; + case relative_position_dependent_variable: + variableName = "Relative position "; + break; + case relative_distance_dependent_variable: + variableName = "Relative distance "; + break; + case relative_velocity_dependent_variable: + variableName = "Relative velocity "; + break; + case radiation_pressure_dependent_variable: + variableName = "Radiation pressure "; + break; + case total_acceleration_norm_dependent_variable: + variableName = "Total acceleration norm "; + break; + case single_acceleration_norm_dependent_variable: + variableName = "Single acceleration norm of type "; + break; + case total_acceleration_dependent_variable: + variableName = "Total acceleration "; + break; + case single_acceleration_dependent_variable: + variableName = "Single acceleration of type "; + break; + case aerodynamic_force_coefficients_dependent_variable: + variableName = "Aerodynamic force coefficients "; + break; + case aerodynamic_moment_coefficients_dependent_variable: + variableName = "Aerodynamic moment coefficients "; + break; + case rotation_matrix_to_body_fixed_frame_variable: + variableName = "Rotation matrix to body-fixed frame "; + break; + case intermediate_aerodynamic_rotation_matrix_variable: + variableName = "Rotation matrix from "; + break; + case relative_body_aerodynamic_orientation_angle_variable: + variableName = "Body orientation angle "; + break; + case body_fixed_airspeed_based_velocity_variable: + variableName = "Airspeed-based velocity "; + break; + default: + std::string errorMessage = "Error, dependent variable " + + boost::lexical_cast< std::string >( propagationDependentVariables ) + + "not found when retrieving parameter name "; + throw std::runtime_error( errorMessage ); + } + return variableName; +} + +//! Function to get a string representing a 'named identification' of a dependent variable +std::string getDependentVariableId( + const boost::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings ) +{ + std::string variableId = getDependentVariableName( dependentVariableSettings->variableType_ ); + + if( ( dependentVariableSettings->variableType_ == single_acceleration_dependent_variable ) || + ( dependentVariableSettings->variableType_ == single_acceleration_norm_dependent_variable ) ) + { + boost::shared_ptr< SingleAccelerationDependentVariableSaveSettings > accelerationDependentVariableSettings = + boost::dynamic_pointer_cast< SingleAccelerationDependentVariableSaveSettings >( dependentVariableSettings ); + if( accelerationDependentVariableSettings == NULL ) + { + throw std::runtime_error( "Error when getting dependent variable ID, input is inconsistent (acceleration type )" ); + } + else + { + variableId += basic_astrodynamics::getAccelerationModelName( + accelerationDependentVariableSettings->accelerationModeType_ ); + } + } + else if( dependentVariableSettings->variableType_ == intermediate_aerodynamic_rotation_matrix_variable ) + { + boost::shared_ptr< IntermediateAerodynamicRotationVariableSaveSettings > rotationDependentVariableSettings = + boost::dynamic_pointer_cast< IntermediateAerodynamicRotationVariableSaveSettings >( dependentVariableSettings ); + if( rotationDependentVariableSettings == NULL ) + { + throw std::runtime_error( "Error when getting dependent variable ID, input is inconsistent (rotation matrix)" ); + } + else + { + variableId += + reference_frames::getAerodynamicFrameName( rotationDependentVariableSettings->baseFrame_ ) + "to " + + reference_frames::getAerodynamicFrameName( rotationDependentVariableSettings->targetFrame_ ); + } + } + + else if( dependentVariableSettings->variableType_ == relative_body_aerodynamic_orientation_angle_variable ) + { + boost::shared_ptr< BodyAerodynamicAngleVariableSaveSettings > angleDependentVariableSettings = + boost::dynamic_pointer_cast< BodyAerodynamicAngleVariableSaveSettings >( dependentVariableSettings ); + if( angleDependentVariableSettings == NULL ) + { + throw std::runtime_error( "Error when getting dependent variable ID, input is inconsistent (angle)" ); + } + else + { + variableId += + reference_frames::getAerodynamicAngleName( angleDependentVariableSettings->angle_ ); + } + } + + + + if( ( dependentVariableSettings->variableType_ == single_acceleration_dependent_variable ) || + ( dependentVariableSettings->variableType_ == single_acceleration_norm_dependent_variable ) ) + { + variableId += ", acting on " + dependentVariableSettings->associatedBody_; + if( dependentVariableSettings->secondaryBody_ != dependentVariableSettings->associatedBody_ ) + { + variableId += ", exerted by " + dependentVariableSettings->secondaryBody_; + } + } + else + { + variableId += "of " + dependentVariableSettings->associatedBody_; + if( dependentVariableSettings->secondaryBody_ != "" ) + { + variableId += " w.r.t. " + dependentVariableSettings->secondaryBody_; + } + + } + return variableId; +} + +} + +} diff --git a/Tudat/SimulationSetup/PropagationSetup/propagationOutputSettings.h b/Tudat/SimulationSetup/PropagationSetup/propagationOutputSettings.h new file mode 100644 index 0000000000..ba6a9cbcd0 --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/propagationOutputSettings.h @@ -0,0 +1,214 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_PROPAGATIONOUTPUTSETTINGS_H +#define TUDAT_PROPAGATIONOUTPUTSETTINGS_H + +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h" +#include "Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.h" + +namespace tudat +{ + +namespace propagators +{ + + +//! Enum listing the dependent variables that can be saved during the propagation +enum PropagationDependentVariables +{ + mach_number_dependent_variable = 0, + altitude_dependent_variable = 1, + airspeed_dependent_variable = 2, + local_density_dependent_variable = 3, + relative_speed_dependent_variable = 4, + relative_position_dependent_variable = 5, + relative_distance_dependent_variable = 6, + relative_velocity_dependent_variable = 7, + radiation_pressure_dependent_variable = 8, + total_acceleration_norm_dependent_variable = 9, + single_acceleration_norm_dependent_variable = 10, + total_acceleration_dependent_variable = 11, + single_acceleration_dependent_variable = 12, + aerodynamic_force_coefficients_dependent_variable = 13, + aerodynamic_moment_coefficients_dependent_variable = 14, + rotation_matrix_to_body_fixed_frame_variable = 15, + intermediate_aerodynamic_rotation_matrix_variable = 16, + relative_body_aerodynamic_orientation_angle_variable = 17, + body_fixed_airspeed_based_velocity_variable = 18 + +}; + +//! Functional base class for defining settings for dependent variables that are to be saved during propagation +/*! + * Functional base class for defining settings for dependent variables that are to be saved during propagation. + * Any dependent variable that requires additional information in addition to what can be provided here, should be + * defined by a dedicated derived class. + */ +class SingleDependentVariableSaveSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param variableType Type of dependent variable that is to be saved. + * \param associatedBody Body associated with dependent variable + * \param secondaryBody Secondary body (not necessarilly required) w.r.t. which parameter is defined (e.g. relative + * position, velocity etc. is defined of associatedBody w.r.t. secondaryBody). + */ + SingleDependentVariableSaveSettings( + const PropagationDependentVariables variableType, + const std::string& associatedBody, + const std::string& secondaryBody = "" ): + variableType_( variableType ), associatedBody_( associatedBody ), secondaryBody_( secondaryBody ){ } + + //! Destructor. + virtual ~SingleDependentVariableSaveSettings( ){ } + + //! Type of dependent variable that is to be saved. + PropagationDependentVariables variableType_; + + //! Body associated with dependent variable + std::string associatedBody_; + + //! Secondary body (not necessarilly required) w.r.t. which parameter is defined (e.g. relative position, + //! velocity etc. is defined of associatedBody w.r.t. secondaryBody). + std::string secondaryBody_; + +}; + +//! Class to define settings for saving a single acceleration (norm or vector) during propagation +class SingleAccelerationDependentVariableSaveSettings: public SingleDependentVariableSaveSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param accelerationModeType Type of acceleration that is to be saved. + * \param bodyUndergoingAcceleration Name of body undergoing the acceleration. + * \param bodyExertingAcceleration Name of body exerting the acceleration. + * \param useNorm Boolean denoting whether to use the norm (if true) or the vector (if false) of the acceleration. + */ + SingleAccelerationDependentVariableSaveSettings( + const basic_astrodynamics::AvailableAcceleration accelerationModeType, + const std::string& bodyUndergoingAcceleration, + const std::string& bodyExertingAcceleration, + const bool useNorm = 0 ): + SingleDependentVariableSaveSettings( + ( useNorm == 1 ) ? ( single_acceleration_norm_dependent_variable ) : ( single_acceleration_dependent_variable ), + bodyUndergoingAcceleration, bodyExertingAcceleration ), + accelerationModeType_( accelerationModeType ) + { } + + //! Boolean denoting whether to use the norm (if true) or the vector (if false) of the acceleration. + basic_astrodynamics::AvailableAcceleration accelerationModeType_; + +}; + +//! Class to define settings for saving a rotation matrix between two AerodynamicsReferenceFrames +class IntermediateAerodynamicRotationVariableSaveSettings: public SingleDependentVariableSaveSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param associatedBody Body for which the rotation matrix is to be saved. + * \param baseFrame Frame from which rotation is to take place. + * \param targetFrame Frame to which the rotation is to take place. + */ + IntermediateAerodynamicRotationVariableSaveSettings( + const std::string& associatedBody, + const reference_frames::AerodynamicsReferenceFrames baseFrame, + const reference_frames::AerodynamicsReferenceFrames targetFrame ): + SingleDependentVariableSaveSettings( intermediate_aerodynamic_rotation_matrix_variable, associatedBody ), + baseFrame_( baseFrame ), targetFrame_( targetFrame ){ } + + //! Frame from which rotation is to take place. + reference_frames::AerodynamicsReferenceFrames baseFrame_; + + //! Frame to which the rotation is to take place. + reference_frames::AerodynamicsReferenceFrames targetFrame_; + +}; + +//! Class to define settings for saving an aerodynamics orientation angle from AerodynamicsReferenceFrameAngles list. +class BodyAerodynamicAngleVariableSaveSettings: public SingleDependentVariableSaveSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param associatedBody Body for which the orientation angle is to be saved. + * \param angle Orientation angle that is to be saved. + */ + BodyAerodynamicAngleVariableSaveSettings( + const std::string& associatedBody, + const reference_frames::AerodynamicsReferenceFrameAngles angle ): + SingleDependentVariableSaveSettings( relative_body_aerodynamic_orientation_angle_variable, associatedBody ), + angle_( angle ){ } + + //! Orientation angle that is to be saved. + reference_frames::AerodynamicsReferenceFrameAngles angle_; +}; + +//! Container class for settings of all dependent variables that are to be saved. +class DependentVariableSaveSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param dependentVariables List of settings for parameters that are to be saved. + * \param printDependentVariableTypes Variable denoting whether to print the list and vector entries of + * dependent variables when propagating. + */ + DependentVariableSaveSettings( + const std::vector< boost::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables, + const bool printDependentVariableTypes = 1 ): + dependentVariables_( dependentVariables ), printDependentVariableTypes_( printDependentVariableTypes ){ } + + //! List of settings for parameters that are to be saved. + std::vector< boost::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables_; + + //! Variable denoting whether to print the list and vector entries of dependent variables when propagating. + bool printDependentVariableTypes_; +}; + +//! Function to get a string representing a 'named identification' of a dependent variable type +/*! + * Function to get a string representing a 'named identification' of a dependent variable type + * \param propagationDependentVariables Dependent variable type + * \return String with variable type id. + */ +std::string getDependentVariableName( const PropagationDependentVariables propagationDependentVariables ); + +//! Function to get a string representing a 'named identification' of a dependent variable +/*! + * Function to get a string representing a 'named identification' of a dependent variable + * \param dependentVariableSettings Dependent variable + * \return String with variable id. + */ +std::string getDependentVariableId( + const boost::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings ); + + +} // namespace propagators + +} // namespace tudat + + +#endif // TUDAT_PROPAGATIONOUTPUTSETTINGS_H diff --git a/Tudat/SimulationSetup/PropagationSetup/propagationSettings.cpp b/Tudat/SimulationSetup/PropagationSetup/propagationSettings.cpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tudat/SimulationSetup/PropagationSetup/propagationSettings.h b/Tudat/SimulationSetup/PropagationSetup/propagationSettings.h new file mode 100644 index 0000000000..f68f776642 --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/propagationSettings.h @@ -0,0 +1,658 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_PROPAGATIONSETTINGS_H +#define TUDAT_PROPAGATIONSETTINGS_H + +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/timeConversions.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/massRateModel.h" +#include "Tudat/Astrodynamics/Propagators/singleStateTypeDerivative.h" +#include "Tudat/Astrodynamics/Propagators/nBodyStateDerivative.h" +#include "Tudat/SimulationSetup/PropagationSetup/propagationOutputSettings.h" +#include "Tudat/SimulationSetup/PropagationSetup/propagationTerminationSettings.h" + +namespace tudat +{ + +namespace propagators +{ + +//! Base class for defining setting of a propagator +/*! + * Base class for defining setting of a propagator. This class is non-functional, and each state type requires its + * own derived class (which may have multiple derived classes of its own). + */ +template< typename StateScalarType > +class PropagatorSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param stateType Type of state being propagated + * \param initialBodyStates Initial state used as input for numerical integration + * \param terminationSettings Settings for creating the object that checks whether the propagation is finished. + * \param dependentVariablesToSave Settings for the dependent variables that are to be saved during propagation + * (default none). + * \param printInterval Variable indicating how often (once per printInterval_ seconds or propagation independenty + * variable) the current state and time are to be printed to console (default never). + */ + PropagatorSettings( const IntegratedStateType stateType, + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > initialBodyStates, + const boost::shared_ptr< PropagationTerminationSettings > terminationSettings, + const boost::shared_ptr< DependentVariableSaveSettings > dependentVariablesToSave = + boost::shared_ptr< DependentVariableSaveSettings >( ), + const double printInterval = TUDAT_NAN ): + stateType_( stateType ), initialStates_( initialBodyStates ), stateSize_( initialBodyStates.rows( ) ), + terminationSettings_( terminationSettings ), dependentVariablesToSave_( dependentVariablesToSave ), + printInterval_( printInterval){ } + + //! Virtual destructor. + virtual ~PropagatorSettings( ){ } + + //!T ype of state being propagated + IntegratedStateType stateType_; + + //! Function to retrieve the initial state used as input for numerical integration + /*! + * Function to retrieve the initial state used as input for numerical integration + * \return Initial state used as input for numerical integration + */ + Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > getInitialStates( ) + { + return initialStates_; + } + + //! Function to reset the initial state used as input for numerical integration + /*! + * Function to reset the initial state used as input for numerical integration + * \param initialBodyStates New initial state used as input for numerical integration + */ + virtual void resetInitialStates( const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& initialBodyStates ) + { + initialStates_ = initialBodyStates; + stateSize_ = initialStates_.rows( ); + } + + //! Get total size of the propagated state. + /*! + * Get total size of the propagated state. + * \return Total size of the propagated state. + */ + int getStateSize( ) + { + return stateSize_; + } + + //! Function to retrieve settings for creating the object that checks whether the propagation is finished. + /*! + * Function to retrieve settings for creating the object that checks whether the propagation is finished. + * \return Settings for creating the object that checks whether the propagation is finished. + */ + boost::shared_ptr< PropagationTerminationSettings > getTerminationSettings( ) + { + return terminationSettings_; + } + + //! Function to retrieve settings for the dependent variables that are to be saved during propagation (default none). + /*! + * Function to retrieve settings for the dependent variables that are to be saved during propagation (default none). + * \return Settings for the dependent variables that are to be saved during propagation (default none). + */ + boost::shared_ptr< DependentVariableSaveSettings > getDependentVariablesToSave( ) + { + return dependentVariablesToSave_; + } + + //! Function to retrieve how often the current state and time are to be printed to console + /*! + * Function to retrieve how often the current state and time are to be printed to console + * \return Time intercal with which the current state and time are to be printed to console (default NaN, meaning never). + */ + double getPrintInterval( ) + { + return printInterval_; + } + + +protected: + + //! Initial state used as input for numerical integration + Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > initialStates_; + + //! Total size of the propagated state. + int stateSize_; + + //! Settings for creating the object that checks whether the propagation is finished. + boost::shared_ptr< PropagationTerminationSettings > terminationSettings_; + + //! Settings for the dependent variables that are to be saved during propagation (default none). + boost::shared_ptr< DependentVariableSaveSettings > dependentVariablesToSave_; + + //! Variable indicating how often (once per printInterval_ seconds or propagation independenty variable) the + //! current state and time are to be printed to console (default never). + double printInterval_; + +}; + +//! Class for defining settings for propagating translational dynamics. +/*! + * Class for defining settings for propagating translational dynamics. The propagator defines the form of the equations of + * motion (i.e. Cowell, Encke, Gauss etc.). This base class can be used for Cowell propagator. + * Other propagators have dedicated derived class. + */ +template< typename StateScalarType = double > +class TranslationalStatePropagatorSettings: public PropagatorSettings< StateScalarType > +{ +public: + + //! Constructor for generic stopping conditions. + /*! + * Constructor for generic stopping conditions. + * \param centralBodies List of bodies w.r.t. which the bodies in bodiesToIntegrate_ are propagated. + * \param accelerationsMap A map containing the list of accelerations acting on each body, identifying + * the body being acted on and the body acted on by an acceleration. The map has as key a string denoting + * the name of the body the list of accelerations, provided as the value corresponding to a key, is acting on. + * This map-value is again a map with string as key, denoting the body exerting the acceleration, and as value + * a pointer to an acceleration model. + * \param bodiesToIntegrate List of bodies for which the translational state is to be propagated. + * \param initialBodyStates Initial state used as input for numerical integration + * \param terminationSettings Settings for creating the object that checks whether the propagation is finished. + * \param propagator Type of translational state propagator to be used + * \param dependentVariablesToSave Settings for the dependent variables that are to be saved during propagation + * (default none). + * \param printInterval Variable indicating how often (once per printInterval_ seconds or propagation independenty + * variable) the current state and time are to be printed to console (default never). + */ + TranslationalStatePropagatorSettings( const std::vector< std::string >& centralBodies, + const basic_astrodynamics::AccelerationMap& accelerationsMap, + const std::vector< std::string >& bodiesToIntegrate, + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& initialBodyStates, + const boost::shared_ptr< PropagationTerminationSettings > terminationSettings, + const TranslationalPropagatorType propagator = cowell, + const boost::shared_ptr< DependentVariableSaveSettings > dependentVariablesToSave = + boost::shared_ptr< DependentVariableSaveSettings >( ), + const double printInterval = TUDAT_NAN ): + PropagatorSettings< StateScalarType >( transational_state, initialBodyStates, terminationSettings, + dependentVariablesToSave, printInterval ), + centralBodies_( centralBodies ), + accelerationsMap_( accelerationsMap ), bodiesToIntegrate_( bodiesToIntegrate ), + propagator_( propagator ){ } + + //! Constructor for fixed propagation time stopping conditions. + /*! + * Constructor for fixed propagation time stopping conditions. + * \param centralBodies List of bodies w.r.t. which the bodies in bodiesToIntegrate_ are propagated. + * \param accelerationsMap A map containing the list of accelerations acting on each body, identifying + * the body being acted on and the body acted on by an acceleration. The map has as key a string denoting + * the name of the body the list of accelerations, provided as the value corresponding to a key, is acting on. + * This map-value is again a map with string as key, denoting the body exerting the acceleration, and as value + * a pointer to an acceleration model. + * \param bodiesToIntegrate List of bodies for which the translational state is to be propagated. + * \param initialBodyStates Initial state used as input for numerical integration + * \param endTime Time at which to stop the numerical propagation + * \param propagator Type of translational state propagator to be used + * \param dependentVariablesToSave Settings for the dependent variables that are to be saved during propagation + * (default none). + * \param printInterval Variable indicating how often (once per printInterval_ seconds or propagation independenty + * variable) the current state and time are to be printed to console (default never). + */ + TranslationalStatePropagatorSettings( const std::vector< std::string >& centralBodies, + const basic_astrodynamics::AccelerationMap& accelerationsMap, + const std::vector< std::string >& bodiesToIntegrate, + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& initialBodyStates, + const double endTime, + const TranslationalPropagatorType propagator = cowell, + const boost::shared_ptr< DependentVariableSaveSettings > dependentVariablesToSave = + boost::shared_ptr< DependentVariableSaveSettings >( ), + const double printInterval = TUDAT_NAN ): + PropagatorSettings< StateScalarType >( + transational_state, initialBodyStates, boost::make_shared< PropagationTimeTerminationSettings >( endTime ), + dependentVariablesToSave, printInterval ), + centralBodies_( centralBodies ), + accelerationsMap_( accelerationsMap ), bodiesToIntegrate_( bodiesToIntegrate ), + propagator_( propagator ){ } + + //! Destructor + ~TranslationalStatePropagatorSettings( ){ } + + //! List of bodies w.r.t. which the bodies in bodiesToIntegrate_ are propagated. + std::vector< std::string > centralBodies_; + + //! A map containing the list of accelerations acting on each body + /*! + * A map containing the list of accelerations acting on each body, identifying + * the body being acted on and the body acted on by an acceleration. The map has as key a string denoting + * the name of the body the list of accelerations, provided as the value corresponding to a key, is acting on. + * This map-value is again a map with string as key, denoting the body exerting the acceleration, and as value + * a pointer to an acceleration model. + */ + basic_astrodynamics::AccelerationMap accelerationsMap_; + + //! List of bodies for which the translational state is to be propagated. + std::vector< std::string > bodiesToIntegrate_; + + //! Type of translational state propagator to be used + TranslationalPropagatorType propagator_; + +}; + + +//! Class for defining settings for propagating the mass of a body +/*! + * Class for defining settings for propagating the mass of a body. The body masses are propagated in their natural + * form (i.e. no choice of equations of motion as is the case for translational dynamics)l + */ +template< typename StateScalarType > +class MassPropagatorSettings: public PropagatorSettings< StateScalarType > +{ +public: + + //! Constructor of mass state propagator settings, with single mass rate model per body. + /*! + * Constructor of mass state propagator settings, with single mass rate model per body. + * \param bodiesWithMassToPropagate List of bodies for which the mass is to be propagated. + * \param massRateModels List of mass rate models per propagated body. + * \param initialBodyMasses Initial masses used as input for numerical integration. + * \param terminationSettings Settings for creating the object that checks whether the propagation is finished. + * \param dependentVariablesToSave Settings for the dependent variables that are to be saved during propagation + * (default none). + * \param printInterval Variable indicating how often (once per printInterval_ seconds or propagation independenty + * variable) the current state and time are to be printed to console (default never). + */ + MassPropagatorSettings( + const std::vector< std::string > bodiesWithMassToPropagate, + const std::map< std::string, boost::shared_ptr< basic_astrodynamics::MassRateModel > > massRateModels, + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& initialBodyMasses, + const boost::shared_ptr< PropagationTerminationSettings > terminationSettings, + const boost::shared_ptr< DependentVariableSaveSettings > dependentVariablesToSave = + boost::shared_ptr< DependentVariableSaveSettings >( ), + const double printInterval = TUDAT_NAN ): + PropagatorSettings< StateScalarType >( body_mass_state, initialBodyMasses, terminationSettings, + dependentVariablesToSave, printInterval ), + bodiesWithMassToPropagate_( bodiesWithMassToPropagate ) + { + for( std::map< std::string, boost::shared_ptr< basic_astrodynamics::MassRateModel > >::const_iterator + massRateIterator = massRateModels.begin( ); massRateIterator != massRateModels.end( ); massRateIterator++ ) + { + massRateModels_[ massRateIterator->first ].push_back( massRateIterator->second ); + } + } + + //! Constructor of mass state propagator settings + /*! + * Constructor of mass state propagator settings + * \param bodiesWithMassToPropagate List of bodies for which the mass is to be propagated. + * \param massRateModels List of mass rate models per propagated body. + * \param initialBodyMasses Initial masses used as input for numerical integration. + * \param terminationSettings Settings for creating the object that checks whether the propagation is finished. + * \param dependentVariablesToSave Settings for the dependent variables that are to be saved during propagation + * (default none). + * \param printInterval Variable indicating how often (once per printInterval_ seconds or propagation independenty + * variable) the current state and time are to be printed to console (default never). + */ + MassPropagatorSettings( + const std::vector< std::string > bodiesWithMassToPropagate, + const std::map< std::string, std::vector< boost::shared_ptr< basic_astrodynamics::MassRateModel > > > + massRateModels, + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& initialBodyMasses, + const boost::shared_ptr< PropagationTerminationSettings > terminationSettings, + const boost::shared_ptr< DependentVariableSaveSettings > dependentVariablesToSave = + boost::shared_ptr< DependentVariableSaveSettings >( ), + const double printInterval = TUDAT_NAN ): + PropagatorSettings< StateScalarType >( body_mass_state, initialBodyMasses, terminationSettings, + dependentVariablesToSave, printInterval ), + bodiesWithMassToPropagate_( bodiesWithMassToPropagate ), massRateModels_( massRateModels ) + { } + + //! List of bodies for which the mass is to be propagated. + std::vector< std::string > bodiesWithMassToPropagate_; + + //! List of mass rate models per propagated body. + std::map< std::string, std::vector< boost::shared_ptr< basic_astrodynamics::MassRateModel > > > massRateModels_; +}; + +//! Function to retrieve the state size for a list of propagator settings. +/*! + * Function to retrieve the initial state for a list of propagator settings. + * \param propagatorSettingsList List of propagator settings (sorted by type as key). Map value provides list + * of propagator settings for given type. + * \return Vector of initial states, sorted in order of IntegratedStateType, and then in the order of the + * vector of PropagatorSettings of given type. + */ +template< typename StateScalarType > +int getMultiTypePropagatorStateSize( + const std::map< IntegratedStateType, std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > >& + propagatorSettingsList ) +{ + int stateSize = 0; + + // Iterate over all propagation settings and add size to list + for( typename std::map< IntegratedStateType, + std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > >::const_iterator + typeIterator = propagatorSettingsList.begin( ); typeIterator != propagatorSettingsList.end( ); typeIterator++ ) + { + for( unsigned int i = 0; i < typeIterator->second.size( ); i++ ) + { + stateSize += typeIterator->second.at( i )->getStateSize( ); + } + } + return stateSize; +} + +//! Function to retrieve the initial state for a list of propagator settings. +/*! + * Function to retrieve the initial state for a list of propagator settings. + * \param propagatorSettingsList List of propagator settings (sorted by type as key). Map value provides list + * of propagator settings for given type. + * \return Vector of initial states, sorted in order of IntegratedStateType, and then in the order of the + * vector of PropagatorSettings of given type. + */ +template< typename StateScalarType > +Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > createCombinedInitialState( + const std::map< IntegratedStateType, std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > >& + propagatorSettingsList ) +{ + // Get total size of propagated state + int totalSize = getMultiTypePropagatorStateSize( propagatorSettingsList ); + + Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > combinedInitialState = + Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >::Zero( totalSize, 1 ); + + // Iterate over all propagation settings and add to total list + int currentIndex = 0; + Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > currentInitialState; + for( typename std::map< IntegratedStateType, + std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > >::const_iterator + typeIterator = propagatorSettingsList.begin( ); typeIterator != propagatorSettingsList.end( ); typeIterator++ ) + { + for( unsigned int i = 0; i < typeIterator->second.size( ); i++ ) + { + currentInitialState = typeIterator->second.at( i )->getInitialStates( ); + combinedInitialState.segment( currentIndex, currentInitialState.rows( ) ) = currentInitialState; + currentIndex += currentInitialState.rows( ); + } + } + + return combinedInitialState; +} + +//! Class for defining settings for propagating multiple types of dynamics concurrently. +template< typename StateScalarType > +class MultiTypePropagatorSettings: public PropagatorSettings< StateScalarType > +{ +public: + + //! Constructor. + /*! + * Constructor + * \param propagatorSettingsMap List of propagator settings to use (state type as key). List of propagator settigns + * per type given as vector in map value. + * \param terminationSettings Settings for creating the object that checks whether the propagation is finished. + * \param dependentVariablesToSave Settings for the dependent variables that are to be saved during propagation + * (default none). + * \param printInterval Variable indicating how often (once per printInterval_ seconds or propagation independenty + * variable) the current state and time are to be printed to console (default never). + */ + MultiTypePropagatorSettings( + const std::map< IntegratedStateType, + std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > > propagatorSettingsMap, + const boost::shared_ptr< PropagationTerminationSettings > terminationSettings, + const boost::shared_ptr< DependentVariableSaveSettings > dependentVariablesToSave = + boost::shared_ptr< DependentVariableSaveSettings >( ), + const double printInterval = TUDAT_NAN ): + PropagatorSettings< StateScalarType >( + hybrid, createCombinedInitialState< StateScalarType >( propagatorSettingsMap ), + terminationSettings, dependentVariablesToSave, printInterval ), + propagatorSettingsMap_( propagatorSettingsMap ) + { + + } + + //! Constructor. + /*! + * Constructor + * \param propagatorSettingsVector Vector of propagator settings to use. + * \param terminationSettings Settings for creating the object that checks whether the propagation is finished. + * \param dependentVariablesToSave Settings for the dependent variables that are to be saved during propagation + * (default none). + * \param printInterval Variable indicating how often (once per printInterval_ seconds or propagation independenty + * variable) the current state and time are to be printed to console (default never). + */ + MultiTypePropagatorSettings( + const std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > propagatorSettingsVector, + const boost::shared_ptr< PropagationTerminationSettings > terminationSettings, + const boost::shared_ptr< DependentVariableSaveSettings > dependentVariablesToSave = + boost::shared_ptr< DependentVariableSaveSettings >( ), + const double printInterval = TUDAT_NAN ): + PropagatorSettings< StateScalarType >( + hybrid, Eigen::VectorXd::Zero( 0 ), + terminationSettings, dependentVariablesToSave, printInterval ) + { + for( unsigned int i = 0; i < propagatorSettingsVector.size( ); i++ ) + { + propagatorSettingsMap_[ propagatorSettingsVector.at( i )->stateType_ ].push_back( + propagatorSettingsVector.at( i ) ); + } + + this->initialStates_ = createCombinedInitialState< StateScalarType >( propagatorSettingsMap_ ); + } + + //! Destructor + ~MultiTypePropagatorSettings( ){ } + + //! Function to reset the initial state used as input for numerical integration + /*! + * Function to reset the initial state used as input for numerical integration + * \param initialBodyStates New initial state used as input for numerical integration, sorted in order of + * IntegratedStateType, and then in the order of the vector of PropagatorSettings of given type. + */ + void resetInitialStates( const Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >& initialBodyStates ) + { + // Iterate over all propagator settings. + int currentStartIndex = 0; + for( typename std::map< IntegratedStateType, + std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > >::iterator + propagatorIterator = propagatorSettingsMap_.begin( ); propagatorIterator != propagatorSettingsMap_.end( ); + propagatorIterator++ ) + { + for( unsigned int i = 0; i < propagatorIterator->second.size( ); i++ ) + { + // Get current state size + int currentParameterSize = propagatorIterator->second.at( i )->getInitialStates( ).rows( ); + + // Check consistency + if( currentParameterSize + currentStartIndex > initialBodyStates.rows( ) ) + { + throw std::runtime_error( + "Error when resetting multi-type state, sizes are incompatible " ); + } + + // Reset state for current settings + propagatorIterator->second.at( i )->resetInitialStates( + initialBodyStates.block( currentStartIndex, 0, currentParameterSize, 1 ) ); + currentStartIndex += currentParameterSize; + + } + } + + // Check consistency + if( currentStartIndex != initialBodyStates.rows( ) ) + { + std::string errorMessage = "Error when resetting multi-type state, total size is incompatible "+ + boost::lexical_cast< std::string >( currentStartIndex ) + + boost::lexical_cast< std::string >( initialBodyStates.rows( ) ); + throw std::runtime_error( errorMessage ); + } + } + + //! List of propagator settings to use + /*! + * List of propagator settings to use (state type as key). List of propagator settigns + * per type given as vector in map value. + */ + + std::map< IntegratedStateType, std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > > + propagatorSettingsMap_; + +}; + +template< typename StateScalarType > +//! Function to retrieve the list of integrated state types and reference ids +/*! +* Function to retrieve the list of integrated state types and reference ids. For translational and rotational dynamics, +* the id refers only to the body being propagated (and the second entry of the pair is empty: ""). For proper time +* propagation, a body and a reference point may be provided, resulting in non-empty first and second pair entries. +* \param propagatorSettings Settings that are to be used for the propagation. +* \return List of integrated state types and reference ids +*/ +std::map< IntegratedStateType, std::vector< std::pair< std::string, std::string > > > getIntegratedTypeAndBodyList( + const boost::shared_ptr< PropagatorSettings< StateScalarType > > propagatorSettings ) +{ + std::map< IntegratedStateType, std::vector< std::pair< std::string, std::string > > > integratedStateList; + + // Identify propagator type + switch( propagatorSettings->stateType_ ) + { + case hybrid: + { + boost::shared_ptr< MultiTypePropagatorSettings< StateScalarType > > multiTypePropagatorSettings = + boost::dynamic_pointer_cast< MultiTypePropagatorSettings< StateScalarType > >( propagatorSettings ); + + std::map< IntegratedStateType, std::vector< std::pair< std::string, std::string > > > singleTypeIntegratedStateList; + + for( typename std::map< IntegratedStateType, + std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > >::const_iterator + typeIterator = multiTypePropagatorSettings->propagatorSettingsMap_.begin( ); + typeIterator != multiTypePropagatorSettings->propagatorSettingsMap_.end( ); typeIterator++ ) + { + if( typeIterator->first != hybrid ) + { + for( unsigned int i = 0; i < typeIterator->second.size( ); i++ ) + { + singleTypeIntegratedStateList = getIntegratedTypeAndBodyList< StateScalarType >( + typeIterator->second.at( i ) ); + + if( singleTypeIntegratedStateList.begin( )->first != typeIterator->first + || singleTypeIntegratedStateList.size( ) != 1 ) + { + std::string errorMessage = "Error when making integrated state list for hybrid propagator, inconsistency encountered " + + boost::lexical_cast< std::string >( singleTypeIntegratedStateList.begin( )->first ) + " " + + boost::lexical_cast< std::string >( typeIterator->first ) + " " + + boost::lexical_cast< std::string >( singleTypeIntegratedStateList.size( ) ) + " " + + boost::lexical_cast< std::string >( singleTypeIntegratedStateList.begin( )->second.size( ) ); + throw std::runtime_error( errorMessage ); + } + else + { + for( unsigned int j = 0; j < singleTypeIntegratedStateList[ typeIterator->first ].size( ); j++ ) + { + integratedStateList[ typeIterator->first ].push_back( + singleTypeIntegratedStateList.begin( )->second.at( j ) ); + } + + } + } + } + else + { + throw std::runtime_error( "Error when making integrated state list, cannot handle hybrid propagator inside hybrid propagator" ); + } + } + break; + } + case transational_state: + { + boost::shared_ptr< TranslationalStatePropagatorSettings< StateScalarType > > + translationalPropagatorSettings = boost::dynamic_pointer_cast< + TranslationalStatePropagatorSettings< StateScalarType > >( propagatorSettings ); + + if( translationalPropagatorSettings == NULL ) + { + throw std::runtime_error( "Error getting integrated state type list, translational state input inconsistent" ); + } + + // Retrieve list of integrated bodies in correct formatting. + std::vector< std::pair< std::string, std::string > > integratedBodies; + for( unsigned int i = 0; i < translationalPropagatorSettings->bodiesToIntegrate_.size( ); i++ ) + { + integratedBodies.push_back( std::make_pair( translationalPropagatorSettings->bodiesToIntegrate_.at( i ), "" ) ); + } + integratedStateList[ transational_state ] = integratedBodies; + + break; + } + case body_mass_state: + { + boost::shared_ptr< MassPropagatorSettings< StateScalarType > > + massPropagatorSettings = boost::dynamic_pointer_cast< + MassPropagatorSettings< StateScalarType > >( propagatorSettings ); + if( massPropagatorSettings == NULL ) + { + throw std::runtime_error( "Error getting integrated state type list, mass state input inconsistent" ); + } + + // Retrieve list of integrated bodies in correct formatting. + std::vector< std::pair< std::string, std::string > > integratedBodies; + for( unsigned int i = 0; i < massPropagatorSettings->bodiesWithMassToPropagate_.size( ); i++ ) + { + integratedBodies.push_back( std::make_pair( + massPropagatorSettings->bodiesWithMassToPropagate_.at( i ), "" ) ); + } + integratedStateList[ body_mass_state ] = integratedBodies; + + break; + } + default: + throw std::runtime_error( "Error, could not process integrated state type ingetIntegratedTypeAndBodyList " + + boost::lexical_cast< std::string >( propagatorSettings->stateType_ ) ); + } + + return integratedStateList; +} + +} // namespace propagators + +} // namespace tudat + +namespace std +{ + +//! Hash for IntegratedStateType enum. +template< > +struct hash< tudat::propagators::IntegratedStateType > +{ + typedef tudat::propagators::IntegratedStateType argument_type; + typedef size_t result_type; + + result_type operator () (const argument_type& x) const + { + using type = typename std::underlying_type::type; + return std::hash< type >( )( static_cast< type >( x ) ); + } +}; + +} // namespace std + +#endif // TUDAT_PROPAGATIONSETTINGS_H diff --git a/Tudat/SimulationSetup/PropagationSetup/propagationTermination.cpp b/Tudat/SimulationSetup/PropagationSetup/propagationTermination.cpp new file mode 100644 index 0000000000..c5e18b7c31 --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/propagationTermination.cpp @@ -0,0 +1,162 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/SimulationSetup/PropagationSetup/propagationTermination.h" + +namespace tudat +{ + +namespace propagators +{ + +//! Function to check whether the propagation is to be be stopped +bool FixedTimePropagationTerminationCondition::checkStopCondition( const double time ) +{ + bool stopPropagation = 0; + + // Check whether stop time has been reached + if( propagationDirectionIsPositive_ && ( time >= stopTime_ ) ) + { + stopPropagation = 1; + } + else if( !propagationDirectionIsPositive_ && ( time <= stopTime_ ) ) + { + stopPropagation = 1; + } + return stopPropagation; +} + +//! Function to check whether the propagation is to be be stopped +bool SingleVariableLimitPropagationTerminationCondition::checkStopCondition( const double time ) +{ + bool stopPropagation = 0; + double currentVariable = variableRetrievalFuntion_( ); + if( useAsLowerBound_ && ( currentVariable < limitingValue_ ) ) + { + stopPropagation = 1; + } + else if( !useAsLowerBound_ && ( currentVariable > limitingValue_ ) ) + { + stopPropagation = 1; + } + return stopPropagation; +} + +//! Function to check whether the propagation is to be be stopped +bool HybridPropagationTerminationCondition::checkStopCondition( const double time ) +{ + // Check if single condition is fulfilled. + if( fulFillSingleCondition_ ) + { + bool stopPropagation = 0; + for( unsigned int i = 0; i < propagationTerminationCondition_.size( ); i++ ) + { + if( propagationTerminationCondition_.at( i )->checkStopCondition( time ) ) + { + stopPropagation = 1; + break; + } + } + return stopPropagation; + } + // Check all conditions are fulfilled. + else + { + bool stopPropagation = 1; + for( unsigned int i = 0; i < propagationTerminationCondition_.size( ); i++ ) + { + if( !propagationTerminationCondition_.at( i )->checkStopCondition( time ) ) + { + stopPropagation = 0; + break; + } + } + return stopPropagation; + } +} + + +//! Function to create propagation termination conditions from associated settings +boost::shared_ptr< PropagationTerminationCondition > createPropagationTerminationConditions( + const boost::shared_ptr< PropagationTerminationSettings > terminationSettings, + const simulation_setup::NamedBodyMap& bodyMap, + const double initialTimeStep ) +{ + boost::shared_ptr< PropagationTerminationCondition > propagationTerminationCondition; + + // Check termination type. + switch( terminationSettings->terminationType_ ) + { + case time_stopping_condition: + { + // Create stopping time termination condition. + boost::shared_ptr< PropagationTimeTerminationSettings > timeTerminationSettings = + boost::dynamic_pointer_cast< PropagationTimeTerminationSettings >( terminationSettings ); + propagationTerminationCondition = boost::make_shared< FixedTimePropagationTerminationCondition >( + timeTerminationSettings->terminationTime_, ( initialTimeStep > 0 ) ? true : false ); + break; + } + case dependent_variable_stopping_condition: + { + boost::shared_ptr< PropagationDependentVariableTerminationSettings > dependentVariableTerminationSettings = + boost::dynamic_pointer_cast< PropagationDependentVariableTerminationSettings >( terminationSettings ); + + // Get dependent variable function + boost::function< double( ) > dependentVariableFunction; + if( getDependentVariableSize( + dependentVariableTerminationSettings->dependentVariableSettings_->variableType_ ) == 1 ) + { + dependentVariableFunction = + getDoubleDependentVariableFunction( + dependentVariableTerminationSettings->dependentVariableSettings_, bodyMap ); + } + else + { + throw std::runtime_error( "Error, cannot make stopping condition from vector dependent variable" ); + } + + // Create dependent variable termination condition. + propagationTerminationCondition = boost::make_shared< SingleVariableLimitPropagationTerminationCondition >( + dependentVariableTerminationSettings->dependentVariableSettings_, + dependentVariableFunction, dependentVariableTerminationSettings->limitValue_, + dependentVariableTerminationSettings->useAsLowerLimit_ ); + break; + } + case hybrid_stopping_condition: + { + boost::shared_ptr< PropagationHybridTerminationSettings > hybridTerminationSettings = + boost::dynamic_pointer_cast< PropagationHybridTerminationSettings >( terminationSettings ); + + // Recursively create termination condition list. + std::vector< boost::shared_ptr< PropagationTerminationCondition > > propagationTerminationConditionList; + for( unsigned int i = 0; i < hybridTerminationSettings->terminationSettings_.size( ); i++ ) + { + propagationTerminationConditionList.push_back( + createPropagationTerminationConditions( + hybridTerminationSettings->terminationSettings_.at( i ), + bodyMap, initialTimeStep ) ); + } + propagationTerminationCondition = boost::make_shared< HybridPropagationTerminationCondition >( + propagationTerminationConditionList, hybridTerminationSettings->fulFillSingleCondition_ ); + break; + } + default: + std::string errorMessage = "Error, stopping condition type " + boost::lexical_cast< std::string >( + terminationSettings->terminationType_ ) + "not recognized when making stopping conditions object"; + throw std::runtime_error( errorMessage ); + break; + } + return propagationTerminationCondition; + +} // namespace propagators + +} // namespace tudat + +} diff --git a/Tudat/SimulationSetup/PropagationSetup/propagationTermination.h b/Tudat/SimulationSetup/PropagationSetup/propagationTermination.h new file mode 100644 index 0000000000..6369899eb1 --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/propagationTermination.h @@ -0,0 +1,191 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_PROPAGATIONTERMINATIONCONDITIONS_H +#define TUDAT_PROPAGATIONTERMINATIONCONDITIONS_H + +#include + +#include "Tudat/SimulationSetup/PropagationSetup/propagationOutput.h" +#include "Tudat/SimulationSetup/PropagationSetup/propagationSettings.h" + +namespace tudat +{ + +namespace propagators +{ + +//! Base class for checking whether the numerical propagation is to be stopped at current time step or not +/*! + * Base class for checking whether the numerical propagation is to be stopped at current time step or not. Derived + * classes implement the various types of conditions (and associated threshold values) under which the propagation is to + * be stopped. + */ +class PropagationTerminationCondition +{ +public: + + //! Constructor + PropagationTerminationCondition( ){ } + + //! Destructor + virtual ~PropagationTerminationCondition( ){ } + + //! (Pure virtual) function to check whether the propagation should be stopped + /*! + * (Pure virtual) function to check whether the propagation should be stopped. Note that the accelerations and + * environment must be updated (done automatically during numerical propagation) to check the stopping condition. + * \param time Current time in propagation + * \return True if propagation is to be stopped, false otherwise. + */ + virtual bool checkStopCondition( const double time ) = 0; +}; + +//! Class for stopping the propagation after a fixed amount of time (i.e. for certain independent variable value) +class FixedTimePropagationTerminationCondition: public PropagationTerminationCondition +{ +public: + + //! Constructor + /*! + * Constructor + * \param stopTime Time at which the propagation is to stop. + * \param propagationDirectionIsPositive Boolean denoting whether propagation is forward (if true) or backwards + * (if false) in time. + */ + FixedTimePropagationTerminationCondition( + const double stopTime, + const bool propagationDirectionIsPositive ): + stopTime_( stopTime ), + propagationDirectionIsPositive_( propagationDirectionIsPositive ){ } + + + //! Function to check whether the propagation is to be be stopped + /*! + * Function to check whether the propagation is to be be stopped, i.e. whether the stopTime_ has been reached or not. + * \param time Current time in propagation + * \return True if propagation is to be stopped, false otherwise. + */ + bool checkStopCondition( const double time ); + +private: + + //! Time at which the propagation is to stop. + double stopTime_; + + //! Boolean denoting whether propagation is forward (if true) or backwards (if false) in time. + bool propagationDirectionIsPositive_; +}; + +//! Class for stopping the propagation when a dependent variable reaches a given value (either upper or lower bound) +class SingleVariableLimitPropagationTerminationCondition: public PropagationTerminationCondition +{ +public: + + //! Constructor + /*! + * Constructor + * \param dependentVariableSettings Settings for dependent variable that is to be checked + * \param variableRetrievalFuntion Function returning the dependent variable. + * \param limitingValue Value at which the propagation is to be stopped + * \param useAsLowerBound Boolean denoting whether the propagation should stop if the dependent variable goes below + * (if true) or above (if false) limitingValue + */ + SingleVariableLimitPropagationTerminationCondition( + const boost::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings, + const boost::function< double( ) > variableRetrievalFuntion, + const double limitingValue, + const bool useAsLowerBound ): + dependentVariableSettings_( dependentVariableSettings ), variableRetrievalFuntion_( variableRetrievalFuntion ), + limitingValue_( limitingValue ), useAsLowerBound_( useAsLowerBound ){ } + + //! Destructor. + ~SingleVariableLimitPropagationTerminationCondition( ){ } + + //! Function to check whether the propagation is to be be stopped + /*! + * Function to check whether the propagation is to be be stopped, i.e. whether the given dependent variable has been + * reached or not. + * \return True if propagation is to be stopped, false otherwise. + */ + bool checkStopCondition( const double time ); + +private: + + //! Settings for dependent variable that is to be checked + boost::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings_; + + //! Function returning the dependent variable. + boost::function< double( ) > variableRetrievalFuntion_; + + //! Value at which the propagation is to be stopped + double limitingValue_; + + //! Boolean denoting whether the propagation should stop if the dependent variable goes below + //! (if true) or above (if false) limitingValue + bool useAsLowerBound_; +}; + +//! Class for stopping the propagation when one or all of a given set of stopping conditions is reached. +class HybridPropagationTerminationCondition: public PropagationTerminationCondition +{ +public: + + //! Constructor + /*! + * Constructor + * \param propagationTerminationCondition List of termination conditions that are checked when calling + * checkStopCondition is called. + * \param fulFillSingleCondition Boolean denoting whether a single (if true) or all (if false) of the entries in the + * propagationTerminationCondition_ should return true from the checkStopCondition function to stop the propagation. + */ + HybridPropagationTerminationCondition( + const std::vector< boost::shared_ptr< PropagationTerminationCondition > > propagationTerminationCondition, + const bool fulFillSingleCondition = 0 ): + propagationTerminationCondition_( propagationTerminationCondition ), + fulFillSingleCondition_( fulFillSingleCondition ){ } + + //! Function to check whether the propagation is to be be stopped + /*! + * Function to check whether the propagation is to be be stopped, i.e. one or all (depending on value of + * fulFillSingleCondition_) of the stopping conditions are fulfilled. + * \return True if propagation is to be stopped, false otherwise. + */ + bool checkStopCondition( const double time ); + +private: + + //! List of termination conditions that are checked when calling checkStopCondition is called. + std::vector< boost::shared_ptr< PropagationTerminationCondition > > propagationTerminationCondition_; + + //! Boolean denoting whether a single (if true) or all (if false) of the entries in the propagationTerminationCondition_ + //! should return true from the checkStopCondition function to stop the propagation. + bool fulFillSingleCondition_; +}; + +//! Function to create propagation termination conditions from associated settings +/*! + * Function to create propagation termination conditions from associated settings + * \param terminationSettings Settings for propagation termination conditions + * \param bodyMap List of body objects that contains all environment models + * \param initialTimeStep Time step at first call of numerical integration. + * \return Object used to check whether propagation is to be stopped or not. + */ +boost::shared_ptr< PropagationTerminationCondition > createPropagationTerminationConditions( + const boost::shared_ptr< PropagationTerminationSettings > terminationSettings, + const simulation_setup::NamedBodyMap& bodyMap, + const double initialTimeStep ); + +} // namespace propagators + +} // namespace tudat + + +#endif // TUDAT_PROPAGATIONTERMINATIONCONDITIONS_H diff --git a/Tudat/SimulationSetup/PropagationSetup/propagationTerminationSettings.h b/Tudat/SimulationSetup/PropagationSetup/propagationTerminationSettings.h new file mode 100644 index 0000000000..35caf1f8ed --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/propagationTerminationSettings.h @@ -0,0 +1,165 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_PROPAGATIONTERMINATIONSETTINGS_H +#define TUDAT_PROPAGATIONTERMINATIONSETTINGS_H + +#include + +#include + +namespace tudat +{ + +namespace propagators +{ + +class SingleDependentVariableSaveSettings; + +//! Enum listing the available types of propagation termination settings. +enum PropagationTerminationTypes +{ + time_stopping_condition, + dependent_variable_stopping_condition, + hybrid_stopping_condition +}; + + +//! Base class for defining propagation termination settings. +/*! + * Base class for defining propagation termination settings, i.e. conditions on which the porpagation is deemed to be + * 'finished'. Each particular type of stopping condition requires a different derived class. + */ +class PropagationTerminationSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param terminationType Type of stopping condition that is to be used. + */ + PropagationTerminationSettings( const PropagationTerminationTypes terminationType ): + terminationType_( terminationType ){ } + + //! Destructor + virtual ~PropagationTerminationSettings( ){ } + + //! Type of stopping condition that is to be used. + PropagationTerminationTypes terminationType_; +}; + +//! Class for propagation stopping conditions settings: stopping the propagation after a fixed amount of time +/*! + * Class for propagation stopping conditions settings: stopping the propagation after a fixed amount of time. Note that the + * propagator will finish a given time step, slightly surpassing the defined final time. + */ +class PropagationTimeTerminationSettings: public PropagationTerminationSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param terminationTime Maximum time for the propagation, upon which the propagation is to be stopped + */ + PropagationTimeTerminationSettings( const double terminationTime ): + PropagationTerminationSettings( time_stopping_condition ), + terminationTime_( terminationTime ){ } + + //! Destructor + ~PropagationTimeTerminationSettings( ){ } + + //! Maximum time for the propagation, upon which the propagation is to be stopped + double terminationTime_; +}; + +//! Class for propagation stopping conditions settings: stopping the propagation after a given dependent variable reaches a +//! certain value. +/*! + * Class for propagation stopping conditions settings: stopping the propagation after a given dependent variable reaches a + * certain value. The limit value may be set as both an upper or lower bound (i.e. the propagation continues while the + * value is below or above some given value). + * Note that the propagator will finish a given time step, slightly surpassing the defined limit value of the dependent + * variable + */ +class PropagationDependentVariableTerminationSettings: public PropagationTerminationSettings +{ +public: + //! Constructor + /*! + * Constructor + * \param dependentVariableSettings Settings for dependent variable that is to be checked + * \param limitValue Value at which the propagation is to be stopped + * \param useAsLowerLimit Boolean denoting whether the propagation should stop if the dependent variable goes below + * (if true) or above (if false) limitingValue + */ + PropagationDependentVariableTerminationSettings( + const boost::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings, + const double limitValue, + const bool useAsLowerLimit ): + PropagationTerminationSettings( dependent_variable_stopping_condition ), + dependentVariableSettings_( dependentVariableSettings ), + limitValue_( limitValue ), useAsLowerLimit_( useAsLowerLimit ){ } + + //! Destructor + ~PropagationDependentVariableTerminationSettings( ){ } + + //! Settings for dependent variable that is to be checked + boost::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings_; + + //! Value at which the propagation is to be stopped + double limitValue_; + + //! Boolean denoting whether the propagation should stop if the dependent variable goes below (if true) or above + //! (if false) limitingValue + bool useAsLowerLimit_; +}; + +//! Class for propagation stopping conditions settings: combination of other stopping conditions. +/*! + * Class for propagation stopping conditions settings: combination of other stopping conditions. This class can be used + * to define that all of any number of conditions must be met, or that a single of these settings must be met to + * stop the propagation. + */ +class PropagationHybridTerminationSettings: public PropagationTerminationSettings +{ +public: + + //! Constructor + /*! + * \brief PropagationHybridTerminationSettings + * \param terminationSettings List of termination settings for which stopping conditions are created. + * \param fulFillSingleCondition Boolean denoting whether a single (if true) or all (if false) of the conditions + * defined by the entries in the terminationSettings list should be met. + */ + PropagationHybridTerminationSettings( + const std::vector< boost::shared_ptr< PropagationTerminationSettings > > terminationSettings, + const bool fulFillSingleCondition = 0 ): + PropagationTerminationSettings( hybrid_stopping_condition ), + terminationSettings_( terminationSettings ), + fulFillSingleCondition_( fulFillSingleCondition ){ } + + //! Destructor + ~PropagationHybridTerminationSettings( ){ } + + //! List of termination settings for which stopping conditions are created. + std::vector< boost::shared_ptr< PropagationTerminationSettings > > terminationSettings_; + + //! Boolean denoting whether a single (if true) or all (if false) of the conditions + //! defined by the entries in the terminationSettings list should be met. + bool fulFillSingleCondition_; +}; + +} // namespace propagators + +} // namespace tudat + +#endif // TUDAT_PROPAGATIONTERMINATIONSETTINGS_H diff --git a/Tudat/SimulationSetup/PropagationSetup/setNumericallyIntegratedStates.cpp b/Tudat/SimulationSetup/PropagationSetup/setNumericallyIntegratedStates.cpp new file mode 100644 index 0000000000..6c331ac8d0 --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/setNumericallyIntegratedStates.cpp @@ -0,0 +1,42 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/SimulationSetup/PropagationSetup/setNumericallyIntegratedStates.h" +#include "Tudat/Mathematics/Interpolators/lagrangeInterpolator.h" + +namespace tudat +{ + +namespace propagators +{ + +//! Function to create an interpolator for the new translational state of a body. +template< > +boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::Matrix< double, 6, 1 > > > +createStateInterpolator( const std::map< double, Eigen::Matrix< double, 6, 1 > >& stateMap ) +{ + return boost::make_shared< + interpolators::LagrangeInterpolator< double, Eigen::Matrix< double, 6, 1 > > >( stateMap, 6 ); +} + +//! Function to create an interpolator for the new translational state of a body. +template< > +boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::Matrix< long double, 6, 1 > > > +createStateInterpolator( const std::map< double, Eigen::Matrix< long double, 6, 1 > >& stateMap ) +{ + return boost::make_shared< + interpolators::LagrangeInterpolator< double, + Eigen::Matrix< long double, 6, 1 > > >( stateMap, 6 ); +} + + +} // namespace propagators + +} // namespace tudat diff --git a/Tudat/Astrodynamics/Propagators/setNumericallyIntegratedStates.h b/Tudat/SimulationSetup/PropagationSetup/setNumericallyIntegratedStates.h similarity index 69% rename from Tudat/Astrodynamics/Propagators/setNumericallyIntegratedStates.h rename to Tudat/SimulationSetup/PropagationSetup/setNumericallyIntegratedStates.h index 6761178415..dd7bbc1a96 100644 --- a/Tudat/Astrodynamics/Propagators/setNumericallyIntegratedStates.h +++ b/Tudat/SimulationSetup/PropagationSetup/setNumericallyIntegratedStates.h @@ -11,10 +11,10 @@ #ifndef TUDAT_SETNUMERICALLYINTEGRATEDSTATES_H #define TUDAT_SETNUMERICALLYINTEGRATEDSTATES_H -#include "Tudat/SimulationSetup/body.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" #include "Tudat/Astrodynamics/Ephemerides/frameManager.h" #include "Tudat/Astrodynamics/Ephemerides/tabulatedEphemeris.h" -#include "Tudat/Astrodynamics/Propagators/propagationSettings.h" +#include "Tudat/SimulationSetup/PropagationSetup/propagationSettings.h" namespace tudat @@ -74,6 +74,7 @@ void resetIntegratedEphemerisOfBody( * integrated states Additionally, it changes the origin of the reference frame in which the states * are given, by using the integrationToEphemerisFrameFunction input variable. * \param bodyIndex Index of integrated body for which the state is to be retrieved + * \param startIndex Index in entries of equationsOfMotionNumericalSolution where the translational states start. * \param equationsOfMotionNumericalSolution Full numerical solution of numerical integrator, * already converted to Cartesian states (w.r.t. the integration origin of the body of bodyIndex) * \param integrationToEphemerisFrameFunction Function to provide the state of the ephemeris origin @@ -83,6 +84,7 @@ void resetIntegratedEphemerisOfBody( template< typename TimeType, typename StateScalarType > std::map< TimeType, Eigen::Matrix< StateScalarType, 6, 1 > > convertNumericalSolutionToEphemerisInput( const int bodyIndex, + const int startIndex, const std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& equationsOfMotionNumericalSolution, const boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) > @@ -98,7 +100,7 @@ std::map< TimeType, Eigen::Matrix< StateScalarType, 6, 1 > > convertNumericalSol bodyIterator = equationsOfMotionNumericalSolution.begin( ); bodyIterator != equationsOfMotionNumericalSolution.end( ); bodyIterator++ ) { - ephemerisTable[ bodyIterator->first ] = bodyIterator->second.block( 6 * bodyIndex, 0, 6, 1 ); + ephemerisTable[ bodyIterator->first ] = bodyIterator->second.block( startIndex + 6 * bodyIndex, 0, 6, 1 ); } } // Else, extract indices and add required translation from integrationToEphemerisFrameFunction @@ -109,7 +111,7 @@ std::map< TimeType, Eigen::Matrix< StateScalarType, 6, 1 > > convertNumericalSol bodyIterator != equationsOfMotionNumericalSolution.end( ); bodyIterator++ ) { - ephemerisTable[ bodyIterator->first ] = bodyIterator->second.block( 6 * bodyIndex, 0, 6, 1 ) - + ephemerisTable[ bodyIterator->first ] = bodyIterator->second.block( startIndex + 6 * bodyIndex, 0, 6, 1 ) - integrationToEphemerisFrameFunction( bodyIterator->first ); } } @@ -134,6 +136,7 @@ createStateInterpolator( * \param bodyMap List of bodies used in simulations. * \param bodiesToIntegrate List of names of bodies which are numericall integrated (in the order in * which they are in the equationsOfMotionNumericalSolution map. + * \param startIndex Index in entries of equationsOfMotionNumericalSolution where the translational states start. * \param ephemerisUpdateOrder Order in which to update the ephemeris objects. * \param equationsOfMotionNumericalSolution Numerical solution of translational equations of * motion, in Cartesian elements w.r.t. integratation origins. @@ -144,14 +147,12 @@ template< typename TimeType, typename StateScalarType > void createAndSetInterpolatorsForEphemerides( const simulation_setup::NamedBodyMap& bodyMap, const std::vector< std::string >& bodiesToIntegrate, + const int startIndex, const std::vector< std::string >& ephemerisUpdateOrder, - const std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& - equationsOfMotionNumericalSolution, - const std::map< std::string, - boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) > >& + const std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& equationsOfMotionNumericalSolution, + const std::map< std::string, boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) > >& integrationToEphemerisFrameFunctions = - std::map< std::string, - boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) > >( ) ) + std::map< std::string, boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) > >( ) ) { using namespace tudat::interpolators; @@ -180,11 +181,12 @@ void createAndSetInterpolatorsForEphemerides( // Create and reset interpolator. boost::shared_ptr< OneDimensionalInterpolator< TimeType, Eigen::Matrix< StateScalarType, 6, 1 > > > ephemerisInterpolator = - createStateInterpolator( convertNumericalSolutionToEphemerisInput( - bodyIndex, equationsOfMotionNumericalSolution, - integrationToEphemerisFrameFunction ) ); - resetIntegratedEphemerisOfBody( bodyMap, ephemerisInterpolator, - bodiesToIntegrate.at( bodyIndex ) ); + createStateInterpolator( + convertNumericalSolutionToEphemerisInput( + bodyIndex, startIndex, equationsOfMotionNumericalSolution, integrationToEphemerisFrameFunction ) ); + + resetIntegratedEphemerisOfBody( + bodyMap, ephemerisInterpolator, bodiesToIntegrate.at( bodyIndex ) ); } } @@ -195,6 +197,8 @@ void createAndSetInterpolatorsForEphemerides( * \param bodyMap List of bodies used in simulations. * \param bodiesToIntegrate List of names of bodies which are numerically integrated (in the order in * which they are in the equationsOfMotionNumericalSolution map. + * \param startIndexAndSize Pair with start index and total (contiguous) size of integrated states in entries of + * equationsOfMotionNumericalSolution * \param ephemerisUpdateOrder Order in which to update the ephemeris objects (empty if arbitrary). * \param equationsOfMotionNumericalSolution Numerical solution of translational equations of * motion, in Cartesian elements w.r.t. integratation origins. @@ -204,14 +208,13 @@ void createAndSetInterpolatorsForEphemerides( template< typename TimeType, typename StateScalarType > void resetIntegratedEphemerides( const simulation_setup::NamedBodyMap& bodyMap, - const std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& - equationsOfMotionNumericalSolution, + const std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& equationsOfMotionNumericalSolution, const std::vector< std::string >& bodiesToIntegrate, + const std::pair< unsigned int, unsigned int > startIndexAndSize, std::vector< std::string > ephemerisUpdateOrder = std::vector< std::string >( ), - const std::map< std::string, - boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) > >& - integrationToEphemerisFrameFunctions = std::map< std::string, - boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) > >( ) ) + const std::map< std::string, boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) > >& + integrationToEphemerisFrameFunctions = + std::map< std::string, boost::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType ) > >( ) ) { // Set update order arbitrarily if no order is provided. if( ephemerisUpdateOrder.size( ) == 0 ) @@ -224,24 +227,68 @@ void resetIntegratedEphemerides( throw std::runtime_error( "Error when resetting ephemerides, input vectors have inconsistent size" ); } + if( equationsOfMotionNumericalSolution.begin( )->second.rows( ) < startIndexAndSize.first + startIndexAndSize.second ) + { + throw std::runtime_error( "Error when resetting ephemerides, input solution inconsistent with start index and size." ); + } + + if( startIndexAndSize.second != 6 * bodiesToIntegrate.size( ) ) + { + throw std::runtime_error( "Error when resetting ephemerides, number of bodies inconsistent with input size." ); + + } + // Create interpolators from numerical integration results (states) at discrete times. createAndSetInterpolatorsForEphemerides( - bodyMap, bodiesToIntegrate, ephemerisUpdateOrder, equationsOfMotionNumericalSolution, - integrationToEphemerisFrameFunctions ); + bodyMap, bodiesToIntegrate, startIndexAndSize.first, ephemerisUpdateOrder, + equationsOfMotionNumericalSolution, integrationToEphemerisFrameFunctions ); } -//! Function to determine in which order the ephemerides are to be updated +//! Resets the mass models of the integrated bodies from the numerical integration results. /*! - * Function to determine in which order the ephemerides are to be updated. The order depends on the - * dependencies between the ephemeris/integration origins. - * \param integratedBodies List of bodies that are numerically integrated. - * \param centralBodies List of origins w.r.t. the integratedBodies' translational dynamics is propagated. - * \param ephemerisOrigins Origin of the Ephemeris objects of the integratedBodies. - * \return + * Resets the mass models of the integrated bodies from the numerical integration results. + * \param bodyMap List of bodies used in simulations. + * \param equationsOfMotionNumericalSolution Numerical solution of the body masses. + * \param bodiesToIntegrate List of names of bodies for which mass is numerically integrated (in the order in + * which they are in the equationsOfMotionNumericalSolution map. + * \param startIndexAndSize Pair with start index and total (contiguous) size of integrated states in entries of + * equationsOfMotionNumericalSolution */ -std::vector< std::string > determineEphemerisUpdateorder( std::vector< std::string > integratedBodies, - std::vector< std::string > centralBodies, - std::vector< std::string > ephemerisOrigins ); +template< typename TimeType, typename StateScalarType > +void resetIntegratedBodyMass( + const simulation_setup::NamedBodyMap& bodyMap, + const std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& equationsOfMotionNumericalSolution, + const std::vector< std::string >& bodiesToIntegrate , + const std::pair< unsigned int, unsigned int > startIndexAndSize ) +{ + if( startIndexAndSize.second != bodiesToIntegrate.size( ) ) + { + throw std::runtime_error( "Error when resetting body masses, number of bodies inconsistent with input size." ); + } + + // Iterate over all bodies for which mass is propagated. + for( unsigned int i = 0; i < bodiesToIntegrate.size( ); i++ ) + { + std::map< double, double > currentBodyMassMap; + + // Create mass map with double entries. + for( typename std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >::const_iterator + stateIterator = equationsOfMotionNumericalSolution.begin( ); + stateIterator != equationsOfMotionNumericalSolution.end( ); stateIterator++ ) + { + currentBodyMassMap[ static_cast< double >( stateIterator->first ) ] = + static_cast< double >( stateIterator->second( startIndexAndSize.first + i ) ); + } + + typedef interpolators::OneDimensionalInterpolator< double, double > LocalInterpolator; + + // Create and set interpolator. + bodyMap.at( bodiesToIntegrate.at( i ) )->setBodyMassFunction( boost::bind( + static_cast< double( LocalInterpolator::* )( const double ) > + ( &LocalInterpolator::interpolate ), + boost::make_shared< interpolators::LagrangeInterpolatorDouble >( currentBodyMassMap, 6 ), _1 ) ); + } +} //! Base class for settings how numerically integrated states are processed /*! @@ -331,6 +378,8 @@ class TranslationalStateIntegratedStateProcessor: public IntegratedStateProcesso bodiesToIntegrate_, frameManager ); } + ~TranslationalStateIntegratedStateProcessor( ){ } + //! Function processing translational state in the full numericalSolution /*! * Function that processes the entries of the translational state in the full numericalSolution, @@ -344,7 +393,7 @@ class TranslationalStateIntegratedStateProcessor: public IntegratedStateProcesso Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& numericalSolution ) { resetIntegratedEphemerides< TimeType, StateScalarType >( - bodyMap_, numericalSolution, bodiesToIntegrate_, ephemerisUpdateOrder_, + bodyMap_, numericalSolution, bodiesToIntegrate_, this->startIndexAndSize_, ephemerisUpdateOrder_, integrationToEphemerisFrameFunctions_ ); } @@ -369,6 +418,58 @@ class TranslationalStateIntegratedStateProcessor: public IntegratedStateProcesso integrationToEphemerisFrameFunctions_; }; +//! Class used for processing numerically integrated masses of bodies. +template< typename TimeType, typename StateScalarType > +class BodyMassIntegratedStateProcessor: public IntegratedStateProcessor< TimeType, StateScalarType > +{ +public: + + //! Constructor + /*! + * Constructor + * \param startIndex Index in the state vector where the translational state starts. + * \param bodyMap List of bodies used in simulations. + * \param bodiesToIntegrate List of bodies for which the mass is numerically + * integrated. Order in this vector is the same as the order in state vector. + */ + BodyMassIntegratedStateProcessor( + const int startIndex, + const simulation_setup::NamedBodyMap& bodyMap, + const std::vector< std::string >& bodiesToIntegrate ): + IntegratedStateProcessor< TimeType, StateScalarType >( + body_mass_state, std::make_pair( startIndex, bodiesToIntegrate.size( ) ) ), + bodyMap_( bodyMap ), bodiesToIntegrate_( bodiesToIntegrate ) + { } + + //! Destructor + ~BodyMassIntegratedStateProcessor( ){ } + + //! Function processing mass state in the full numericalSolution + /*! + * Function that processes the entries of the propagated mass in the full numericalSolution. + * \param numericalSolution Full numerical solution of state, in global representation (representation is constant + * for mass). + */ + void processIntegratedStates( + const std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& numericalSolution ) + { + resetIntegratedBodyMass( bodyMap_, numericalSolution, bodiesToIntegrate_, this->startIndexAndSize_ ); + } + +private: + + //! List of bodies used in simulations. + simulation_setup::NamedBodyMap bodyMap_; + + //! List of bodies for which the body mass is numerically integrated. + /*! + * List of bodies for which the body mass is numerically integrated. Order in this + * vector is the same as the order in state vector. + */ + std::vector< std::string > bodiesToIntegrate_; +}; + + //! Function checking feasibility of resetting the translational dynamics /*! * Function to check the feasibility of resetting the translational dynamics of a set of @@ -466,6 +567,50 @@ createIntegratedStateProcessors( // Check dynamics type. switch( propagatorSettings->stateType_ ) { + case hybrid: + { + boost::shared_ptr< MultiTypePropagatorSettings< StateScalarType > > multiTypePropagatorSettings = + boost::dynamic_pointer_cast< MultiTypePropagatorSettings< StateScalarType > >( propagatorSettings ); + std::map< IntegratedStateType, std::vector< boost::shared_ptr< IntegratedStateProcessor< TimeType, StateScalarType > > > > + singleTypeIntegratedStateProcessors; + int currentStartIndex = 0; + for( typename std::map< IntegratedStateType, std::vector< boost::shared_ptr< PropagatorSettings< StateScalarType > > > >::const_iterator + typeIterator = multiTypePropagatorSettings->propagatorSettingsMap_.begin( ); + typeIterator != multiTypePropagatorSettings->propagatorSettingsMap_.end( ); typeIterator++ ) + { + if( typeIterator->first != hybrid ) + { + for( unsigned int i = 0; i < typeIterator->second.size( ); i++ ) + { + singleTypeIntegratedStateProcessors = createIntegratedStateProcessors< TimeType, StateScalarType >( + typeIterator->second.at( i ), bodyMap, frameManager, currentStartIndex ); + + if( singleTypeIntegratedStateProcessors.size( ) != 1 ) + { + throw std::runtime_error( "Error when making hybrid integrated result processors, multiple types found" ); + } + else + { + if( singleTypeIntegratedStateProcessors.begin( )->second.size( ) != 1 ) + { + throw std::runtime_error( "Error when making hybrid integrated result processors, multiple processors of single type found" ); + } + } + + currentStartIndex += typeIterator->second.at( i )->getStateSize( ); + integratedStateProcessors[ singleTypeIntegratedStateProcessors.begin( )->first ].push_back( + singleTypeIntegratedStateProcessors.begin( )->second.at( 0 ) ); + } + } + else + { + throw std::runtime_error( "Error when making integrated state processors, cannot handle hybrid propagator inside hybrid propagator" ); + } + + } + + break; + } case transational_state: { @@ -480,13 +625,31 @@ createIntegratedStateProcessors( checkTranslationalStatesFeasibility< TimeType, StateScalarType >( translationalPropagatorSettings->bodiesToIntegrate_, bodyMap ); - // Create state propagator settings + // Create state processors integratedStateProcessors[ transational_state ].push_back( boost::make_shared< TranslationalStateIntegratedStateProcessor< TimeType, StateScalarType > >( startIndex, bodyMap, translationalPropagatorSettings->bodiesToIntegrate_, translationalPropagatorSettings->centralBodies_, frameManager ) ); break; } + case body_mass_state: + { + + // Check input feasibility + boost::shared_ptr< MassPropagatorSettings< StateScalarType > > + massPropagatorSettings = boost::dynamic_pointer_cast + < MassPropagatorSettings< StateScalarType > >( propagatorSettings ); + if( massPropagatorSettings == NULL ) + { + throw std::runtime_error( "Error, input type is inconsistent in createIntegratedStateProcessors" ); + } + + // Create mass processors. + integratedStateProcessors[ body_mass_state ].push_back( + boost::make_shared< BodyMassIntegratedStateProcessor< TimeType, StateScalarType > >( + startIndex, bodyMap, massPropagatorSettings->bodiesWithMassToPropagate_ ) ); + break; + } default: throw std::runtime_error( "Error, could not process integrated state type " + boost::lexical_cast< std::string >( propagatorSettings->stateType_ ) ); diff --git a/Tudat/SimulationSetup/PropagationSetup/thrustSettings.h b/Tudat/SimulationSetup/PropagationSetup/thrustSettings.h new file mode 100644 index 0000000000..0b9c4f6eb5 --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/thrustSettings.h @@ -0,0 +1,304 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_THRUSTSETTINGS_H +#define TUDAT_THRUSTSETTINGS_H + +#include + +namespace tudat +{ + +namespace simulation_setup +{ + +//! List of available types of thust direction guidance +enum ThrustDirectionGuidanceTypes +{ + colinear_with_state_segment_thrust_direction, + thrust_direction_from_existing_body_orientation, + custom_thrust_direction, + custom_thrust_orientation + +}; + +//! Class defining settings for the thrust direction +/*! + * Class for providing settings the thrust direction of a single thrust model. This class is a functional (base) class for + * settings of thrust direction that require no information in addition to their type. + * Classes defining settings for thrust direction requiring additional information must be derived from this class. + */ +class ThrustDirectionGuidanceSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param thrustDirectionType Type of thrust direction that is to be used. + * \param relativeBody Body relative to which thrust guidance algorithm is defined (empty if N/A). + */ + ThrustDirectionGuidanceSettings( + const ThrustDirectionGuidanceTypes thrustDirectionType, + const std::string relativeBody ): + thrustDirectionType_( thrustDirectionType ), relativeBody_( relativeBody ){ } + + //! Destructor. + virtual ~ThrustDirectionGuidanceSettings( ){ } + + //! Type of thrust direction that is to be used. + ThrustDirectionGuidanceTypes thrustDirectionType_; + + //! Body relative to which thrust guidance algorithm is defined. + std::string relativeBody_; +}; + +//! Thrust guidance settings for thrust that is colinear with position/velocity vector +class ThrustDirectionFromStateGuidanceSettings: public ThrustDirectionGuidanceSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param centralBody Body w.r.t. which the state of the body undergoing thust is computed. This state is then + * used to directly set the thrust direction. + * \param isColinearWithVelocity Boolean denoting whether thrust is colinear with velocity (if true) or position + * (if false) + * \param directionIsOppositeToVector Boolean denoting whether the thrust is in the direction of position/velocity + * of x_{thrusting body}-x_{central body}, or opposite this direction + */ + ThrustDirectionFromStateGuidanceSettings( + const std::string& centralBody, + const bool isColinearWithVelocity, + const bool directionIsOppositeToVector ): + ThrustDirectionGuidanceSettings( colinear_with_state_segment_thrust_direction, centralBody ), + isColinearWithVelocity_( isColinearWithVelocity ), + directionIsOppositeToVector_( directionIsOppositeToVector ){ } + + //! Destructor + ~ThrustDirectionFromStateGuidanceSettings( ){ } + + //! Boolean denoting whether thrust is colinear with velocity (if true) or position (if false) + bool isColinearWithVelocity_; + + //! Boolean denoting whether the thrust is in the direction of position/velocity of x_{thrusting body}-x_{central body}/ + bool directionIsOppositeToVector_; + +}; + +//! Class for defining custom thrust direction (i.e. predefined thrust function of time) +class CustomThrustDirectionSettings: public ThrustDirectionGuidanceSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param thrustDirectionFunction Function returning thrust direction unit vector as function fo time. + */ + CustomThrustDirectionSettings( + const boost::function< Eigen::Vector3d( const double ) > thrustDirectionFunction ): + ThrustDirectionGuidanceSettings( custom_thrust_direction, "" ), + thrustDirectionFunction_( thrustDirectionFunction ){ } + + //! Destructor. + ~CustomThrustDirectionSettings( ){ } + + //! Function returning thrust direction unit vector as function fo time. + boost::function< Eigen::Vector3d( const double ) > thrustDirectionFunction_; +}; + +//! Class for defining custom orientation of thrust (i.e. predefined body-fixed-to-propagation rotation as function of time) +/*! + * Class for defining custom orientation of thrust (i.e. predefined body-fixed-to-propagation rotation as function of time). + * Thrust is then computed from body-fixed direction of thrust (defined in ThrustEngineSettings). + */ +class CustomThrustOrientationSettings: public ThrustDirectionGuidanceSettings +{ +public: + + //! Constructor. + /*! + * Constructor + * \param thrustOrientationFunction Custom orientation of thrust (i.e. predefined body-fixed-to-propagation rotation + * as function of time) + */ + CustomThrustOrientationSettings( + const boost::function< Eigen::Quaterniond( const double ) > thrustOrientationFunction ): + ThrustDirectionGuidanceSettings( custom_thrust_orientation, "" ), + thrustOrientationFunction_( thrustOrientationFunction ){ } + + //! Destructor. + ~CustomThrustOrientationSettings( ){ } + + //! Custom orientation of thrust (i.e. predefined body-fixed-to-propagation rotation as function of time. + boost::function< Eigen::Quaterniond( const double ) > thrustOrientationFunction_ ; +}; + +//! Function to create the object determining the direction of the thrust acceleration. +/*! + * Function to create the object determining the direction of the thrust acceleration. + * \param thrustDirectionGuidanceSettings Settings for thrust direction gudiance. + * \param bodyMap List of pointers to body objects defining the full simulation environment. + * \param nameOfBodyWithGuidance Name of body for which thrust guidane is to be created. + * \param bodyFixedThrustOrientation Thrust direction in body-fixed frame. + * \param magnitudeUpdateSettings Settings for the required updates to the environment during propagation. List is + * extended by this function as needed. + * \return Function determining the thrust direction in the propagation frame according to given requirements. + */ +boost::shared_ptr< propulsion::BodyFixedForceDirectionGuidance > createThrustGuidanceModel( + const boost::shared_ptr< ThrustDirectionGuidanceSettings > thrustDirectionGuidanceSettings, + const NamedBodyMap& bodyMap, + const std::string& nameOfBodyWithGuidance, + const boost::function< Eigen::Vector3d( ) > bodyFixedThrustOrientation, + std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > >& magnitudeUpdateSettings ); + +//! List of available types of thust magnitude types +enum ThrustMagnitudeTypes +{ + constant_thrust_magnitude, + from_engine_properties_thrust_magnitude, + thrust_magnitude_from_time_function +}; + +//! Class defining settings for the thrust magnitude +/*! + * Class for providing settings the thrust magnitude of a single thrust model. This class is a functional (base) class for + * settings of thrust magnitude that require no information in addition to their type. + * Classes defining settings for thrust magnitude requiring additional information must be derived from this class. + */ +class ThrustEngineSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param thrustMagnitudeGuidanceType Type of thrust magnitude guidance that is to be used + * \param thrustOriginId Reference id of thrust origin that is to be used (empty if N/A). + */ + ThrustEngineSettings( + const ThrustMagnitudeTypes thrustMagnitudeGuidanceType, + const std::string& thrustOriginId ): + thrustMagnitudeGuidanceType_( thrustMagnitudeGuidanceType ), + thrustOriginId_( thrustOriginId ){ } + + //! Destructor + virtual ~ThrustEngineSettings( ){ } + + //! Type of thrust magnitude guidance that is to be used + ThrustMagnitudeTypes thrustMagnitudeGuidanceType_; + + //! Reference id of thrust origin that is to be used (empty if N/A). + std::string thrustOriginId_; +}; + +//! Class to define settigns for constant thrust settings. +class ConstantThrustEngineSettings: public ThrustEngineSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param thrustMagnitude Constant thrust magnitude that is to be used. + * \param specificImpulse Constant specific impulse that is to be used + * \param bodyFixedThrustDirection Direction of thrust force in body-fixed frame (along longitudinal axis by default). + */ + ConstantThrustEngineSettings( + const double thrustMagnitude, + const double specificImpulse, + const Eigen::Vector3d bodyFixedThrustDirection = Eigen::Vector3d::UnitX( ) ): + ThrustEngineSettings( constant_thrust_magnitude, "" ), + thrustMagnitude_( thrustMagnitude ), specificImpulse_( specificImpulse ), + bodyFixedThrustDirection_( bodyFixedThrustDirection ){ } + + //! Destructor + ~ConstantThrustEngineSettings( ){ } + + //! Constant thrust magnitude that is to be used. + double thrustMagnitude_; + + //! Constant specific impulse that is to be used + double specificImpulse_; + + //! Direction of thrust force in body-fixed frame + Eigen::Vector3d bodyFixedThrustDirection_; +}; + +//! Class to define thrust magnitude to be taken directly from an engine model +class FromBodyThrustEngineSettings: public ThrustEngineSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param useAllEngines Boolean denoting whether all engines of the associated body are to be combined into a + * single thrust magnitude. + * \param thrustOrigin Name of engine model from which thrust is to be derived (must be empty if + * useAllThrustModels is set to true) + */ + FromBodyThrustEngineSettings( + const bool useAllEngines = 1, + const std::string& thrustOrigin = "" ): + ThrustEngineSettings( from_engine_properties_thrust_magnitude, thrustOrigin ), + useAllEngines_( useAllEngines ){ } + + //! Boolean denoting whether all engines of the associated body are to be combined into a single thrust magnitude + bool useAllEngines_; +}; + +//! Class to define custom settings for thrust magnitude/specific impulse. +class FromFunctionThrustEngineSettings: public ThrustEngineSettings +{ +public: + + //! Constructor + /*! + * Constructor + * \param thrustMagnitudeFunction Function returning thrust magnitude as a function of time. + * \param specificImpulseFunction Function returning specific impulse as a function of time. + * \param isEngineOnFunction Function returning boolean denoting whether thrust is to be used (thrust and mass rate + * set to zero if false, regardless of output of thrustMagnitudeFunction). + * \param bodyFixedThrustDirection Direction of thrust force in body-fixed frame (along longitudinal axis by default). + */ + FromFunctionThrustEngineSettings( + const boost::function< double( const double ) > thrustMagnitudeFunction, + const boost::function< double( const double ) > specificImpulseFunction, + const boost::function< bool( const double ) > isEngineOnFunction = boost::lambda::constant( true ), + const Eigen::Vector3d bodyFixedThrustDirection = Eigen::Vector3d::UnitX( ) ): + ThrustEngineSettings( thrust_magnitude_from_time_function, "" ), + thrustMagnitudeFunction_( thrustMagnitudeFunction ), + specificImpulseFunction_( specificImpulseFunction ), + isEngineOnFunction_( isEngineOnFunction ), + bodyFixedThrustDirection_( bodyFixedThrustDirection ){ } + + //! Destructor. + ~FromFunctionThrustEngineSettings( ){ } + + //! Function returning thrust magnitude as a function of time. + boost::function< double( const double) > thrustMagnitudeFunction_; + + //! Function returning specific impulse as a function of time. + boost::function< double( const double) > specificImpulseFunction_; + + //! Function returning boolean denoting whether thrust is to be used. + boost::function< bool( const double ) > isEngineOnFunction_; + + //! Direction of thrust force in body-fixed frame + Eigen::Vector3d bodyFixedThrustDirection_; +}; + +} // namespace simulation_setup + +} // namespace tudat +#endif // TUDAT_THRUSTSETTINGS_H diff --git a/Tudat/SimulationSetup/PropagationSetup/variationalEquationsSolver.cpp b/Tudat/SimulationSetup/PropagationSetup/variationalEquationsSolver.cpp new file mode 100644 index 0000000000..c963a75fff --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/variationalEquationsSolver.cpp @@ -0,0 +1,49 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#include "Tudat/SimulationSetup/PropagationSetup/variationalEquationsSolver.h" + +namespace tudat +{ + +namespace propagators +{ + +//! Function to create interpolators for state transition and sensitivity matrices from numerical results. +void createStateTransitionAndSensitivityMatrixInterpolator( + boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > >& stateTransitionMatrixInterpolator, + boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > >& sensitivityMatrixInterpolator, + std::vector< std::map< double, Eigen::MatrixXd > >& variationalEquationsSolution, + const bool clearRawSolution ) +{ + // Create interpolator for state transition matrix. + stateTransitionMatrixInterpolator= + boost::make_shared< interpolators::LagrangeInterpolator< double, Eigen::MatrixXd > >( + utilities::createVectorFromMapKeys< Eigen::MatrixXd, double >( variationalEquationsSolution[ 0 ] ), + utilities::createVectorFromMapValues< Eigen::MatrixXd, double >( variationalEquationsSolution[ 0 ] ), 4 ); + if( clearRawSolution ) + { + variationalEquationsSolution[ 0 ].clear( ); + } + + // Create interpolator for sensitivity matrix. + sensitivityMatrixInterpolator = + boost::make_shared< interpolators::LagrangeInterpolator< double, Eigen::MatrixXd > >( + utilities::createVectorFromMapKeys< Eigen::MatrixXd, double >( variationalEquationsSolution[ 1 ] ), + utilities::createVectorFromMapValues< Eigen::MatrixXd, double >( variationalEquationsSolution[ 1 ] ), 4 ); + if( clearRawSolution ) + { + variationalEquationsSolution[ 1 ].clear( ); + } +} + +} + +} diff --git a/Tudat/SimulationSetup/PropagationSetup/variationalEquationsSolver.h b/Tudat/SimulationSetup/PropagationSetup/variationalEquationsSolver.h new file mode 100644 index 0000000000..b0084edb90 --- /dev/null +++ b/Tudat/SimulationSetup/PropagationSetup/variationalEquationsSolver.h @@ -0,0 +1,666 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_VARIATIONALEQUATIONSSOLVER_H +#define TUDAT_VARIATIONALEQUATIONSSOLVER_H + +#include +#include +#include +#include + +#include "Tudat/Basics/utilities.h" + +#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" +#include "Tudat/Mathematics/Interpolators/interpolator.h" + +#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/estimatableParameter.h" +#include "Tudat/Astrodynamics/Propagators/stateTransitionMatrixInterface.h" +#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h" +#include "Tudat/Astrodynamics/Ephemerides/tabulatedEphemeris.h" +#include "Tudat/SimulationSetup/EstimationSetup/createStateDerivativePartials.h" + +namespace tudat +{ + +namespace propagators +{ + + +//! Base class to manage and execute the numerical integration of equations of motion and variational equations. +/*! + * Base class to manage and execute the numerical integration of equations of motion and variational equations. + * Governing equations are set once, but can be re-integrated for different initial conditions using the same + * instance of the class. Derived classes define the specific kind of integration that is performed + * (single-arc/multi-arc; dynamics/variational equations, etc.) + */ +template< typename StateScalarType = double, typename TimeType = double, typename ParameterType = double > +class VariationalEquationsSolver +{ +public: + + typedef Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > MatrixType; + typedef Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > VectorType; + + //! Constructor + /*! + * Constructor, sets up object for automatic evaluation and numerical integration of variational equations and + * equations of motion. + * \param bodyMap Map of bodies (with names) of all bodies in integration. + * \param integratorSettings Settings for numerical integrator of combined propagation of variational equations + * and equations of motion. + * \param propagatorSettings Settings for propagation of equations of motion. + * \param parametersToEstimate Object containing all parameters that are to be estimated and their current + * settings and values. + * \param variationalOnlyIntegratorSettings Settings for numerical integrator when integrating only variational + * equations. + * \param clearNumericalSolution Boolean to determine whether to clear the raw numerical solution member variables + * (default true) after propagation and resetting of state transition interface. + */ + VariationalEquationsSolver( + const simulation_setup::NamedBodyMap& bodyMap, + const boost::shared_ptr< numerical_integrators::IntegratorSettings< TimeType > > integratorSettings, + const boost::shared_ptr< PropagatorSettings< StateScalarType > > propagatorSettings, + const boost::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > parametersToEstimate, + const boost::shared_ptr< numerical_integrators::IntegratorSettings< double > > variationalOnlyIntegratorSettings= + boost::shared_ptr< numerical_integrators::IntegratorSettings< double > >( ), + const bool clearNumericalSolution = 1 ): + parametersToEstimate_( parametersToEstimate ), + bodyMap_( bodyMap ), + propagatorSettings_( propagatorSettings ), integratorSettings_( integratorSettings ), + variationalOnlyIntegratorSettings_( variationalOnlyIntegratorSettings ), + stateTransitionMatrixSize_( parametersToEstimate_->getInitialDynamicalStateParameterSize( ) ), + parameterVectorSize_( parametersToEstimate_->getParameterSetSize( ) ), + clearNumericalSolution_( clearNumericalSolution ) + { } + + //! Destructor + virtual ~VariationalEquationsSolver( ){ } + + //! Pure virtual function to integrate variational equations and equations of motion. + /*! + * Pure virtual function to integrate variational equations and equations of motion, to be implemented in derived + * class + * \param initialStateEstimate Initial state of the equations of motion that is to be used. + * \param integrateEquationsConcurrently Variable determining whether the equations of motion are to be + * propagated concurrently with variational equations of motion (if true), or before variational equations (if false). + */ + virtual void integrateVariationalAndDynamicalEquations( + const VectorType& initialStateEstimate, const bool integrateEquationsConcurrently ) = 0; + + //! Pure virtual function to integrate equations of motion only. + /*! + * Pure virtual function to integrate equations of motion only, to be implemented in derived + * class + * \param initialStateEstimate Initial state of the equations of motion that is to be used. + */ + virtual void integrateDynamicalEquationsOfMotionOnly( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& initialStateEstimate ) = 0; + + + //! Function to get the list of objects representing the parameters that are to be integrated. + /*! + * Function to get the list of objects representing the parameters that are to be integrated. + * \return List of objects representing the parameters that are to be integrated. + */ + boost::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > getParametersToEstimate( ) + { + return parametersToEstimate_; + } + + //! Function to reset parameter estimate and re-integrate equations of motion and, if desired, variational equations. + /*! + * Function to reset parameter estimate and re-integrate equations of motion and, if desired, variational equations + * using the new physical parameters/body initial states. + * \param newParameterEstimate New estimate of parameters that are to be estimated, in same order as defined + * in parametersToEstimate_ member. + * \param areVariationalEquationsToBeIntegrated Boolean defining whether the variational equations are to be + * reintegrated with the new parameter values. + */ + void resetParameterEstimate( const Eigen::Matrix< ParameterType, Eigen::Dynamic, 1 > newParameterEstimate, + const bool areVariationalEquationsToBeIntegrated = true ) + + { + // Reset values of parameters. + parametersToEstimate_->template resetParameterValues< ParameterType >( newParameterEstimate ); + propagatorSettings_->resetInitialStates( + estimatable_parameters::getInitialStateVectorOfBodiesToEstimate( parametersToEstimate_ ) ); + + dynamicsStateDerivative_->template updateStateDerivativeModelSettings( + propagatorSettings_->getInitialStates( ), 0 ); + + // Check if re-integration of variational equations is requested + if( areVariationalEquationsToBeIntegrated ) + { + // Integrate variational and state equations. + this->integrateVariationalAndDynamicalEquations( propagatorSettings_->getInitialStates( ), 1 ); + } + else + { + this->integrateDynamicalEquationsOfMotionOnly( propagatorSettings_->getInitialStates( ) ); + } + } + + //! Function to get the state transition matric interface object. + /*! + * Function to get the state transition matric interface object. + * \return The state transition matric interface object. + */ + boost::shared_ptr< CombinedStateTransitionAndSensitivityMatrixInterface > getStateTransitionMatrixInterface( ) + { + return stateTransitionInterface_; + } + + +protected: + + + //! Create initial matrix of numerical soluation to variational + dynamical equations. + /*! + * Create initial matrix of numerical soluation to variational + dynamical equations. The structure of the matrix is + * [Phi;S;y], with Phi the state transition matrix, S the sensitivity matrix y the state vector. + * \param initialStateEstimate vector of initial state (position/velocity) of bodies to be integrated numerically. + * order determined by order of bodiesToIntegrate_. + * \return Initial matrix of numerical soluation to variation + state equations. + */ + MatrixType createInitialConditions( const VectorType initialStateEstimate ) + { + // Initialize initial conditions to zeros. + MatrixType varSystemInitialState = MatrixType( stateTransitionMatrixSize_, + parameterVectorSize_ + 1 ).setZero( ); + + // Set initial state transition matrix to identity + varSystemInitialState.block( 0, 0, stateTransitionMatrixSize_, stateTransitionMatrixSize_ ).setIdentity( ); + + // Set initial body states to current estimate of initial body states. + varSystemInitialState.block( 0, parameterVectorSize_, + stateTransitionMatrixSize_, 1 ) = initialStateEstimate; + + return varSystemInitialState; + } + + //! Create initial matrix of numerical soluation to variational equations + /*! + * Create initial matrix of numerical soluation to variational equations, with structure [Phi;S]. Initial state + * transition matrix Phi is identity matrix. Initial sensitivity matrix S is all zeros. + * \return Initial matrix solution to variational equations. + */ + Eigen::MatrixXd createInitialVariationalEquationsSolution( ) + { + // Initialize initial conditions to zeros. + Eigen::MatrixXd varSystemInitialState = Eigen::MatrixXd::Zero( + stateTransitionMatrixSize_, parameterVectorSize_ ); + + // Set initial state transition matrix to identity + varSystemInitialState.block( 0, 0, stateTransitionMatrixSize_, stateTransitionMatrixSize_ ).setIdentity( ); + + return varSystemInitialState; + } + + //! Object containing all parameters that are to be estimated and their current settings and values. + boost::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > parametersToEstimate_ ; + + //! Map of bodies (with names) of all bodies in integration. + simulation_setup::NamedBodyMap bodyMap_; + + //! Settings for propagation of equations of motion. + boost::shared_ptr< PropagatorSettings< StateScalarType > > propagatorSettings_; + + //! Settings for numerical integrator of combined propagation of variational equations and equations of motion. + boost::shared_ptr< numerical_integrators::IntegratorSettings< TimeType > > integratorSettings_; + + //! Settings for numerical integrator when integrating only variational equations. + boost::shared_ptr< numerical_integrators::IntegratorSettings< double > > variationalOnlyIntegratorSettings_; + + //! Size (rows and columns are equal) of state transition matrix. + int stateTransitionMatrixSize_; + + //! Number of rows in sensitivity matrix + int parameterVectorSize_; + + //! Boolean to determine whether to clear the raw numerical solution member variables after propagation + /*! + * Boolean to determine whether to clear the raw numerical solution member variables after propagation + * and resetting of state transition interface. + */ + bool clearNumericalSolution_; + + //! Object used for interpolating numerical results of state transition and sensitivity matrix. + boost::shared_ptr< CombinedStateTransitionAndSensitivityMatrixInterface > stateTransitionInterface_; + + //! Object used to compute the full state derivative in equations of motion and variational equations. + /*! + * Object used to compute the full state derivative in equations of motion and variational equations, + * including relevant updates of environment from current state and time. Object may be used for + * either full or separate propagation of equations. + */ + boost::shared_ptr< DynamicsStateDerivativeModel< TimeType, StateScalarType > > dynamicsStateDerivative_; + +}; + +//! Function to separate the time histories of the sensitivity and state transition matrices from a full numerical solution. +/*! + * Function to separate the time histories of the sensitivity and state transition matrices from a full numerical solution, + * in which the solution is represented as a single matrix block per time value. + * NOTE: numericalIntegrationResult contents are deleted by this function (all information is conserved in + * variationalEquationsSolution. + * \param numericalIntegrationResult Full time history from which separate matrix histories are to be retrieved. + * \param variationalEquationsSolution Vector of two matrix histories (returned by reference). First vector entry + * is state transition matrix history, second entry is sensitivity matrix history. + * \param stateTransitionStartIndices First row and column (first and second) of state transition matrix in entries of + * numericalIntegrationResult. + * \param sensitivityStartIndices First row and column (first and second) of sensitivity matrix in entries of + * numericalIntegrationResult. + * \param stateTransitionMatrixSize Size (rows and columns are equal) of state transition matrix. + * \param parameterSetSize Number of rows in sensitivity matrix + */ +template< typename TimeType, typename StateScalarType > +void setVariationalEquationsSolution( + std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > >& + numericalIntegrationResult, + std::vector< std::map< double, Eigen::MatrixXd > >& variationalEquationsSolution, + const std::pair< int, int > stateTransitionStartIndices, + const std::pair< int, int > sensitivityStartIndices, + const int stateTransitionMatrixSize, + const int parameterSetSize ) +{ + variationalEquationsSolution.clear( ); + variationalEquationsSolution.resize( 2 ); + + for( typename std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > >::iterator + integrationIterator = numericalIntegrationResult.begin( ); + integrationIterator != numericalIntegrationResult.end( ); ) + { + // Set result for state transition matrix in each time step. + variationalEquationsSolution[ 0 ][ integrationIterator->first ] = + ( integrationIterator->second.block( stateTransitionStartIndices.first, stateTransitionStartIndices.second, + stateTransitionMatrixSize, + stateTransitionMatrixSize ) ).template cast< double >( ); + + // Set result for sensitivity matrix in each time step. + variationalEquationsSolution[ 1 ][ integrationIterator->first ] = + ( integrationIterator->second.block( sensitivityStartIndices.first, sensitivityStartIndices.second, + stateTransitionMatrixSize, + parameterSetSize - + stateTransitionMatrixSize ) ).template cast< double >( ); + numericalIntegrationResult.erase( integrationIterator++ ); + } +} + +//! Function to create interpolators for state transition and sensitivity matrices from numerical results. +/*! + * Function to create interpolators for state transition and sensitivity matrices from numerical results. + * \param stateTransitionMatrixInterpolator Interpolator object for state transition matrix (returned by reference). + * \param sensitivityMatrixInterpolator Interpolator object for sensitivity matrix (returned by reference). + * \param variationalEquationsSolution Vector of two matrix histories. First vector entry + * is state transition matrix history, second entry is sensitivity matrix history. + * \param clearRawSolution Boolean denoting whether to clear entries of variationalEquationsSolution after creation + * of interpolators. + */ +void createStateTransitionAndSensitivityMatrixInterpolator( + boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > >& + stateTransitionMatrixInterpolator, + boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > >& + sensitivityMatrixInterpolator, + std::vector< std::map< double, Eigen::MatrixXd > >& variationalEquationsSolution, + const bool clearRawSolution = 1 ); + +//! Function to check the consistency between propagation settings of equations of motion, and estimated parameters. +/*! + * Function to check the consistency between propagation settings of equations of motion, and estimated parameters. + * In particular, it is presently required that the set of propagated states is equal to the set of estimated states. + * \param propagatorSettings Settings for propagation of equations of motion. + * \param parametersToEstimate Object containing all parameters that are to be estimated and their current + * settings and values. + */ +template< typename StateScalarType = double, typename TimeType = double, typename ParameterType = double > +bool checkPropagatorSettingsAndParameterEstimationConsistency( + const boost::shared_ptr< PropagatorSettings< StateScalarType > > propagatorSettings, + const boost::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > parametersToEstimate ) +{ + bool isInputConsistent = 1; + + // Check type of dynamics + switch( propagatorSettings->stateType_ ) + { + case transational_state: + { + boost::shared_ptr< TranslationalStatePropagatorSettings< StateScalarType > > translationalPropagatorSettings = + boost::dynamic_pointer_cast< TranslationalStatePropagatorSettings< StateScalarType > >( propagatorSettings ); + + // Retrieve estimated and propagated translational states, and check equality. + std::vector< std::string > propagatedBodies = translationalPropagatorSettings->bodiesToIntegrate_; + std::vector< std::string > estimatedBodies = estimatable_parameters::getListOfBodiesWithTranslationalStateToEstimate( + parametersToEstimate ); + if( propagatedBodies.size( ) != estimatedBodies.size( ) ) + { + std::string errorMessage = "Error, propagated and estimated body vector sizes are inconsistent " + + boost::lexical_cast< std::string >( propagatedBodies.size( ) ) + " " + + boost::lexical_cast< std::string >( estimatedBodies.size( ) ); + throw std::runtime_error( errorMessage ); + isInputConsistent = 0; + } + else + { + for( unsigned int i = 0; i < propagatedBodies.size( ); i++ ) + { + if( propagatedBodies.at( i ) != estimatedBodies.at( i ) ) + { + std::string errorMessage = "Error, propagated and estimated body vectors inconsistent at index" + + boost::lexical_cast< std::string >( propagatedBodies.at( i ) ) + " " + + boost::lexical_cast< std::string >( estimatedBodies.at( i ) ); + throw std::runtime_error( errorMessage ); + isInputConsistent = 0; + } + } + + } + break; + } + default: + std::string errorMessage = "Error, cannot yet check consistency of propagator settings for type " + + boost::lexical_cast< std::string >( propagatorSettings->stateType_ ); + throw std::runtime_error( errorMessage ); + } + return isInputConsistent; +} + +//! Class to manage and execute the numerical integration of variational equations of a dynamical system in a single arc. +/*! + * Class to manage and execute the numerical integration of variational equations of a dynamical system, in addition + * to the dynamics itself, in a single arc: i.e. the governing equations a single initial time, and are propagated once + * for the full prescribed time interval. This is in contrast to multi-arc dynamics, where the time interval is cut into + * pieces. In this class, the governing equations are set once, but can be re-integrated for + * different initial conditions using the same instance of the class. + */ +template< typename StateScalarType = double, typename TimeType = double, typename ParameterType = double > +class SingleArcVariationalEquationsSolver: public VariationalEquationsSolver< StateScalarType, TimeType, ParameterType > +{ +public: + + //! Local typedefs for vector and matrix of given scalar type + typedef Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > MatrixType; + typedef Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > VectorType; + + //! Base class using statements + using VariationalEquationsSolver< StateScalarType, TimeType, ParameterType >::parametersToEstimate_; + using VariationalEquationsSolver< StateScalarType, TimeType, ParameterType >::bodyMap_; + using VariationalEquationsSolver< StateScalarType, TimeType, ParameterType >::dynamicsStateDerivative_; + using VariationalEquationsSolver< StateScalarType, TimeType, ParameterType >::propagatorSettings_; + using VariationalEquationsSolver< StateScalarType, TimeType, ParameterType >::integratorSettings_; + using VariationalEquationsSolver< StateScalarType, TimeType, ParameterType >::stateTransitionMatrixSize_; + using VariationalEquationsSolver< StateScalarType, TimeType, ParameterType >::parameterVectorSize_; + using VariationalEquationsSolver< StateScalarType, TimeType, ParameterType >::variationalOnlyIntegratorSettings_; + using VariationalEquationsSolver< StateScalarType, TimeType, ParameterType >::stateTransitionInterface_; + + //! Constructor + /*! + * Constructor, sets up object for automatic evaluation and numerical integration of variational equations and + * equations of motion. + * \param bodyMap Map of bodies (with names) of all bodies in integration. + * \param integratorSettings Settings for numerical integrator of combined propagation of variational equations + * and equations of motion. + * \param propagatorSettings Settings for propagation of equations of motion. + * \param parametersToEstimate Object containing all parameters that are to be estimated and their current + * settings and values. + * \param integrateDynamicalAndVariationalEquationsConcurrently Boolean defining whether variational and dynamical + * equations are to be propagated concurrently (if true) or sequentially (of false) + * \param variationalOnlyIntegratorSettings Settings for numerical integrator when integrating only variational + * equations. + * \param clearNumericalSolution Boolean to determine whether to clear the raw numerical solution member variables + * (default true) after propagation and resetting of state transition interface. + * \param integrateEquationsOnCreation Boolean to denote whether equations should be integrated immediately at the + * end of this contructor. + */ + SingleArcVariationalEquationsSolver( + const simulation_setup::NamedBodyMap& bodyMap, + const boost::shared_ptr< numerical_integrators::IntegratorSettings< TimeType > > integratorSettings, + const boost::shared_ptr< PropagatorSettings< StateScalarType > > propagatorSettings, + const boost::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > parametersToEstimate, + const bool integrateDynamicalAndVariationalEquationsConcurrently = 1, + const boost::shared_ptr< numerical_integrators::IntegratorSettings< double > > variationalOnlyIntegratorSettings + = boost::shared_ptr< numerical_integrators::IntegratorSettings< double > >( ), + const bool clearNumericalSolution = 1, + const bool integrateEquationsOnCreation = 1 ): + VariationalEquationsSolver< StateScalarType, TimeType, ParameterType >( + bodyMap, integratorSettings, propagatorSettings, parametersToEstimate, + variationalOnlyIntegratorSettings, clearNumericalSolution ) + { + // Check input consistency + if( !checkPropagatorSettingsAndParameterEstimationConsistency< StateScalarType, TimeType, ParameterType >( + propagatorSettings, parametersToEstimate ) ) + { + throw std::runtime_error( + "Error when making single arc variational equations solver, estimated and propagated bodies are inconsistent" ); + } + else + { + // Create simulation object for dynamics only. + dynamicsSimulator_ = boost::make_shared< SingleArcDynamicsSimulator< StateScalarType, TimeType > >( + bodyMap, integratorSettings, propagatorSettings, false, clearNumericalSolution ); + dynamicsStateDerivative_ = dynamicsSimulator_->getDynamicsStateDerivative( ); + + // Create state derivative partials + std::map< IntegratedStateType, orbit_determination::StateDerivativePartialsMap > + stateDerivativePartials = + simulation_setup::createStateDerivativePartials + < StateScalarType, TimeType, ParameterType >( + dynamicsStateDerivative_->getStateDerivativeModels( ), bodyMap, parametersToEstimate ); + + // Create variational equations objects. + variationalEquationsObject_ = boost::make_shared< VariationalEquations >( + stateDerivativePartials, parametersToEstimate_, + dynamicsStateDerivative_->getStateTypeStartIndices( ) ); + dynamicsStateDerivative_->addVariationalEquations( variationalEquationsObject_ ); + + // Resize solution of variational equations to 2 (state transition and sensitivity matrices) + variationalEquationsSolution_.resize( 2 ); + + // Integrate variational equations from initial state estimate. + if( integrateEquationsOnCreation ) + { + if( integrateDynamicalAndVariationalEquationsConcurrently ) + { + integrateVariationalAndDynamicalEquations( propagatorSettings->getInitialStates( ), 1 ); + } + else + { + integrateVariationalAndDynamicalEquations( propagatorSettings->getInitialStates( ), 0 ); + } + } + } + } + + //! Destructor + ~SingleArcVariationalEquationsSolver( ){ } + + //! Function to integrate equations of motion only. + /*! + * Function to integrate equations of motion only (in single arc). If dynamical + * solution is to be processed, the environment is also updtaed to teh new solution. + * \param initialStateEstimate Initial state of the equations of motion that is to be used (in same order as in + * parametersToEstimate_) + */ + void integrateDynamicalEquationsOfMotionOnly( + const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& initialStateEstimate ) + { + dynamicsStateDerivative_->setPropagationSettings( std::vector< IntegratedStateType >( ), 1, 0 ); + dynamicsSimulator_->integrateEquationsOfMotion( initialStateEstimate ); + } + + //! Function to integrate variational equations and equations of motion. + /*! + * Function to integrate variational equations and equations of motion (in single arc). At the end of this function, + * the stateTransitionInterface_ is reset with the new state transition and sensitivity matrices. If dynamical + * solution is to be processed, the environment is also updtaed to the new solution. + * \param initialStateEstimate Initial state of the equations of motion that is to be used (in same order as in + * parametersToEstimate_). + * \param integrateEquationsConcurrently Variable determining whether the equations of motion are to be + * propagated concurrently with variational equations of motion (if true), or before variational equations (if false). + */ + void integrateVariationalAndDynamicalEquations( + const VectorType& initialStateEstimate, const bool integrateEquationsConcurrently ) + { + variationalEquationsSolution_[ 0 ].clear( ); + variationalEquationsSolution_[ 1 ].clear( ); + + + if( integrateEquationsConcurrently ) + { + + // Create initial conditions from new estimate. + MatrixType initialVariationalState = this->createInitialConditions( + dynamicsStateDerivative_->convertFromOutputSolution( + initialStateEstimate, integratorSettings_->initialTime_ ) ); + + + // Integrate variational and state equations. + dynamicsStateDerivative_->setPropagationSettings( std::vector< IntegratedStateType >( ), 1, 1 ); + std::map< TimeType, Eigen::VectorXd > dependentVariableHistory; + std::map< TimeType, MatrixType > rawNumericalSolution; + integrateEquations< MatrixType, TimeType >( + dynamicsSimulator_->getStateDerivativeFunction( ), rawNumericalSolution, + initialVariationalState, integratorSettings_, + boost::bind( &PropagationTerminationCondition::checkStopCondition, + dynamicsSimulator_->getPropagationTerminationCondition( ), _1 ), + dependentVariableHistory ); + + std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > equationsOfMotionNumericalSolution; + utilities::createVectorBlockMatrixHistory( + rawNumericalSolution, equationsOfMotionNumericalSolution, + std::make_pair( 0, parameterVectorSize_ ), stateTransitionMatrixSize_ ); + + equationsOfMotionNumericalSolution = convertNumericalStateSolutionsToOutputSolutions( + equationsOfMotionNumericalSolution, dynamicsStateDerivative_ ); + dynamicsSimulator_->manuallySetAndProcessRawNumericalEquationsOfMotionSolution( + equationsOfMotionNumericalSolution ); + + // Reset solution for state transition and sensitivity matrices. + setVariationalEquationsSolution< TimeType, StateScalarType >( + rawNumericalSolution, variationalEquationsSolution_, + std::make_pair( 0, 0 ), std::make_pair( 0, stateTransitionMatrixSize_ ), + stateTransitionMatrixSize_, parameterVectorSize_ ); + } + else + { + + dynamicsStateDerivative_->setPropagationSettings( std::vector< IntegratedStateType >( ), 1, 0 ); + dynamicsSimulator_->integrateEquationsOfMotion( initialStateEstimate ); + + // Integrate variational equations. + dynamicsStateDerivative_->setPropagationSettings( boost::assign::list_of( transational_state ), 0, 1 ); + Eigen::MatrixXd initialVariationalState = this->createInitialVariationalEquationsSolution( ); + std::map< double, Eigen::MatrixXd > rawNumericalSolution; + std::map< TimeType, Eigen::VectorXd > dependentVariableHistory; + + integrateEquations< Eigen::MatrixXd, double >( + dynamicsSimulator_->getDoubleStateDerivativeFunction( ), rawNumericalSolution, initialVariationalState, + variationalOnlyIntegratorSettings_, + boost::bind( &PropagationTerminationCondition::checkStopCondition, + dynamicsSimulator_->getPropagationTerminationCondition( ), _1 ), + dependentVariableHistory ); + + setVariationalEquationsSolution< double, double >( + rawNumericalSolution, variationalEquationsSolution_, std::make_pair( 0, 0 ), + std::make_pair( 0, stateTransitionMatrixSize_ ), + stateTransitionMatrixSize_, parameterVectorSize_ ); + + } + + // Reset solution for state transition and sensitivity matrices. + resetVariationalEquationsInterpolators( ); + + } + + //! Function to return the numerical solution history of numerically integrated variational equations. + /*! + * Function to return the numerical solution history of numerically integrated variational equations. + * \return Vector of mapa of state transition matrix history (first vector entry) + * and sensitivity matrix history (second vector entry) + */ + std::vector< std::map< double, Eigen::MatrixXd > >& getNumericalVariationalEquationsSolution( ) + { + return variationalEquationsSolution_; + } + + //! Function to return object used for numerically propagating and managing the solution of the equations of motion. + /*! + * Function to return object used for numerically propagating and managing the solution of the equations of motion. + * \return Object used for numerically propagating and managing the solution of the equations of motion. + */ + boost::shared_ptr< SingleArcDynamicsSimulator< StateScalarType, TimeType > > getDynamicsSimulator( ) + { + return dynamicsSimulator_; + } + +protected: + +private: + + + //! Reset solutions of variational equations. + /*! + * Reset solutions of variational equations (stateTransitionMatrixInterpolator_ and sensitivityMatrixInterpolator_), + * i.e. use numerical integration results to create new look-up tables + * and interpolators of state transition and sensitivity matrix through the createInterpolatorsForVariationalSolution + * function + */ + void resetVariationalEquationsInterpolators( ) + { + using namespace interpolators; + using namespace utilities; + + // Create interpolators. + boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > + stateTransitionMatrixInterpolator; + boost::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > + sensitivityMatrixInterpolator; + createStateTransitionAndSensitivityMatrixInterpolator( + stateTransitionMatrixInterpolator, sensitivityMatrixInterpolator, variationalEquationsSolution_, + this->clearNumericalSolution_ ); + + // Create (if non-existent) or reset state transition matrix interface + if( stateTransitionInterface_ == NULL ) + { + stateTransitionInterface_ = boost::make_shared< SingleArcCombinedStateTransitionAndSensitivityMatrixInterface >( + stateTransitionMatrixInterpolator, sensitivityMatrixInterpolator, + propagatorSettings_->getStateSize( ), parameterVectorSize_ ); + } + else + { + boost::dynamic_pointer_cast< SingleArcCombinedStateTransitionAndSensitivityMatrixInterface >( + stateTransitionInterface_ )->updateMatrixInterpolators( + stateTransitionMatrixInterpolator, sensitivityMatrixInterpolator ); + } + } + + //! Object used for numerically propagating and managing the solution of the equations of motion. + boost::shared_ptr< SingleArcDynamicsSimulator< StateScalarType, TimeType > > dynamicsSimulator_; + + //! Object that is used to evaluate the variational equations at the given state and time. + boost::shared_ptr< VariationalEquations > variationalEquationsObject_; + + //! Map of history of numerically integrated variational equations. + /*! + * Map of history of numerically integrated variational equations. Key of map denotes time, values are + * state transition matrix Phi (first vector entry) and sensitivity matrix S (second vector entry) + */ + std::vector< std::map< double, Eigen::MatrixXd > > variationalEquationsSolution_; + +}; + +} // namespace propagators + +} // namespace tudat + + + + +#endif // TUDAT_VARIATIONALEQUATIONSSOLVER_H diff --git a/Tudat/SimulationSetup/UnitTests/unitTestAccelerationModelSetup.cpp b/Tudat/SimulationSetup/UnitTests/unitTestAccelerationModelSetup.cpp index 0856100742..927fd2ad8e 100644 --- a/Tudat/SimulationSetup/UnitTests/unitTestAccelerationModelSetup.cpp +++ b/Tudat/SimulationSetup/UnitTests/unitTestAccelerationModelSetup.cpp @@ -29,9 +29,9 @@ #include "Tudat/InputOutput/basicInputOutput.h" #include "Tudat/Mathematics/Interpolators/linearInterpolator.h" -#include "Tudat/SimulationSetup/createAccelerationModels.h" -#include "Tudat/SimulationSetup/createBodies.h" -#include "Tudat/SimulationSetup/defaultBodies.h" +#include "Tudat/SimulationSetup/PropagationSetup/createNumericalSimulator.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h" #include "Tudat/Astrodynamics/Aerodynamics/UnitTests/testApolloCapsuleCoefficients.h" namespace tudat @@ -250,7 +250,7 @@ BOOST_AUTO_TEST_CASE( test_shGravityModelSetup ) // Manually create acceleration model. boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > manualAcceleration = - boost::make_shared< gravitation::SphericalHarmonicsGravitationalAccelerationModel< > >( + boost::make_shared< gravitation::SphericalHarmonicsGravitationalAccelerationModel >( boost::bind( &Body::getPosition, bodyMap[ "Vehicle" ] ), gravitationalParameter, planetaryRadius, cosineCoefficients, sineCoefficients, @@ -274,7 +274,7 @@ BOOST_AUTO_TEST_CASE( test_shGravityModelSetup ) // Manually create acceleration. manualAcceleration = - boost::make_shared< gravitation::SphericalHarmonicsGravitationalAccelerationModel< > >( + boost::make_shared< gravitation::SphericalHarmonicsGravitationalAccelerationModel >( boost::bind( &Body::getPosition, bodyMap[ "Vehicle" ] ), gravitationalParameter * 1.1, planetaryRadius, cosineCoefficients, sineCoefficients, @@ -657,6 +657,7 @@ BOOST_AUTO_TEST_CASE( test_aerodynamicAccelerationModelSetupWithCoefficientIndep boost::lambda::constant( bankAngle ) ); // Update flight conditions + vehicleFlightConditions->resetCurrentTime( TUDAT_NAN ); vehicleFlightConditions->updateConditions( testTime ); // Calculate Mach number diff --git a/Tudat/SimulationSetup/UnitTests/unitTestEnvironmentModelSetup.cpp b/Tudat/SimulationSetup/UnitTests/unitTestEnvironmentModelSetup.cpp index d1fe05c148..d52ccb66b5 100644 --- a/Tudat/SimulationSetup/UnitTests/unitTestEnvironmentModelSetup.cpp +++ b/Tudat/SimulationSetup/UnitTests/unitTestEnvironmentModelSetup.cpp @@ -18,6 +18,7 @@ #include #include "Tudat/Astrodynamics/Aerodynamics/exponentialAtmosphere.h" + #if USE_NRLMSISE00 #include "Tudat/Astrodynamics/Aerodynamics/nrlmsise00Atmosphere.h" #include "Tudat/Astrodynamics/Aerodynamics/nrlmsise00InputFunctions.h" @@ -44,14 +45,15 @@ #include "Tudat/InputOutput/parseSolarActivityData.h" #include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h" #include "Tudat/Mathematics/Interpolators/lagrangeInterpolator.h" -#include "Tudat/SimulationSetup/createAtmosphereModel.h" -#include "Tudat/SimulationSetup/createEphemeris.h" -#include "Tudat/SimulationSetup/createGravityField.h" -#include "Tudat/SimulationSetup/createRotationModel.h" -#include "Tudat/SimulationSetup/defaultBodies.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createAtmosphereModel.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createEphemeris.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createGravityField.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createRotationModel.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h" namespace tudat { + namespace unit_tests { diff --git a/Tudat/SimulationSetup/accelerationSettings.h b/Tudat/SimulationSetup/accelerationSettings.h deleted file mode 100644 index ba9d159afc..0000000000 --- a/Tudat/SimulationSetup/accelerationSettings.h +++ /dev/null @@ -1,95 +0,0 @@ -/* Copyright (c) 2010-2016, Delft University of Technology - * All rigths reserved - * - * This file is part of the Tudat. Redistribution and use in source and - * binary forms, with or without modification, are permitted exclusively - * under the terms of the Modified BSD license. You should have received - * a copy of the license with this file. If not, please or visit: - * http://tudat.tudelft.nl/LICENSE. - */ - -#ifndef TUDAT_ACCELERATIONSETTINGS_H -#define TUDAT_ACCELERATIONSETTINGS_H - -#include "Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.h" -#include "Tudat/Astrodynamics/Gravitation/centralGravityModel.h" -#include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityModel.h" -#include "Tudat/Astrodynamics/Gravitation/thirdBodyPerturbation.h" -#include "Tudat/Astrodynamics/Aerodynamics/aerodynamicAcceleration.h" -#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModelTypes.h" - - - -namespace tudat -{ - -namespace simulation_setup -{ - -//! Class for providing settings for acceleration model. -/*! - * Class for providing settings for acceleration model. This class is a functional (base) class for - * settings of acceleration models that require no information in addition to their type. - * Classes defining settings for acceleration models requiring additional information must be - * derived from this class. - * Bodies exerting and undergong acceleration are set externally from this class. - * This class can be used for the easy setup of acceleration models - * (see createAccelerationModels.h), but users may also chose to do so manually. - * (Derived) Class members are all public, for ease of access and modification. - */ -class AccelerationSettings -{ -public: - //! Constructor, sets type of acceleration. - /*! - * Constructor, sets type of acceleration. - * \param accelerationType Type of acceleration from AvailableAcceleration enum. - */ - AccelerationSettings( const basic_astrodynamics::AvailableAcceleration accelerationType ): - accelerationType_( accelerationType ){ } - - //! Destructor. - virtual ~AccelerationSettings( ){ } - - //! Type of acceleration from AvailableAcceleration enum. - basic_astrodynamics::AvailableAcceleration accelerationType_; - -}; - -//! Class for providing settings for spherical harmonics acceleration model. -/*! - * Class for providing settings for spherical harmonics acceleration model, - * specifically the maximum degree and order up to which the field is to be expanded. Note that - * the minimum degree and order are currently always set to zero. - */ -class SphericalHarmonicAccelerationSettings: public AccelerationSettings -{ -public: - //! Constructor to set maximum degree and order that is to be taken into account. - /*! - * Constructor to set maximum degree and order that is to be taken into account. - * \param maximumDegree Maximum degree - * \param maximumOrder Maximum order - */ - SphericalHarmonicAccelerationSettings( const int maximumDegree, - const int maximumOrder ): - AccelerationSettings( basic_astrodynamics::spherical_harmonic_gravity ), - maximumDegree_( maximumDegree ), maximumOrder_( maximumOrder ){ } - - //! Maximum degree that is to be used for spherical harmonic acceleration - int maximumDegree_; - - //! Maximum order that is to be used for spherical harmonic acceleration - int maximumOrder_; -}; - -//! Typedef defining a list of acceleration settings, set up in the same manner as the -//! AccelerationMap typedef. -typedef std::map< std::string, std::map< std::string, std::vector< boost::shared_ptr< -AccelerationSettings > > > > SelectedAccelerationMap; - -} // namespace simulation_setup - -} // namespace tudat - -#endif // TUDAT_ACCELERATIONSETTINGS_H diff --git a/Tudat/SimulationSetup/createAccelerationModels.cpp b/Tudat/SimulationSetup/createAccelerationModels.cpp deleted file mode 100644 index 62dddd67c7..0000000000 --- a/Tudat/SimulationSetup/createAccelerationModels.cpp +++ /dev/null @@ -1,579 +0,0 @@ -/* Copyright (c) 2010-2016, Delft University of Technology - * All rigths reserved - * - * This file is part of the Tudat. Redistribution and use in source and - * binary forms, with or without modification, are permitted exclusively - * under the terms of the Modified BSD license. You should have received - * a copy of the license with this file. If not, please or visit: - * http://tudat.tudelft.nl/LICENSE. - */ - - -#include -#include -#include - -#include "Tudat/Astrodynamics/Aerodynamics/flightConditions.h" -#include "Tudat/Astrodynamics/Ephemerides/frameManager.h" -#include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.h" -#include "Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.h" -#include "Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h" -#include "Tudat/SimulationSetup/accelerationSettings.h" -#include "Tudat/SimulationSetup/createAccelerationModels.h" -#include "Tudat/SimulationSetup/createFlightConditions.h" - -namespace tudat -{ - -namespace simulation_setup -{ - -using namespace aerodynamics; -using namespace gravitation; -using namespace basic_astrodynamics; -using namespace electro_magnetism; -using namespace ephemerides; - -//! Function to add to double-returning functions. -double evaluateDoubleFunctions( - const boost::function< double( ) >& function1, - const boost::function< double( ) >& function2 ) -{ - return function1( ) + function2( ); -} - - -//! Function to create central gravity acceleration model. -boost::shared_ptr< CentralGravitationalAccelerationModel3d > createCentralGravityAcceleratioModel( - const boost::shared_ptr< Body > bodyUndergoingAcceleration, - const boost::shared_ptr< Body > bodyExertingAcceleration, - const std::string& nameOfBodyUndergoingAcceleration, - const std::string& nameOfBodyExertingAcceleration, - const bool useCentralBodyFixedFrame ) -{ - // Declare pointer to return object. - boost::shared_ptr< CentralGravitationalAccelerationModel3d > accelerationModelPointer; - - // Check if body is endowed with a gravity field model (i.e. is capable of exerting - // gravitation acceleration). - if( bodyExertingAcceleration->getGravityFieldModel( ) == NULL ) - { - throw std::runtime_error( - std::string( "Error, gravity field model not set when making central ") + - " gravitational acceleration of " + nameOfBodyExertingAcceleration + " on " + - nameOfBodyUndergoingAcceleration ); - } - else - { - boost::function< double( ) > gravitationalParameterFunction; - - // Set correct value for gravitational parameter. - if( useCentralBodyFixedFrame == 0 || - bodyUndergoingAcceleration->getGravityFieldModel( ) == NULL ) - { - gravitationalParameterFunction = - boost::bind( &gravitation::GravityFieldModel::getGravitationalParameter, - bodyExertingAcceleration->getGravityFieldModel( ) ); - } - else - { - boost::function< double( ) > gravitationalParameterOfBodyExertingAcceleration = - boost::bind( &gravitation::GravityFieldModel::getGravitationalParameter, - bodyExertingAcceleration->getGravityFieldModel( ) ); - boost::function< double( ) > gravitationalParameterOfBodyUndergoingAcceleration = - boost::bind( &gravitation::GravityFieldModel::getGravitationalParameter, - bodyUndergoingAcceleration->getGravityFieldModel( ) ); - gravitationalParameterFunction = - boost::bind( &evaluateDoubleFunctions, - gravitationalParameterOfBodyExertingAcceleration, - gravitationalParameterOfBodyUndergoingAcceleration ); - } - - // Create acceleration object. - accelerationModelPointer = - boost::make_shared< CentralGravitationalAccelerationModel3d >( - boost::bind( &Body::getPosition, bodyUndergoingAcceleration ), - gravitationalParameterFunction, - boost::bind( &Body::getPosition, bodyExertingAcceleration ) ); - } - - - return accelerationModelPointer; -} - -//! Function to create spherical harmonic gravity acceleration model. -boost::shared_ptr< gravitation::SphericalHarmonicsGravitationalAccelerationModelXd > -createSphericalHarmonicsGravityAcceleration( - const boost::shared_ptr< Body > bodyUndergoingAcceleration, - const boost::shared_ptr< Body > bodyExertingAcceleration, - const std::string& nameOfBodyUndergoingAcceleration, - const std::string& nameOfBodyExertingAcceleration, - const boost::shared_ptr< AccelerationSettings > accelerationSettings, - const bool useCentralBodyFixedFrame ) -{ - // Declare pointer to return object - boost::shared_ptr< SphericalHarmonicsGravitationalAccelerationModelXd > accelerationModel; - - // Dynamic cast acceleration settings to required type and check consistency. - boost::shared_ptr< SphericalHarmonicAccelerationSettings > sphericalHarmonicsSettings = - boost::dynamic_pointer_cast< SphericalHarmonicAccelerationSettings >( - accelerationSettings ); - if( sphericalHarmonicsSettings == NULL ) - { - throw std::runtime_error( - std::string( "Error, acceleration settings inconsistent ") + - " making sh gravitational acceleration of " + nameOfBodyExertingAcceleration + - " on " + nameOfBodyUndergoingAcceleration ); - } - else - { - // Get pointer to gravity field of central body and cast to required type. - boost::shared_ptr< SphericalHarmonicsGravityField > sphericalHarmonicsGravityField = - boost::dynamic_pointer_cast< SphericalHarmonicsGravityField >( - bodyExertingAcceleration->getGravityFieldModel( ) ); - - boost::shared_ptr< RotationalEphemeris> rotationalEphemeris = - bodyExertingAcceleration->getRotationalEphemeris( ); - if( sphericalHarmonicsGravityField == NULL ) - { - throw std::runtime_error( - std::string( "Error, spherical harmonic gravity field model not set when ") - + " making sh gravitational acceleration of " + - nameOfBodyExertingAcceleration + - " on " + nameOfBodyUndergoingAcceleration ); - } - else - { - if( rotationalEphemeris == NULL ) - { - throw std::runtime_error( "Warning when making spherical harmonic acceleration on body " + - nameOfBodyUndergoingAcceleration + ", no rotation model found for " + - nameOfBodyExertingAcceleration ); - } - - if( rotationalEphemeris->getTargetFrameOrientation( ) != - sphericalHarmonicsGravityField->getFixedReferenceFrame( ) ) - { - throw std::runtime_error( "Warning when making spherical harmonic acceleration on body " + - nameOfBodyUndergoingAcceleration + ", rotation model found for " + - nameOfBodyExertingAcceleration + " is incompatible, frames are: " + - rotationalEphemeris->getTargetFrameOrientation( ) + " and " + - sphericalHarmonicsGravityField->getFixedReferenceFrame( ) ); - } - - boost::function< double( ) > gravitationalParameterFunction; - - // Check if mutual acceleration is to be used. - if( useCentralBodyFixedFrame == false || - bodyUndergoingAcceleration->getGravityFieldModel( ) == NULL ) - { - gravitationalParameterFunction = - boost::bind( &SphericalHarmonicsGravityField::getGravitationalParameter, - sphericalHarmonicsGravityField ); - } - else - { - // Create function returning summed gravitational parameter of the two bodies. - boost::function< double( ) > gravitationalParameterOfBodyExertingAcceleration = - boost::bind( &gravitation::GravityFieldModel::getGravitationalParameter, - sphericalHarmonicsGravityField ); - boost::function< double( ) > gravitationalParameterOfBodyUndergoingAcceleration = - boost::bind( &gravitation::GravityFieldModel::getGravitationalParameter, - bodyUndergoingAcceleration->getGravityFieldModel( ) ); - gravitationalParameterFunction = - boost::bind( &evaluateDoubleFunctions, - gravitationalParameterOfBodyExertingAcceleration, - gravitationalParameterOfBodyUndergoingAcceleration ); - } - - // Create acceleration object. - accelerationModel = - boost::make_shared< SphericalHarmonicsGravitationalAccelerationModelXd > - ( boost::bind( &Body::getPosition, bodyUndergoingAcceleration ), - gravitationalParameterFunction, - sphericalHarmonicsGravityField->getReferenceRadius( ), - boost::bind( &SphericalHarmonicsGravityField::getCosineCoefficients, - sphericalHarmonicsGravityField, - sphericalHarmonicsSettings->maximumDegree_, - sphericalHarmonicsSettings->maximumOrder_ ), - boost::bind( &SphericalHarmonicsGravityField::getSineCoefficients, - sphericalHarmonicsGravityField, - sphericalHarmonicsSettings->maximumDegree_, - sphericalHarmonicsSettings->maximumOrder_ ), - boost::bind( &Body::getPosition, bodyExertingAcceleration ), - boost::bind( &Body::getCurrentRotationToGlobalFrame, - bodyExertingAcceleration ) ); - } - } - return accelerationModel; -} - - -//! Function to create a third body central gravity acceleration model. -boost::shared_ptr< gravitation::ThirdBodyCentralGravityAcceleration > -createThirdBodyCentralGravityAccelerationModel( - const boost::shared_ptr< Body > bodyUndergoingAcceleration, - const boost::shared_ptr< Body > bodyExertingAcceleration, - const boost::shared_ptr< Body > centralBody, - const std::string& nameOfBodyUndergoingAcceleration, - const std::string& nameOfBodyExertingAcceleration, - const std::string& nameOfCentralBody ) -{ - // Declare pointer to return object. - boost::shared_ptr< ThirdBodyCentralGravityAcceleration > accelerationModelPointer; - - // Create acceleration object. - accelerationModelPointer = boost::make_shared< ThirdBodyCentralGravityAcceleration >( - boost::dynamic_pointer_cast< CentralGravitationalAccelerationModel3d >( - createCentralGravityAcceleratioModel( bodyUndergoingAcceleration, - bodyExertingAcceleration, - nameOfBodyUndergoingAcceleration, - nameOfBodyExertingAcceleration, 0 ) ), - boost::dynamic_pointer_cast< CentralGravitationalAccelerationModel3d >( - createCentralGravityAcceleratioModel( centralBody, bodyExertingAcceleration, - nameOfCentralBody, - nameOfBodyExertingAcceleration, 0 ) ), nameOfCentralBody ); - - return accelerationModelPointer; -} - - -//! Function to create an aerodynamic acceleration model. -boost::shared_ptr< aerodynamics::AerodynamicAcceleration > createAerodynamicAcceleratioModel( - const boost::shared_ptr< Body > bodyUndergoingAcceleration, - const boost::shared_ptr< Body > bodyExertingAcceleration, - const std::string& nameOfBodyUndergoingAcceleration, - const std::string& nameOfBodyExertingAcceleration ) -{ - // Check existence of required environment models - if( bodyUndergoingAcceleration->getAerodynamicCoefficientInterface( ) == NULL ) - { - throw std::runtime_error( "Error when making aerodynamic acceleration, body " + - nameOfBodyUndergoingAcceleration + - "has no aerodynamic coefficients." ); - } - - if( bodyExertingAcceleration->getAtmosphereModel( ) == NULL ) - { - throw std::runtime_error( "Error when making aerodynamic acceleration, central body " + - nameOfBodyExertingAcceleration + " has no atmosphere model."); - } - - if( bodyExertingAcceleration->getShapeModel( ) == NULL ) - { - throw std::runtime_error( "Error when making aerodynamic acceleration, central body " + - nameOfBodyExertingAcceleration + " has no shape model." ); - } - - // Retrieve flight conditions; create object if not yet extant. - boost::shared_ptr< FlightConditions > bodyFlightConditions = - bodyUndergoingAcceleration->getFlightConditions( ); - if( bodyFlightConditions == NULL ) - { - bodyUndergoingAcceleration->setFlightConditions( - createFlightConditions( bodyUndergoingAcceleration, - bodyExertingAcceleration, - nameOfBodyUndergoingAcceleration, - nameOfBodyExertingAcceleration ) ); - bodyFlightConditions = bodyUndergoingAcceleration->getFlightConditions( ); - } - - // Retrieve frame in which aerodynamic coefficients are defined. - boost::shared_ptr< aerodynamics::AerodynamicCoefficientInterface > aerodynamicCoefficients = - bodyUndergoingAcceleration->getAerodynamicCoefficientInterface( ); - reference_frames::AerodynamicsReferenceFrames accelerationFrame; - if( aerodynamicCoefficients->getAreCoefficientsInAerodynamicFrame( ) ) - { - accelerationFrame = reference_frames::aerodynamic_frame; - } - else - { - accelerationFrame = reference_frames::body_frame; - } - - // Create function to transform from frame of aerodynamic coefficienrs to that of propagation. - boost::function< Eigen::Vector3d( const Eigen::Vector3d& ) > toPropagationFrameTransformation; - toPropagationFrameTransformation = - reference_frames::getAerodynamicForceTransformationFunction( - bodyFlightConditions->getAerodynamicAngleCalculator( ), - accelerationFrame, - boost::bind( &Body::getCurrentRotationToGlobalFrame, bodyExertingAcceleration ), - reference_frames::inertial_frame ); - - - boost::function< Eigen::Vector3d( ) > coefficientFunction = - boost::bind( &AerodynamicCoefficientInterface::getCurrentForceCoefficients, - aerodynamicCoefficients ); - boost::function< Eigen::Vector3d( ) > coefficientInPropagationFrameFunction = - boost::bind( &reference_frames::transformVectorFunctionFromVectorFunctions, - coefficientFunction, toPropagationFrameTransformation ); - - // Create acceleration model. - return boost::make_shared< AerodynamicAcceleration >( - coefficientInPropagationFrameFunction, - boost::bind( &FlightConditions::getCurrentDensity, bodyFlightConditions ), - boost::bind( &FlightConditions::getCurrentAirspeed, bodyFlightConditions ), - boost::bind( &Body::getBodyMass, bodyUndergoingAcceleration ), - boost::bind( &AerodynamicCoefficientInterface::getReferenceArea, - aerodynamicCoefficients ), - aerodynamicCoefficients->getAreCoefficientsInNegativeAxisDirection( ) ); -} - -//! Function to create a cannonball radiation pressure acceleration model. -boost::shared_ptr< CannonBallRadiationPressureAcceleration > -createCannonballRadiationPressureAcceleratioModel( - const boost::shared_ptr< Body > bodyUndergoingAcceleration, - const boost::shared_ptr< Body > bodyExertingAcceleration, - const std::string& nameOfBodyUndergoingAcceleration, - const std::string& nameOfBodyExertingAcceleration ) -{ - // Retrieve radiation pressure interface - if( bodyUndergoingAcceleration->getRadiationPressureInterfaces( ).count( - nameOfBodyExertingAcceleration ) == 0 ) - { - throw std::runtime_error( - "Error when making radiation pressure, no radiation pressure interface found in " + - nameOfBodyUndergoingAcceleration + - " for body " + nameOfBodyExertingAcceleration ); - } - boost::shared_ptr< RadiationPressureInterface > radiationPressureInterface = - bodyUndergoingAcceleration->getRadiationPressureInterfaces( ).at( - nameOfBodyExertingAcceleration ); - - // Create acceleration model. - return boost::make_shared< CannonBallRadiationPressureAcceleration >( - boost::bind( &Body::getPosition, bodyExertingAcceleration ), - boost::bind( &Body::getPosition, bodyUndergoingAcceleration ), - boost::bind( &RadiationPressureInterface::getCurrentRadiationPressure, - radiationPressureInterface ), - boost::bind( &RadiationPressureInterface::getRadiationPressureCoefficient, - radiationPressureInterface ), - boost::bind( &RadiationPressureInterface::getArea, radiationPressureInterface ), - boost::bind( &Body::getBodyMass, bodyUndergoingAcceleration ) ); - -} - - -//! Function to create acceleration model object. -boost::shared_ptr< AccelerationModel< Eigen::Vector3d > > createAccelerationModel( - const boost::shared_ptr< Body > bodyUndergoingAcceleration, - const boost::shared_ptr< Body > bodyExertingAcceleration, - const boost::shared_ptr< AccelerationSettings > accelerationSettings, - const std::string& nameOfBodyUndergoingAcceleration, - const std::string& nameOfBodyExertingAcceleration, - const boost::shared_ptr< Body > centralBody, - const std::string& nameOfCentralBody ) -{ - // Declare pointer to return object. - boost::shared_ptr< AccelerationModel< Eigen::Vector3d > > accelerationModelPointer; - - // Switch to call correct acceleration model type factory function. - switch( accelerationSettings->accelerationType_ ) - { - case central_gravity: - // Check if body is a single-body central gravity acceleration (use third-body if not) - if( nameOfCentralBody == nameOfBodyExertingAcceleration || - isFrameInertial( nameOfCentralBody ) ) - { - // Check if gravitational parameter to use is sum of gravitational paramater of the - // two bodies. - bool useCentralBodyFixedFrame = 0; - if( nameOfCentralBody == nameOfBodyExertingAcceleration ) - { - useCentralBodyFixedFrame = 1; - } - - accelerationModelPointer = createCentralGravityAcceleratioModel( - bodyUndergoingAcceleration, - bodyExertingAcceleration, - nameOfBodyUndergoingAcceleration, - nameOfBodyExertingAcceleration, useCentralBodyFixedFrame ); - } - // Create third body central gravity acceleration - else - { - - accelerationModelPointer = createThirdBodyCentralGravityAccelerationModel( - bodyUndergoingAcceleration, - bodyExertingAcceleration, - centralBody, - nameOfBodyUndergoingAcceleration, - nameOfBodyExertingAcceleration, - nameOfCentralBody ); - } - break; - case spherical_harmonic_gravity: - if( nameOfCentralBody == nameOfBodyExertingAcceleration || - isFrameInertial( nameOfCentralBody ) ) - { - // Check if gravitational parameter to use is sum of gravitational paramater of the - // two bodies. - bool useCentralBodyFixedFrame = 0; - if( nameOfCentralBody == nameOfBodyExertingAcceleration ) - { - useCentralBodyFixedFrame = 1; - } - - accelerationModelPointer = createSphericalHarmonicsGravityAcceleration( - bodyUndergoingAcceleration, - bodyExertingAcceleration, - nameOfBodyUndergoingAcceleration, - nameOfBodyExertingAcceleration, - accelerationSettings, useCentralBodyFixedFrame ); - - } - else - { - throw std::runtime_error( - "Error, cannot yet make third body spherical harmonic acceleration." ); - - } - break; - case aerodynamic: - accelerationModelPointer = createAerodynamicAcceleratioModel( - bodyUndergoingAcceleration, - bodyExertingAcceleration, - nameOfBodyUndergoingAcceleration, - nameOfBodyExertingAcceleration ); - break; - case cannon_ball_radiation_pressure: - accelerationModelPointer = createCannonballRadiationPressureAcceleratioModel( - bodyUndergoingAcceleration, - bodyExertingAcceleration, - nameOfBodyUndergoingAcceleration, - nameOfBodyExertingAcceleration ); - break; - default: - throw std::runtime_error( - std::string( "Error, acceleration model ") + - boost::lexical_cast< std::string >( accelerationSettings->accelerationType_ ) + - " not recognized when making acceleration model of" + - nameOfBodyExertingAcceleration + " on " + - nameOfBodyUndergoingAcceleration ); - break; - } - return accelerationModelPointer; -} - -//! Function to create a set of acceleration models from a map of bodies and acceleration model -//! types. -AccelerationMap createAccelerationModelsMap( - const NamedBodyMap& bodyMap, - const SelectedAccelerationMap& selectedAccelerationPerBody, - const std::map< std::string, std::string >& centralBodies ) -{ - // Declare return map. - AccelerationMap accelerationModelMap; - - // Iterate over all bodies which are undergoing acceleration - for( SelectedAccelerationMap::const_iterator bodyIterator = - selectedAccelerationPerBody.begin( ); bodyIterator != selectedAccelerationPerBody.end( ); - bodyIterator++ ) - { - boost::shared_ptr< Body > currentCentralBody; - - // Retrieve name of body undergoing acceleration. - std::string bodyUndergoingAcceleration = bodyIterator->first; - - // Retrieve name of current central body. - std::string currentCentralBodyName = centralBodies.at( bodyUndergoingAcceleration ); - - if( !isFrameInertial( currentCentralBodyName ) ) - { - if( bodyMap.count( currentCentralBodyName ) == 0 ) - { - throw std::runtime_error( - std::string( "Error, could not find non-inertial central body ") + - currentCentralBodyName + " of " + bodyUndergoingAcceleration + - " when making acceleration model." ); - } - else - { - currentCentralBody = bodyMap.at( currentCentralBodyName ); - } - } - - // Check if body undergoing acceleration is included in bodyMap - if( bodyMap.count( bodyUndergoingAcceleration ) == 0 ) - { - throw std::runtime_error( - std::string( "Error when making acceleration models, requested forces" ) + - "acting on body " + bodyUndergoingAcceleration + - ", but no such body found in map of bodies" ); - } - - // Declare map of acceleration models acting on current body. - SingleBodyAccelerationMap mapOfAccelerationsForBody; - - // Retrieve list of required acceleration model types and bodies exerting accelerationd on - // current body. - std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > - accelerationsForBody = bodyIterator->second; - - // Iterate over all bodies exerting an acceleration - for( std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > >:: - iterator body2Iterator = accelerationsForBody.begin( ); - body2Iterator != accelerationsForBody.end( ); body2Iterator++ ) - { - // Retrieve name of body exerting acceleration. - std::string bodyExertingAcceleration = body2Iterator->first; - - // Check if body exerting acceleration is included in bodyMap - if( bodyMap.count( bodyExertingAcceleration ) == 0 ) - { - throw std::runtime_error( - std::string( "Error when making acceleration models, requested forces ") - + "acting on body " + bodyUndergoingAcceleration + " due to body " + - bodyExertingAcceleration + - ", but no such body found in map of bodies" ); - } - - // Retrieve list of accelerations due to current body. - std::vector< boost::shared_ptr< AccelerationSettings > > accelerationList = - body2Iterator->second; - - for( unsigned int i = 0; i < accelerationList.size( ); i++ ) - { - // Create acceleration model. - mapOfAccelerationsForBody[ bodyExertingAcceleration ].push_back( - createAccelerationModel( bodyMap.at( bodyUndergoingAcceleration ), - bodyMap.at( bodyExertingAcceleration ), - accelerationList.at( i ), - bodyUndergoingAcceleration, - bodyExertingAcceleration, - currentCentralBody, - currentCentralBodyName ) ); - } - } - - // Put acceleration models on current body in return map. - accelerationModelMap[ bodyUndergoingAcceleration ] = mapOfAccelerationsForBody; - } - - return accelerationModelMap; -} - -//! Function to create acceleration models from a map of bodies and acceleration model types. -basic_astrodynamics::AccelerationMap createAccelerationModelsMap( - const NamedBodyMap& bodyMap, - const SelectedAccelerationMap& selectedAccelerationPerBody, - const std::vector< std::string >& propagatedBodies, - const std::vector< std::string >& centralBodies ) -{ - if( centralBodies.size( ) != propagatedBodies.size( ) ) - { - throw std::runtime_error( "Error, number of propagated bodies must equal number of central bodies" ); - } - - std::map< std::string, std::string > centralBodyMap; - for( unsigned int i = 0; i < propagatedBodies.size( ); i++ ) - { - centralBodyMap[ propagatedBodies.at( i ) ] = centralBodies.at( i ); - } - - return createAccelerationModelsMap( bodyMap, selectedAccelerationPerBody, centralBodyMap ); -} - -} // namespace simulation_setup - -} // namespace tudat diff --git a/Tudat/SimulationSetup/createAccelerationModels.h b/Tudat/SimulationSetup/createAccelerationModels.h deleted file mode 100644 index 3063a0edcb..0000000000 --- a/Tudat/SimulationSetup/createAccelerationModels.h +++ /dev/null @@ -1,208 +0,0 @@ -/* Copyright (c) 2010-2016, Delft University of Technology - * All rigths reserved - * - * This file is part of the Tudat. Redistribution and use in source and - * binary forms, with or without modification, are permitted exclusively - * under the terms of the Modified BSD license. You should have received - * a copy of the license with this file. If not, please or visit: - * http://tudat.tudelft.nl/LICENSE. - */ - -#ifndef TUDAT_CREATEACCELERATIONMODELS_H -#define TUDAT_CREATEACCELERATIONMODELS_H - -#include -#include - -#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h" -#include "Tudat/Astrodynamics/Gravitation/centralGravityModel.h" -#include "Tudat/SimulationSetup/body.h" -#include "Tudat/Astrodynamics/Aerodynamics/aerodynamicAcceleration.h" -#include "Tudat/SimulationSetup/accelerationSettings.h" -#include "Tudat/Astrodynamics/ElectroMagnetism/cannonBallRadiationPressureAcceleration.h" -#include "Tudat/Astrodynamics/Gravitation/thirdBodyPerturbation.h" - -namespace tudat -{ - -namespace simulation_setup -{ - -//! Function to create central gravity acceleration model. -/*! - * Function to create central gravity acceleration model from bodies exerting and undergoing - * acceleration. - * \param bodyUndergoingAcceleration Pointer to object of body that is being accelerated. - * \param bodyExertingAcceleration Pointer to object of body that is exerting the central gravity - * acceleration. - * \param nameOfBodyUndergoingAcceleration Name of body that is being accelerated. - * \param nameOfBodyExertingAcceleration Name of body that is exerting the central gravity - * acceleration. - * \param useCentralBodyFixedFrame Boolean setting whether the central attraction of body - * undergoing acceleration on body exerting acceleration is to be included in acceleration model. - * Should be set to true in case the body undergoing acceleration is a celestial body - * (with gravity field) and integration is performed in the frame centered at the body exerting - * acceleration. - * \return Central gravity acceleration model pointer. - */ -boost::shared_ptr< gravitation::CentralGravitationalAccelerationModel3d > -createCentralGravityAcceleratioModel( - const boost::shared_ptr< Body > bodyUndergoingAcceleration, - const boost::shared_ptr< Body > bodyExertingAcceleration, - const std::string& nameOfBodyUndergoingAcceleration, - const std::string& nameOfBodyExertingAcceleration, - const bool useCentralBodyFixedFrame ); - -//! Function to create spherical harmonic gravity acceleration model. -/*! - * Function to create spherical harmonic gravity acceleration model from bodies exerting and - * undergoing acceleration. - * \param bodyUndergoingAcceleration Pointer to object of body that is being accelerated. - * \param bodyExertingAcceleration Pointer to object of body that is exerting the spherical - * harmonic gravity acceleration. - * \param nameOfBodyUndergoingAcceleration Name of body that is being accelerated. - * \param nameOfBodyExertingAcceleration Name of body that is exerting the spherical harmonic - * gravity acceleration. - * \param accelerationSettings Settings for acceleration model that is to be created (should - * be of derived type associated with spherical harmonic acceleration. - * \param useCentralBodyFixedFrame Boolean setting whether the central attraction of body - * undergoing acceleration on body exerting acceleration is to be included in acceleration model. - * Should be set to true in case the body undergoing acceleration is a celestial body - * (with gravity field) and integration is performed in the frame centered at the body exerting - * acceleration. - * \return Spherical harmonic gravity acceleration model pointer. - */ -boost::shared_ptr< gravitation::SphericalHarmonicsGravitationalAccelerationModelXd > -createSphericalHarmonicsGravityAcceleration( - const boost::shared_ptr< Body > bodyUndergoingAcceleration, - const boost::shared_ptr< Body > bodyExertingAcceleration, - const std::string& nameOfBodyUndergoingAcceleration, - const std::string& nameOfBodyExertingAcceleration, - const boost::shared_ptr< AccelerationSettings > accelerationSettings, - const bool useCentralBodyFixedFrame ); - -//! Function to create a third body central gravity acceleration model. -/*! - * Function to create a third body central gravity acceleration model from bodies exerting and - * undergoing acceleration, as well as the central body, w.r.t. which the integration is to be - * performed. - * \param bodyUndergoingAcceleration Pointer to object of body that is being accelerated. - * \param bodyExertingAcceleration Pointer to object of body that is exerting the acceleration. - * \param centralBody Pointer to central body in frame centered at which acceleration is to be - * calculated. - * \param nameOfBodyUndergoingAcceleration Name of object of body that is being accelerated. - * \param nameOfBodyExertingAcceleration Name of object of body that is exerting the central - * gravity acceleration. - * \param nameOfCentralBody Name of central body in frame cenetered at which acceleration is to - * be calculated. - * \return Pointer to object for calculating central gravity acceleration between bodies. - */ -boost::shared_ptr< gravitation::ThirdBodyCentralGravityAcceleration > -createThirdBodyCentralGravityAccelerationModel( - const boost::shared_ptr< Body > bodyUndergoingAcceleration, - const boost::shared_ptr< Body > bodyExertingAcceleration, - const boost::shared_ptr< Body > centralBody, - const std::string& nameOfBodyUndergoingAcceleration, - const std::string& nameOfBodyExertingAcceleration, - const std::string& nameOfCentralBody ); - -//! Function to create an aerodynamic acceleration model. -/*! - * Function to create an aerodynamic acceleration model, automatically creates all required - * links to environment models, vehicle properies and frame conversions - * \param bodyUndergoingAcceleration Pointer to object of body that is being accelerated. - * \param bodyExertingAcceleration Pointer to object of body that is exerting the acceleration, - * i.e. body with the atmosphere through which the accelerated body is flying. - * \param nameOfBodyUndergoingAcceleration Name of object of body that is being accelerated. - * \param nameOfBodyExertingAcceleration Name of object of body that is exerting the acceleration. - * \return Pointer to object for calculating aerodynamic acceleration. - */ -boost::shared_ptr< aerodynamics::AerodynamicAcceleration > -createAerodynamicAcceleratioModel( - const boost::shared_ptr< Body > bodyUndergoingAcceleration, - const boost::shared_ptr< Body > bodyExertingAcceleration, - const std::string& nameOfBodyUndergoingAcceleration, - const std::string& nameOfBodyExertingAcceleration ); - -//! Function to create a cannonball radiation pressure acceleration model. -/*! - * Function to create a cannonball radiation pressure automatically creates all required - * links to environment models, vehicle properies and frame conversions - * \param bodyUndergoingAcceleration Pointer to object of body that is being accelerated. - * \param bodyExertingAcceleration Pointer to object of body that is exerting the acceleration, - * i.e. body emitting the radiation. - * \param nameOfBodyUndergoingAcceleration Name of object of body that is being accelerated. - * \param nameOfBodyExertingAcceleration Name of object of body that is exerting the acceleration. - * \return Pointer to object for calculating cannonball radiation pressures acceleration. - */ -boost::shared_ptr< electro_magnetism::CannonBallRadiationPressureAcceleration > -createCannonballRadiationPressureAcceleratioModel( - const boost::shared_ptr< Body > bodyUndergoingAcceleration, - const boost::shared_ptr< Body > bodyExertingAcceleration, - const std::string& nameOfBodyUndergoingAcceleration, - const std::string& nameOfBodyExertingAcceleration ); - -//! Function to create acceleration model object. -/*! - * Function to create acceleration model object. - * Type of requested model is checked and corresponding factory function is called. - * \param bodyUndergoingAcceleration Pointer to object of body that is being accelerated. - * \param bodyExertingAcceleration Pointer to object of body that is exerting acceleration, - * \param accelerationSettings Settings for acceleration model that is to be created. - * \param nameOfBodyUndergoingAcceleration Name of object of body that is being accelerated. - * \param nameOfBodyExertingAcceleration Name of object of body that is exerting the acceleration. - * \param centralBody Pointer to central body in frame centered at which acceleration is to be - * calculated (optional, only relevant for third body accelerations). - * \param nameOfCentralBody Name of central body in frame cenetered at which acceleration is to - * be calculated (optional, only relevant for third body accelerations). - * \return Acceleration model pointer. - */ -boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > -createAccelerationModel( - const boost::shared_ptr< Body > bodyUndergoingAcceleration, - const boost::shared_ptr< Body > bodyExertingAcceleration, - const boost::shared_ptr< AccelerationSettings > accelerationSettings, - const std::string& nameOfBodyUndergoingAcceleration, - const std::string& nameOfBodyExertingAcceleration, - const boost::shared_ptr< Body > centralBody = boost::shared_ptr< Body >( ), - const std::string& nameOfCentralBody = "" ); - -//! Function to create acceleration models from a map of bodies and acceleration model types. -/*! - * Function to create acceleration models from a map of bodies and acceleration model types. - * The return type can be used to identify both the body undergoing and exerting acceleration. - * \param bodyMap List of pointers to bodies required for the creation of the acceleration model - * objects. - * \param selectedAccelerationPerBody List identifying which bodies exert which type of - * acceleration(s) on which bodies. - * \param centralBodies Map of central bodies for each body undergoing acceleration. - * \return List of acceleration model objects, in form of AccelerationMap. - */ -basic_astrodynamics::AccelerationMap createAccelerationModelsMap( - const NamedBodyMap& bodyMap, - const SelectedAccelerationMap& selectedAccelerationPerBody, - const std::map< std::string, std::string >& centralBodies ); - -//! Function to create acceleration models from a map of bodies and acceleration model types. -/*! - * Function to create acceleration models from a map of bodies and acceleration model types. - * The return type can be used to identify both the body undergoing and exerting acceleration. - * \param bodyMap List of pointers to bodies required for the creation of the acceleration model - * objects. - * \param selectedAccelerationPerBody List identifying which bodies exert which type of - * acceleration(s) on which bodies. - * \param propagatedBodies List of bodies that are to be propagated - * \param centralBodies List of central bodies for each body undergoing acceleration (in same order as propagatedBodies). - * \return List of acceleration model objects, in form of AccelerationMap. - */ -basic_astrodynamics::AccelerationMap createAccelerationModelsMap( - const NamedBodyMap& bodyMap, - const SelectedAccelerationMap& selectedAccelerationPerBody, - const std::vector< std::string >& propagatedBodies, - const std::vector< std::string >& centralBodies ); - - -} // namespace simulation_setup - -} // namespace tudat -#endif // TUDAT_CREATEACCELERATIONMODELS_H diff --git a/Tudat/SimulationSetup/tudatSimulationHeader.h b/Tudat/SimulationSetup/tudatSimulationHeader.h new file mode 100644 index 0000000000..82e348119b --- /dev/null +++ b/Tudat/SimulationSetup/tudatSimulationHeader.h @@ -0,0 +1,50 @@ +/* Copyright (c) 2010-2016, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + + +#ifndef TUDAT_SIMULATIONHEADER_H +#define TUDAT_SIMULATIONHEADER_H + +#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/timeConversions.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/keplerPropagator.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/sphericalStateConversions.h" +#include "Tudat/Astrodynamics/BasicAstrodynamics/stateRepresentationConversions.h" +#include "Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h" + +#include "Tudat/InputOutput/basicInputOutput.h" + +#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h" +#include "Tudat/Mathematics/BasicMathematics/linearAlgebraTypes.h" +#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h" +#include "Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h" +#include "Tudat/Mathematics/Interpolators/createInterpolator.h" + +#ifdef USE_CSPICE + +#include "Tudat/External/SpiceInterface/spiceInterface.h" + +#endif + +#include "Tudat/SimulationSetup/EnvironmentSetup/body.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h" +#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h" +#include "Tudat/SimulationSetup/EstimationSetup/createEstimatableParameters.h" +#include "Tudat/SimulationSetup/EstimationSetup/estimatableParameterSettings.h" +#include "Tudat/SimulationSetup/PropagationSetup/accelerationSettings.h" +#include "Tudat/SimulationSetup/PropagationSetup/propagationSettings.h" +#include "Tudat/SimulationSetup/PropagationSetup/propagationOutputSettings.h" +#include "Tudat/SimulationSetup/PropagationSetup/propagationTerminationSettings.h" +#include "Tudat/SimulationSetup/PropagationSetup/createNumericalSimulator.h" +#include "Tudat/SimulationSetup/PropagationSetup/thrustSettings.h" +#include "Tudat/SimulationSetup/PropagationSetup/createMassRateModels.h" + +#endif // TUDAT_SIMULATIONHEADER_H