diff --git a/include/tudat/astro/basic_astro/customAccelerationModel.h b/include/tudat/astro/basic_astro/customAccelerationModel.h index feae948b73..96a28279f8 100644 --- a/include/tudat/astro/basic_astro/customAccelerationModel.h +++ b/include/tudat/astro/basic_astro/customAccelerationModel.h @@ -25,7 +25,6 @@ class CustomAccelerationModel: public basic_astrodynamics::AccelerationModel3d const std::function< Eigen::Vector3d( const double ) > accelerationFunction ): accelerationFunction_( accelerationFunction ) { - updateMembers( TUDAT_NAN ); } virtual void updateMembers( const double currentTime = TUDAT_NAN ) diff --git a/include/tudat/astro/electromagnetism/cannonBallRadiationPressureAcceleration.h b/include/tudat/astro/electromagnetism/cannonBallRadiationPressureAcceleration.h index 42fb39f8c0..9b4eb3df89 100644 --- a/include/tudat/astro/electromagnetism/cannonBallRadiationPressureAcceleration.h +++ b/include/tudat/astro/electromagnetism/cannonBallRadiationPressureAcceleration.h @@ -105,7 +105,6 @@ class CannonBallRadiationPressureAcceleration: public basic_astrodynamics::Accel areaFunction_( areaFunction ), massFunction_( massFunction ) { - this->updateMembers( ); } //! Constructor taking functions pointers and constant values for parameters. @@ -135,7 +134,6 @@ class CannonBallRadiationPressureAcceleration: public basic_astrodynamics::Accel areaFunction_( [ = ]( ){ return area; } ), massFunction_( [ = ]( ){ return mass; } ) { - this->updateMembers( ); } //! Update member variables used by the radiation pressure acceleration model. diff --git a/include/tudat/astro/electromagnetism/solarSailAcceleration.h b/include/tudat/astro/electromagnetism/solarSailAcceleration.h index c145dc47f7..7fdcb6f200 100644 --- a/include/tudat/astro/electromagnetism/solarSailAcceleration.h +++ b/include/tudat/astro/electromagnetism/solarSailAcceleration.h @@ -145,7 +145,6 @@ class SolarSailAcceleration: public basic_astrodynamics::AccelerationModel3d areaFunction_( areaFunction ), massFunction_( massFunction ) { - this->updateMembers( ); } //! Constructor taking functions pointers and constant values for parameters. @@ -201,7 +200,6 @@ class SolarSailAcceleration: public basic_astrodynamics::AccelerationModel3d areaFunction_( [ = ]( ){ return area;} ), massFunction_( [ = ]( ){ return mass;} ) { - this->updateMembers( ); } @@ -232,7 +230,6 @@ class SolarSailAcceleration: public basic_astrodynamics::AccelerationModel3d areaFunction_( std::bind( &RadiationPressureInterface::getArea, radiationPressureInterface ) ), massFunction_( massFunction ) { - this->updateMembers( ); } diff --git a/include/tudat/astro/ephemerides/compositeEphemeris.h b/include/tudat/astro/ephemerides/compositeEphemeris.h index cc8b47a785..2bcf83ed49 100644 --- a/include/tudat/astro/ephemerides/compositeEphemeris.h +++ b/include/tudat/astro/ephemerides/compositeEphemeris.h @@ -358,6 +358,16 @@ std::shared_ptr< Ephemeris > createReferencePointEphemeris( std::shared_ptr< RotationalEphemeris > bodyRotationModel, std::function< Eigen::Vector6d( const double& ) > referencePointRelativeStateFunction ) { + if( bodyEphemeris == nullptr ) + { + throw std::runtime_error( "Error when creating reference point composite ephemeris, no body ephemeris is provided" ); + } + + if( bodyRotationModel == nullptr ) + { + throw std::runtime_error( "Error when creating reference point composite ephemeris, no body rotation model is provided" ); + } + typedef Eigen::Matrix< StateScalarType, 6, 1 > StateType; // Cast state fucntion of body (global) and reference point (local) into correct form. diff --git a/include/tudat/astro/gravitation/centralGravityModel.h b/include/tudat/astro/gravitation/centralGravityModel.h index b362011b75..ef1833563c 100644 --- a/include/tudat/astro/gravitation/centralGravityModel.h +++ b/include/tudat/astro/gravitation/centralGravityModel.h @@ -175,7 +175,6 @@ class CentralGravitationalAccelerationModel positionOfBodyExertingAccelerationFunction, isMutualAttractionUsed ) { - this->updateMembers( ); } //! Constructor taking position-functions for bodies, and constant gravitational parameter. @@ -209,7 +208,6 @@ class CentralGravitationalAccelerationModel positionOfBodyExertingAccelerationFunction, isMutualAttractionUsed ) { - this->updateMembers( ); } //! Update members. diff --git a/include/tudat/astro/gravitation/sphericalHarmonicsGravityModel.h b/include/tudat/astro/gravitation/sphericalHarmonicsGravityModel.h index 40979d8305..b20d41d738 100644 --- a/include/tudat/astro/gravitation/sphericalHarmonicsGravityModel.h +++ b/include/tudat/astro/gravitation/sphericalHarmonicsGravityModel.h @@ -228,7 +228,6 @@ class SphericalHarmonicsGravitationalAccelerationModel sphericalHarmonicsCache_->getMaximumDegree( ) ), std::max< int >( maximumOrder_, sphericalHarmonicsCache_->getMaximumOrder( ) ) + 1 ); - this->updateMembers( ); } //! Constructor taking functions for position of bodies, and parameters of spherical harmonics @@ -292,7 +291,6 @@ class SphericalHarmonicsGravitationalAccelerationModel sphericalHarmonicsCache_->getMaximumOrder( ) ) + 1 ); - this->updateMembers( ); } //! Get gravitational acceleration in body-fixed frame of body undergoing acceleration. diff --git a/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h b/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h index d31f144714..6746409e7b 100644 --- a/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h +++ b/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h @@ -225,6 +225,9 @@ class EstimatableParameter return Eigen::VectorXd::Zero( 0 ); } + virtual void throwExceptionIfNotFullyDefined( ){ } + + protected: diff --git a/include/tudat/astro/orbit_determination/estimatable_parameters/observationBiasParameter.h b/include/tudat/astro/orbit_determination/estimatable_parameters/observationBiasParameter.h index b1ba0c30af..e31bbe660e 100644 --- a/include/tudat/astro/orbit_determination/estimatable_parameters/observationBiasParameter.h +++ b/include/tudat/astro/orbit_determination/estimatable_parameters/observationBiasParameter.h @@ -83,7 +83,14 @@ class ConstantObservationBiasParameter: public EstimatableParameter< Eigen::Vect */ void setParameterValue( Eigen::VectorXd parameterValue ) { - resetCurrentBias_( parameterValue ); + if( resetCurrentBias_ != nullptr ) + { + resetCurrentBias_( parameterValue ); + } + else + { + throwExceptionIfNotFullyDefined( ); + } } //! Function to retrieve the size of the parameter (equal to the size of the observable). @@ -117,6 +124,20 @@ class ConstantObservationBiasParameter: public EstimatableParameter< Eigen::Vect resetCurrentBias_ = resetCurrentBias; } + void throwExceptionIfNotFullyDefined( ) + { + if( getCurrentBias_ == nullptr || resetCurrentBias_ == nullptr ) + { + throw std::runtime_error( + "Error in " + getParameterTypeString( parameterName_.first ) + + " of observable type " + observation_models::getObservableName( + observableType_, linkEnds_.size( ) ) + + " with link ends: " + observation_models::getLinkEndsString( linkEnds_ ) + + " parameter not linked to bias object. Associated bias model been implemented in observation model. " + + " This may be because you are resetting the parameter value before creating observation models, or because you have not defined the required bias model."); + } + } + //! Function to retrieve the observation link ends for which the bias is active. /*! * Function to retrieve the observation link ends for which the bias is active. @@ -242,12 +263,19 @@ class ArcWiseObservationBiasParameter: public EstimatableParameter< Eigen::Vecto { std::vector< Eigen::VectorXd > observationBiases; - for( int i = 0; i < numberOfArcs_; i++ ) + if( resetBiasList_ != nullptr ) { - observationBiases.push_back( parameterValue.segment( i * observableSize_, observableSize_ ) ); - } + for( int i = 0; i < numberOfArcs_; i++ ) + { + observationBiases.push_back( parameterValue.segment( i * observableSize_, observableSize_ ) ); + } - resetBiasList_( observationBiases ); + resetBiasList_( observationBiases ); + } + else + { + throwExceptionIfNotFullyDefined( ); + } } //! Function to retrieve the size of the parameter (equal to the size of the observable). @@ -348,6 +376,19 @@ class ArcWiseObservationBiasParameter: public EstimatableParameter< Eigen::Vecto lookupScheme_ = lookupScheme; } + void throwExceptionIfNotFullyDefined( ) + { + if( getBiasList_ == nullptr || resetBiasList_ == nullptr ) + { + throw std::runtime_error( + "Error in " + getParameterTypeString( parameterName_.first ) + + " of observable type " + observation_models::getObservableName( + observableType_, linkEnds_.size( ) ) + + " with link ends: " + observation_models::getLinkEndsString( linkEnds_ ) + + " parameter not linked to bias object. Has associated bias model been implemented in observation model?"); + } + } + protected: private: diff --git a/include/tudat/astro/orbit_determination/podInputOutputTypes.h b/include/tudat/astro/orbit_determination/podInputOutputTypes.h index 60288ea9d1..80c89f2c45 100644 --- a/include/tudat/astro/orbit_determination/podInputOutputTypes.h +++ b/include/tudat/astro/orbit_determination/podInputOutputTypes.h @@ -428,6 +428,18 @@ class EstimationConvergenceChecker unsigned int numberOfIterationsWithoutImprovement_; }; + +inline std::shared_ptr< EstimationConvergenceChecker > estimationConvergenceChecker( + const unsigned int maximumNumberOfIterations = 5, + const double minimumResidualChange = 0.0, + const double minimumResidual = 1.0E-20, + const int numberOfIterationsWithoutImprovement = 2 ) +{ + return std::make_shared< EstimationConvergenceChecker >( + maximumNumberOfIterations, minimumResidualChange, minimumResidual, numberOfIterationsWithoutImprovement ); +} + + void scaleInformationMatrixWithWeights( Eigen::MatrixXd& informationMatrix, const Eigen::VectorXd& weightsDiagonal ); diff --git a/include/tudat/simulation/environment_setup/body.h b/include/tudat/simulation/environment_setup/body.h index 8e5a51c919..c6d5cec464 100644 --- a/include/tudat/simulation/environment_setup/body.h +++ b/include/tudat/simulation/environment_setup/body.h @@ -220,6 +220,8 @@ class Body { bodyName_( "unnamed_body" ) { currentLongState_ = currentState_.cast< long double >( ); + isStateSet_ = false; + isRotationSet_ = false; } //! Function to retrieve the class returning the state of this body's ephemeris origin w.r.t. the global origin @@ -245,7 +247,17 @@ class Body { * Returns the internally stored current state vector. * \return Current state. */ - Eigen::Vector6d getState() { return currentState_; } + Eigen::Vector6d getState( ) + { + if( !isStateSet_ ) + { + throw std::runtime_error( "Error when retrieving state from body " + bodyName_ + ", state of body is not yet defined" ); + } + else + { + return currentState_; + } + } //! Set current state of body manually /*! @@ -254,8 +266,10 @@ class Body { * long precision current state. * \param state Current state of the body that is set. */ - void setState(const Eigen::Vector6d &state) { + void setState(const Eigen::Vector6d &state) + { currentState_ = state; + isStateSet_ = true; } //! Set current state of body manually in long double precision. @@ -268,6 +282,8 @@ class Body { void setLongState(const Eigen::Matrix &longState) { currentLongState_ = longState; currentState_ = longState.cast(); + isStateSet_ = true; + } //! Templated function to set the state manually. @@ -299,7 +315,8 @@ class Body { currentState_ = (bodyEphemeris_->getTemplatedStateFromEphemeris(time) + ephemerisFrameToBaseFrame_->getBaseFrameState(time)).template cast(); currentLongState_ = currentState_.template cast(); - } else + } + else { currentLongState_ = (bodyEphemeris_->getTemplatedStateFromEphemeris(time) + ephemerisFrameToBaseFrame_->getBaseFrameState(time)).template cast(); @@ -332,6 +349,7 @@ class Body { timeOfCurrentState_ = static_cast(time); } + isStateSet_ = true; } // extern template void setStateFromEphemeris< double, double >( const double& time ); @@ -346,7 +364,8 @@ class Body { * \return State at requested time */ template - Eigen::Matrix getStateInBaseFrameFromEphemeris(const TimeType time) { + Eigen::Matrix getStateInBaseFrameFromEphemeris(const TimeType time) + { setStateFromEphemeris(time); if (sizeof(StateScalarType) == 8) { return currentState_.template cast(); @@ -399,7 +418,17 @@ class Body { * Returns the internally stored current position vector. * \return Current position. */ - Eigen::Vector3d getPosition() { return currentState_.segment(0, 3); } + Eigen::Vector3d getPosition() + { + if( !isStateSet_ ) + { + throw std::runtime_error( "Error when retrieving position from body " + bodyName_ + ", state of body is not yet defined" ); + } + else + { + return currentState_.segment(0, 3); + } + } void getPositionByReference( Eigen::Vector3d& position ); @@ -408,28 +437,68 @@ class Body { * Returns the internally stored current velocity vector. * \return Current velocity. */ - Eigen::Vector3d getVelocity() { return currentState_.segment(3, 3); } + Eigen::Vector3d getVelocity() + { + if( !isStateSet_ ) + { + throw std::runtime_error( "Error when retrieving velociy from body " + bodyName_ + ", state of body is not yet defined" ); + } + else + { + return currentState_.segment(3, 3); + } + } //! Get current state, in long double precision /*! * Returns the internally stored current state vector, in long double precision * \return Current state, in long double precisio */ - Eigen::Matrix getLongState() { return currentLongState_; } + Eigen::Matrix getLongState() + { + if( !isStateSet_ ) + { + throw std::runtime_error( "Error when retrieving long state from body " + bodyName_ + ", state of body is not yet defined" ); + } + else + { + return currentLongState_; + } + } //! Get current position, in long double precision /*! * Returns the internally stored current position vector, in long double precision * \return Current position, in long double precision */ - Eigen::Matrix getLongPosition() { return currentLongState_.segment(0, 3); } + Eigen::Matrix getLongPosition( ) + { + if( !isStateSet_ ) + { + throw std::runtime_error( "Error when retrieving long position from body " + bodyName_ + ", state of body is not yet defined" ); + } + else + { + return currentLongState_.segment(0, 3); + } + } //! Get current velocity, in long double precision. /*! * Returns the internally stored current velocity vector. * \return Current velocity, in long double precision */ - Eigen::Matrix getLongVelocity() { return currentLongState_.segment(3, 3); } + Eigen::Matrix getLongVelocity( ) + { + if( !isStateSet_ ) + { + throw std::runtime_error( "Error when retrieving long velocity from body " + bodyName_ + ", state of body is not yet defined" ); + } + else + { + return currentLongState_.segment(3, 3); + } + } //! Templated function to retrieve the state. /*! @@ -461,44 +530,45 @@ class Body { "Error, no rotation model found in Body::setCurrentRotationToLocalFrameFromEphemeris" ); } currentRotationToGlobalFrame_ = currentRotationToLocalFrame_.inverse( ); - } - - //! Function to set the rotation matrix derivative from global to body-fixed frame at given time - /*! - * Function to set the rotation matrix derivative from global to body-fixed frame at given time, - * using the rotationalEphemeris_ member object - * \param time Time at which the rotation matrix derivative is to be retrieved. - */ - void setCurrentRotationToLocalFrameDerivativeFromEphemeris(const double time) { - if (rotationalEphemeris_ != nullptr) { - currentRotationToLocalFrameDerivative_ = rotationalEphemeris_->getDerivativeOfRotationToTargetFrame(time); - } else if (dependentOrientationCalculator_ != nullptr) { - currentRotationToLocalFrameDerivative_.setZero(); - } else { - throw std::runtime_error( - "Error, no rotationalEphemeris_ found in Body::setCurrentRotationToLocalFrameDerivativeFromEphemeris"); - } - } - - //! Function to set the angular velocity vector in the global frame at given time - /*! - * Function to set the angular velocity vector in the global frame at given time, using the - * rotationalEphemeris_ member object - * \param time Time at which the angular velocity vector in the global frame is to be retrieved. - */ - void setCurrentAngularVelocityVectorInGlobalFrame(const double time) { - if (rotationalEphemeris_ != nullptr) { - currentAngularVelocityVectorInGlobalFrame_ = rotationalEphemeris_->getRotationalVelocityVectorInBaseFrame(time); - currentAngularVelocityVectorInLocalFrame_ = currentRotationToLocalFrame_ * currentAngularVelocityVectorInGlobalFrame_; - - } else if (dependentOrientationCalculator_ != nullptr) { - currentAngularVelocityVectorInGlobalFrame_.setZero(); - currentAngularVelocityVectorInLocalFrame_.setZero(); - } else { - throw std::runtime_error( - "Error, no rotationalEphemeris_ found in Body::setCurrentAngularVelocityVectorInGlobalFrame"); - } - } + isRotationSet_ = true; + } + +// //! Function to set the rotation matrix derivative from global to body-fixed frame at given time +// /*! +// * Function to set the rotation matrix derivative from global to body-fixed frame at given time, +// * using the rotationalEphemeris_ member object +// * \param time Time at which the rotation matrix derivative is to be retrieved. +// */ +// void setCurrentRotationToLocalFrameDerivativeFromEphemeris(const double time) { +// if (rotationalEphemeris_ != nullptr) { +// currentRotationToLocalFrameDerivative_ = rotationalEphemeris_->getDerivativeOfRotationToTargetFrame(time); +// } else if (dependentOrientationCalculator_ != nullptr) { +// currentRotationToLocalFrameDerivative_.setZero(); +// } else { +// throw std::runtime_error( +// "Error, no rotationalEphemeris_ found in Body::setCurrentRotationToLocalFrameDerivativeFromEphemeris"); +// } +// } + +// //! Function to set the angular velocity vector in the global frame at given time +// /*! +// * Function to set the angular velocity vector in the global frame at given time, using the +// * rotationalEphemeris_ member object +// * \param time Time at which the angular velocity vector in the global frame is to be retrieved. +// */ +// void setCurrentAngularVelocityVectorInGlobalFrame(const double time) { +// if (rotationalEphemeris_ != nullptr) { +// currentAngularVelocityVectorInGlobalFrame_ = rotationalEphemeris_->getRotationalVelocityVectorInBaseFrame(time); +// currentAngularVelocityVectorInLocalFrame_ = currentRotationToLocalFrame_ * currentAngularVelocityVectorInGlobalFrame_; + +// } else if (dependentOrientationCalculator_ != nullptr) { +// currentAngularVelocityVectorInGlobalFrame_.setZero(); +// currentAngularVelocityVectorInLocalFrame_.setZero(); +// } else { +// throw std::runtime_error( +// "Error, no rotationalEphemeris_ found in Body::setCurrentAngularVelocityVectorInGlobalFrame"); +// } +// } //! Function to set the full rotational state at given time /*! @@ -530,6 +600,7 @@ class Body { "Error, no rotationalEphemeris_ found in Body::setCurrentRotationalStateToLocalFrameFromEphemeris" ); } currentRotationToGlobalFrame_ = currentRotationToLocalFrame_.inverse( ); + isRotationSet_ = true; } @@ -559,6 +630,8 @@ class Body { currentRotationToLocalFrameDerivative_ = linear_algebra::getCrossProductMatrix( currentRotationalStateFromLocalToGlobalFrame.block< 3, 1 >(4, 0 )) * currentRotationMatrixToLocalFrame; + isRotationSet_ = true; + } //! Get current rotation from body-fixed to inertial frame. @@ -571,12 +644,26 @@ class Body { */ Eigen::Quaterniond getCurrentRotationToGlobalFrame( ) { - return currentRotationToGlobalFrame_; + if( !isRotationSet_ ) + { + throw std::runtime_error( "Error when retrieving rotation to global frame from body " + bodyName_ + ", state of body is not yet defined" ); + } + else + { + return currentRotationToGlobalFrame_; + } } Eigen::Quaterniond& getCurrentRotationToGlobalFrameReference( ) { - return currentRotationToGlobalFrame_; + if( !isRotationSet_ ) + { + throw std::runtime_error( "Error when retrieving rotation to global frame from body " + bodyName_ + ", state of body is not yet defined" ); + } + else + { + return currentRotationToGlobalFrame_; + } } //! Get current rotation from inertial to body-fixed frame. @@ -587,17 +674,41 @@ class Body { * an identity quaternion (no rotation) is returned. * \return Current rotation from inertial to body-fixed frame. */ - Eigen::Quaterniond getCurrentRotationToLocalFrame() { - return currentRotationToLocalFrame_; + Eigen::Quaterniond getCurrentRotationToLocalFrame() + { + if( !isRotationSet_ ) + { + throw std::runtime_error( "Error when retrieving rotation to local frame from body " + bodyName_ + ", state of body is not yet defined" ); + } + else + { + return currentRotationToLocalFrame_; + } } - Eigen::Matrix3d getCurrentRotationMatrixToGlobalFrame() { - return Eigen::Matrix3d( currentRotationToLocalFrame_.inverse() ); - } + Eigen::Matrix3d getCurrentRotationMatrixToGlobalFrame() + { + if( !isRotationSet_ ) + { + throw std::runtime_error( "Error when retrieving rotation to global frame from body " + bodyName_ + ", state of body is not yet defined" ); + } + else + { + return Eigen::Matrix3d( currentRotationToLocalFrame_.inverse() ); + } + } - Eigen::Matrix3d getCurrentRotationMatrixToLocalFrame() { - return Eigen::Matrix3d( currentRotationToLocalFrame_ ); - } + Eigen::Matrix3d getCurrentRotationMatrixToLocalFrame() + { + if( !isRotationSet_ ) + { + throw std::runtime_error( "Error when retrieving rotation to local frame from body " + bodyName_ + ", state of body is not yet defined" ); + } + else + { + return Eigen::Matrix3d( currentRotationToLocalFrame_ ); + } + } //! Get current rotational state. /*! @@ -605,10 +716,18 @@ class Body { * (in vector form) and the body's angular velocity vector in inertial frame. * \return Current rotational state in quaternions and rotational velocity. */ - Eigen::Vector7d getCurrentRotationalState() { - return (Eigen::VectorXd(7) << linear_algebra::convertQuaternionToVectorFormat(getCurrentRotationToGlobalFrame()), - getCurrentAngularVelocityVectorInGlobalFrame()) - .finished(); + Eigen::Vector7d getCurrentRotationalState() + { + if( !isRotationSet_ ) + { + throw std::runtime_error( "Error when retrieving rotation from body " + bodyName_ + ", state of body is not yet defined" ); + } + else + { + return (Eigen::VectorXd(7) << linear_algebra::convertQuaternionToVectorFormat(getCurrentRotationToGlobalFrame()), + getCurrentAngularVelocityVectorInGlobalFrame()) + .finished(); + } } //! Get current rotation matrix derivative from body-fixed to global frame. @@ -619,8 +738,16 @@ class Body { * ephemeris, an zero matrix (no rotation) is returned. * \return Current otation matrix derivative from global to body-fixed frame. */ - Eigen::Matrix3d getCurrentRotationMatrixDerivativeToGlobalFrame() { - return currentRotationToLocalFrameDerivative_.transpose(); + Eigen::Matrix3d getCurrentRotationMatrixDerivativeToGlobalFrame( ) + { + if( !isRotationSet_ ) + { + throw std::runtime_error( "Error when retrieving derivative of rotation to global frame from body " + bodyName_ + ", state of body is not yet defined" ); + } + else + { + return currentRotationToLocalFrameDerivative_.transpose(); + } } //! Get current rotation matrix derivative from global to body-fixed frame. @@ -631,8 +758,16 @@ class Body { * ephemeris, an zero matrix (no rotation) is returned. * \return Current otation matrix derivative from global to body-fixed frame. */ - Eigen::Matrix3d getCurrentRotationMatrixDerivativeToLocalFrame() { - return currentRotationToLocalFrameDerivative_; + Eigen::Matrix3d getCurrentRotationMatrixDerivativeToLocalFrame() + { + if( !isRotationSet_ ) + { + throw std::runtime_error( "Error when retrieving derivative of rotation to local frame from body " + bodyName_ + ", state of body is not yet defined" ); + } + else + { + return currentRotationToLocalFrameDerivative_; + } } //! Get current angular velocity vector for body's rotation, expressed in the global frame. @@ -640,8 +775,16 @@ class Body { * Get current angular velocity vector for body's rotation, expressed in the global frame. * \return Current angular velocity vector for body's rotation, expressed in the global frame. */ - Eigen::Vector3d getCurrentAngularVelocityVectorInGlobalFrame() { - return currentAngularVelocityVectorInGlobalFrame_; + Eigen::Vector3d getCurrentAngularVelocityVectorInGlobalFrame( ) + { + if( !isRotationSet_ ) + { + throw std::runtime_error( "Error when retrieving angular velocioty of body " + bodyName_ + ", state of body is not yet defined" ); + } + else + { + return currentAngularVelocityVectorInGlobalFrame_; + } } //! Get current angular velocity vector for body's rotation, expressed in the local frame. @@ -651,8 +794,16 @@ class Body { * current quaternion to local frame. * \return Current angular velocity vector for body's rotation, expressed in the local frame. */ - Eigen::Vector3d getCurrentAngularVelocityVectorInLocalFrame() { - return currentAngularVelocityVectorInLocalFrame_; + Eigen::Vector3d getCurrentAngularVelocityVectorInLocalFrame( ) + { + if( !isRotationSet_ ) + { + throw std::runtime_error( "Error when retrieving angular velocioty of body " + bodyName_ + ", state of body is not yet defined" ); + } + else + { + return currentAngularVelocityVectorInLocalFrame_; + } } //! Function to set the ephemeris of the body. @@ -664,10 +815,6 @@ class Body { void setEphemeris( const std::shared_ptr< ephemerides::Ephemeris > bodyEphemeris ) { bodyEphemeris_ = bodyEphemeris; -// if( resetBaseFrames_ != nullptr ) -// { -// resetBaseFrames_( ); -// } } //! Function to set the gravity field of the body. @@ -1205,10 +1352,6 @@ class Body { void setBodyName( const std::string bodyName ){ bodyName_ = bodyName; } - void setBaseFrameFunction( const std::function< void( ) > resetBaseFrames ) - { - resetBaseFrames_ = resetBaseFrames; - } protected: private: //! Variable denoting whether this body is the global frame origin (1 if true, 0 if false, -1 if not yet set) @@ -1306,7 +1449,9 @@ class Body { std::string bodyName_; - std::function< void( ) > resetBaseFrames_; + bool isStateSet_; + + bool isRotationSet_; }; diff --git a/include/tudat/simulation/environment_setup/createGroundStations.h b/include/tudat/simulation/environment_setup/createGroundStations.h index 8953b736c9..9fc452bc54 100644 --- a/include/tudat/simulation/environment_setup/createGroundStations.h +++ b/include/tudat/simulation/environment_setup/createGroundStations.h @@ -123,9 +123,24 @@ std::shared_ptr< ephemerides::Ephemeris > createReferencePointEphemeris( { typedef Eigen::Matrix< StateScalarType, 6, 1 > StateType; + if( bodyWithReferencePoint == nullptr ) + { + throw std::runtime_error( "Error when creating reference point ephemeris, body is not provided" ); + } + std::shared_ptr< ephemerides::RotationalEphemeris > bodyRotationModel = bodyWithReferencePoint->getRotationalEphemeris( ); + if( bodyRotationModel == nullptr ) + { + throw std::runtime_error( "Error when creating reference point ephemeris, no body rotation model is provided" ); + } + + if( referencePointStateFunction == nullptr ) + { + throw std::runtime_error( "Error when creating reference point ephemeris, no reference point state function is provided" ); + } + // Create list of state/rotation functions that are to be used std::map< int, std::function< Eigen::Matrix< StateScalarType, 6, 1 >( const TimeType& ) > > stationEphemerisVector; stationEphemerisVector[ 2 ] = std::bind( &simulation_setup::Body::getStateInBaseFrameFromEphemeris diff --git a/include/tudat/simulation/estimation_setup/observationSimulationSettings.h b/include/tudat/simulation/estimation_setup/observationSimulationSettings.h index a366ecd412..b917165fec 100644 --- a/include/tudat/simulation/estimation_setup/observationSimulationSettings.h +++ b/include/tudat/simulation/estimation_setup/observationSimulationSettings.h @@ -192,7 +192,11 @@ inline std::shared_ptr< ObservationSimulationSettings< TimeType > > tabulatedObs template< typename TimeType = double > std::vector< std::shared_ptr< ObservationSimulationSettings< TimeType > > > createTabulatedObservationSimulationSettingsList( const std::map< observation_models::ObservableType, std::vector< observation_models::LinkEnds > > linkEndsPerObservable, - const std::vector< TimeType >& simulationTimes ) + const std::vector< TimeType >& simulationTimes, + const observation_models::LinkEndType linkEndType = observation_models::receiver, + const std::vector< std::shared_ptr< observation_models::ObservationViabilitySettings > >& viabilitySettingsList = + std::vector< std::shared_ptr< observation_models::ObservationViabilitySettings > >( ) + ) { std::vector< std::shared_ptr< ObservationSimulationSettings< TimeType > > > observationSimulationSettingsList; for( auto observableIterator : linkEndsPerObservable ) @@ -201,7 +205,8 @@ std::vector< std::shared_ptr< ObservationSimulationSettings< TimeType > > > crea { observationSimulationSettingsList.push_back( std::make_shared< TabulatedObservationSimulationSettings< TimeType > >( - observableIterator.first, observableIterator.second.at( i ), simulationTimes ) ); + observableIterator.first, observableIterator.second.at( i ), simulationTimes, linkEndType, + viabilitySettingsList) ); } } return observationSimulationSettingsList; diff --git a/tests/src/astro/electromagnetism/unitTestCannonBallRadiationPressureAccelerationAndForce.cpp b/tests/src/astro/electromagnetism/unitTestCannonBallRadiationPressureAccelerationAndForce.cpp index 4a04f5d15e..ee6f8c90c1 100644 --- a/tests/src/astro/electromagnetism/unitTestCannonBallRadiationPressureAccelerationAndForce.cpp +++ b/tests/src/astro/electromagnetism/unitTestCannonBallRadiationPressureAccelerationAndForce.cpp @@ -433,6 +433,8 @@ BOOST_AUTO_TEST_CASE( testRadiationPressureAccelerationModelClassConstructor ) const Eigen::Vector3d expectedRadiationPressureAcceleration = Eigen::Vector3d( -2.964e-06, 0.0, 0.0 ); + radiationPressureModel->updateMembers( 0.0 ); + // Compute radiation pressure acceleration [m/s^2]. const Eigen::Vector3d computedRadiationPressureAcceleration = radiationPressureModel->getAcceleration( ); @@ -453,6 +455,8 @@ BOOST_AUTO_TEST_CASE( testRadiationPressureAccelerationModelClassUpdateMembers ) &getRadiationPressureCoefficient, &getAreaSubjectToRadiationPressure, &getMassOfAcceleratedBody ); + radiationPressureModel->updateMembers( 0.0 ); + // Set expected radiation pressure acceleration [m/s^2]. const Eigen::Vector3d expectedRadiationPressureAcceleration = Eigen::Vector3d( -2.05147517201883e-05, @@ -482,7 +486,7 @@ BOOST_AUTO_TEST_CASE( testRadiationPressureAccelerationModelClassUpdateMembers ) massOfAcceleratedBody = 0.0022; // Update class members. - radiationPressureModel->updateMembers( ); + radiationPressureModel->updateMembers( 0.0 ); // Compute radiation pressure acceleration [m/s^2]. const Eigen::Vector3d computedRadiationPressureAcceleration diff --git a/tests/src/astro/electromagnetism/unitTestSolarSailAccelerationAndForce.cpp b/tests/src/astro/electromagnetism/unitTestSolarSailAccelerationAndForce.cpp index 9abeef546f..a3b5f5a1ce 100644 --- a/tests/src/astro/electromagnetism/unitTestSolarSailAccelerationAndForce.cpp +++ b/tests/src/astro/electromagnetism/unitTestSolarSailAccelerationAndForce.cpp @@ -589,6 +589,7 @@ BOOST_AUTO_TEST_CASE( testSolarSailAccelerationModelClassConstructor ) &getSourcePosition, &getAcceleratedBodyPosition, &getRadiationPressure, &getRadiationPressureCoefficient, &getAreaSubjectToRadiationPressure, &getMassOfAcceleratedBody ); + radiationPressureModel->updateMembers( 0.0 ); // Declare and initialize solar sail radiation acceleration model. electromagnetism::SolarSailAccelerationPointer solarSailModel @@ -599,6 +600,7 @@ BOOST_AUTO_TEST_CASE( testSolarSailAccelerationModelClassConstructor ) &getFrontLambertianCoefficient, &getBackLambertianCoefficient, &getReflectivityCoefficient, &getSpecularReflectionCoefficient, &getAreaSubjectToRadiationPressure, &getMassOfAcceleratedBody ); + solarSailModel->updateMembers( 0.0 ); // Compute solar sail acceleration [m/s^2]. const Eigen::Vector3d computedSolarSailAcceleration = solarSailModel->getAcceleration( ); @@ -622,6 +624,7 @@ BOOST_AUTO_TEST_CASE( testSolarSailAccelerationModelClassUpdateMembers ) &getSourcePosition, &getAcceleratedBodyPosition, &getRadiationPressure, &getRadiationPressureCoefficient, &getAreaSubjectToRadiationPressure, &getMassOfAcceleratedBody ); + radiationPressureModel->updateMembers( 0.0 ); // Declare and initialize solar sail radiation acceleration model. electromagnetism::SolarSailAccelerationPointer solarSailModel @@ -632,6 +635,7 @@ BOOST_AUTO_TEST_CASE( testSolarSailAccelerationModelClassUpdateMembers ) &getFrontLambertianCoefficient, &getBackLambertianCoefficient, &getReflectivityCoefficient, &getSpecularReflectionCoefficient, &getAreaSubjectToRadiationPressure, &getMassOfAcceleratedBody ); + solarSailModel->updateMembers( 0.0 ); // Set the distance from the Sun to Venus [m]. const double distanceSunToVenus = 0.732 * astronomicalUnitInMeters; @@ -658,8 +662,8 @@ BOOST_AUTO_TEST_CASE( testSolarSailAccelerationModelClassUpdateMembers ) massOfAcceleratedBody = 0.0022; // Update class members. - radiationPressureModel->updateMembers( ); - solarSailModel->updateMembers( ); + radiationPressureModel->updateMembers( 0.0 ); + solarSailModel->updateMembers( 0.0 ); // Compute radiation pressure acceleration [m/s^2]. const Eigen::Vector3d computedRadiationPressureAcceleration = radiationPressureModel->getAcceleration( ); @@ -874,6 +878,7 @@ BOOST_AUTO_TEST_CASE( testSolarSailAccelerationModelClassUpdateMembersIncludingC &getFrontLambertianCoefficient, &getBackLambertianCoefficient, &getReflectivityCoefficient, &getSpecularReflectionCoefficient, &getAreaSubjectToRadiationPressure, &getMassOfAcceleratedBody ); + solarSailModel->updateMembers( 0.0 ); // Set expected radiation pressure acceleration [m/s^2]. const Eigen::Vector3d expectedSolarSailAcceleration = Eigen::Vector3d( 8.59654650354177e-06, 7.44856903789465e-06, 6.83437564207585e-06 ); @@ -921,7 +926,7 @@ BOOST_AUTO_TEST_CASE( testSolarSailAccelerationModelClassUpdateMembersIncludingC massOfAcceleratedBody = 0.0022; // Update class members. - solarSailModel->updateMembers( ); + solarSailModel->updateMembers( 0.0 ); // Compute radiation pressure acceleration [m/s^2]. const Eigen::Vector3d computedSolarSailAcceleration = solarSailModel->getAcceleration( ); diff --git a/tests/src/astro/gravitation/unitTestSphericalHarmonicsGravityModel.cpp b/tests/src/astro/gravitation/unitTestSphericalHarmonicsGravityModel.cpp index 183a34d0fe..681da52ce9 100644 --- a/tests/src/astro/gravitation/unitTestSphericalHarmonicsGravityModel.cpp +++ b/tests/src/astro/gravitation/unitTestSphericalHarmonicsGravityModel.cpp @@ -286,6 +286,7 @@ BOOST_AUTO_TEST_CASE( test_SphericalHarmonicsGravitationalAccelerationWrapperCla = std::make_shared< SphericalHarmonicsGravitationalAccelerationModel >( [ & ]( Eigen::Vector3d& input ){ input = position; }, gravitationalParameter, planetaryRadius, cosineCoefficients, sineCoefficients ); + earthGravity->updateMembers( ); // Compute resultant acceleration [m s^-2]. const Eigen::Vector3d acceleration = earthGravity->getAcceleration( ); diff --git a/tests/src/astro/orbit_determination/acceleration_partials/unitTestAccelerationPartials.cpp b/tests/src/astro/orbit_determination/acceleration_partials/unitTestAccelerationPartials.cpp index cf90dcd1d2..f895159123 100644 --- a/tests/src/astro/orbit_determination/acceleration_partials/unitTestAccelerationPartials.cpp +++ b/tests/src/astro/orbit_determination/acceleration_partials/unitTestAccelerationPartials.cpp @@ -568,10 +568,14 @@ BOOST_AUTO_TEST_CASE( testAerodynamicAccelerationPartials ) vehicleSphericalEntryState( SphericalOrbitalStateElementIndices::headingAngleIndex ) = 0.6; // Convert vehicle state from spherical elements to Cartesian elements. - Eigen::Vector6d systemInitialState = convertSphericalOrbitalToCartesianState( - vehicleSphericalEntryState ); + Eigen::Vector6d systemInitialState = tudat::ephemerides::transformStateToTargetFrame( + convertSphericalOrbitalToCartesianState( + vehicleSphericalEntryState ), 0.0, bodies.at( "Earth" )->getRotationalEphemeris( ) ); + bodies.at( "Earth" )->setStateFromEphemeris( 0.0 ); + bodies.at( "Earth" )->setCurrentRotationToLocalFrameFromEphemeris( 0.0 ); + bodies.at( "Vehicle" )->setState( systemInitialState ); diff --git a/tests/src/astro/propagators/unitTestEnvironmentUpdater.cpp b/tests/src/astro/propagators/unitTestEnvironmentUpdater.cpp index d2384d1334..4263255273 100644 --- a/tests/src/astro/propagators/unitTestEnvironmentUpdater.cpp +++ b/tests/src/astro/propagators/unitTestEnvironmentUpdater.cpp @@ -162,9 +162,16 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) std::numeric_limits< double >::epsilon( ) ); // Test if Mars is not updated - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( - bodies.at( "Mars" )->getState( ), Eigen::Vector6d::Zero( ), - std::numeric_limits< double >::epsilon( ) ); + bool exceptionIsCaught = false; + try + { + bodies.at( "Mars" )->getState( ); + } + catch( ... ) + { + exceptionIsCaught = true; + } + BOOST_CHECK_EQUAL( exceptionIsCaught, true ); // Update environment to new time, and state from environment. updater->updateEnvironment( @@ -181,9 +188,6 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) bodies.at( "Moon" )->getState( ), bodies.at( "Moon" )->getEphemeris( )->getCartesianState( 0.5 * testTime ), std::numeric_limits< double >::epsilon( ) ); - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( - bodies.at( "Mars" )->getState( ), Eigen::Vector6d::Zero( ), - std::numeric_limits< double >::epsilon( ) ); } // Test third body acceleration updates. @@ -244,10 +248,18 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) bodies.at( "Mars" )->getEphemeris( )->getCartesianState( testTime ), std::numeric_limits< double >::epsilon( ) ); - // Test if Venus is not updated - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( - bodies.at( "Venus" )->getState( ), Eigen::Vector6d::Zero( ), - std::numeric_limits< double >::epsilon( ) ); + // Test if Mars is not updated + bool exceptionIsCaught = false; + try + { + bodies.at( "Venus" )->getState( ); + } + catch( ... ) + { + exceptionIsCaught = true; + } + BOOST_CHECK_EQUAL( exceptionIsCaught, true ); + // Update environment to new time, and state from environment. updater->updateEnvironment( @@ -318,10 +330,18 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) bodies.at( "Mars" )->getEphemeris( )->getCartesianState( testTime ), std::numeric_limits< double >::epsilon( ) ); - // Test if Venus is not updated - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( - bodies.at( "Venus" )->getState( ), Eigen::Vector6d::Zero( ), - std::numeric_limits< double >::epsilon( ) ); + + // Test if Mars is not updated + bool exceptionIsCaught = false; + try + { + bodies.at( "Venus" )->getState( ); + } + catch( ... ) + { + exceptionIsCaught = true; + } + BOOST_CHECK_EQUAL( exceptionIsCaught, true ); // Test if Earth rotation is updated TUDAT_CHECK_MATRIX_CLOSE_FRACTION( @@ -341,19 +361,29 @@ BOOST_AUTO_TEST_CASE( test_centralGravityEnvironmentUpdate ) bodies.at( "Earth" )->getRotationalEphemeris( )->getDerivativeOfRotationToTargetFrame( testTime ), std::numeric_limits< double >::epsilon( ) ); - // Test if Mars rotation is not updated - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( - bodies.at( "Mars" )->getCurrentRotationToLocalFrame( ).toRotationMatrix( ), - Eigen::Matrix3d::Identity( ), std::numeric_limits< double >::epsilon( ) ); - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( - bodies.at( "Mars" )->getCurrentRotationToGlobalFrame( ).toRotationMatrix( ), - Eigen::Matrix3d::Identity( ), std::numeric_limits< double >::epsilon( ) ); - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( - bodies.at( "Mars" )->getCurrentRotationMatrixDerivativeToGlobalFrame( ), - Eigen::Matrix3d::Zero( ), std::numeric_limits< double >::epsilon( ) ); - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( - bodies.at( "Mars" )->getCurrentRotationMatrixDerivativeToLocalFrame( ), - Eigen::Matrix3d::Zero( ), std::numeric_limits< double >::epsilon( ) ); + + // Test if Mars is not updated + exceptionIsCaught = false; + try + { + bodies.at( "Mars" )->getCurrentRotationToLocalFrame( ).toRotationMatrix( ); + } + catch( ... ) + { + exceptionIsCaught = true; + } + BOOST_CHECK_EQUAL( exceptionIsCaught, true ); + + exceptionIsCaught = false; + try + { + bodies.at( "Mars" )->getCurrentRotationMatrixDerivativeToGlobalFrame( ); + } + catch( ... ) + { + exceptionIsCaught = true; + } + BOOST_CHECK_EQUAL( exceptionIsCaught, true ); // Update environment to new time, and state from environment. updater->updateEnvironment( diff --git a/tests/src/simulation/unitTestAccelerationModelSetup.cpp b/tests/src/simulation/unitTestAccelerationModelSetup.cpp index cf0efd69a1..552d6ff7a1 100644 --- a/tests/src/simulation/unitTestAccelerationModelSetup.cpp +++ b/tests/src/simulation/unitTestAccelerationModelSetup.cpp @@ -194,10 +194,14 @@ BOOST_AUTO_TEST_CASE( test_shGravityModelSetup ) Eigen::Vector6d dummyEarthState = ( Eigen::Vector6d ( ) << 1.1E11, 0.5E11, 0.01E11, 0.0, 0.0, 0.0 ).finished( ); + Eigen::Vector7d unitRotationalState = Eigen::Vector7d::Zero( ); + unitRotationalState.segment( 0, 4 ) = linear_algebra::convertQuaternionToVectorFormat( + Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ) ); bodies.at( "Earth" )->setState( dummyEarthState ); bodies.at( "Vehicle" )->setState( ( Eigen::Vector6d ( ) << 7.0e6, 8.0e6, 9.0e6, 0.0, 0.0, 0.0 ).finished( ) + dummyEarthState ); + bodies.at( "Earth" )->setCurrentRotationalStateToLocalFrame( unitRotationalState ); // Define Earth gravity field. double gravitationalParameter = 3.986004418e14; diff --git a/tests/src/simulation/unitTestEnvironmentModelSetup.cpp b/tests/src/simulation/unitTestEnvironmentModelSetup.cpp index 1da24689ac..84bb90e522 100644 --- a/tests/src/simulation/unitTestEnvironmentModelSetup.cpp +++ b/tests/src/simulation/unitTestEnvironmentModelSetup.cpp @@ -1605,6 +1605,8 @@ BOOST_AUTO_TEST_CASE( test_panelledRadiationPressureInterfaceSetup ) initialKeplerElements, 0.0, spice_interface::getBodyGravitationalParameter( "Earth" ), "Earth", "ECLIPJ2000" ); + + // Create radiation pressure properties std::vector< double > areas; areas.push_back( 4.0 ); @@ -1659,6 +1661,11 @@ BOOST_AUTO_TEST_CASE( test_panelledRadiationPressureInterfaceSetup ) SystemOfBodies bodies = createSystemOfBodies( bodySettings ); + Eigen::Vector7d unitRotationalState = Eigen::Vector7d::Zero( ); + unitRotationalState.segment( 0, 4 ) = linear_algebra::convertQuaternionToVectorFormat( + Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ) ); + bodies.at( "Vehicle" )->setCurrentRotationalStateToLocalFrame( unitRotationalState ); + BOOST_CHECK_EQUAL( bodies.at( "Vehicle" )->getRadiationPressureInterfaces( ).size( ), 1 ); BOOST_CHECK_EQUAL( bodies.at( "Vehicle" )->getRadiationPressureInterfaces( ).count( "Sun" ), 1 );