Skip to content

Commit

Permalink
Merged and updated to long double compatibility
Browse files Browse the repository at this point in the history
  • Loading branch information
@DominicDirkx
DominicDirkx committed Nov 5, 2024
2 parents 33a4b3f + c511dc4 commit f5d0c94
Show file tree
Hide file tree
Showing 6 changed files with 171 additions and 27 deletions.
21 changes: 15 additions & 6 deletions include/tudat/astro/ground_stations/transmittingFrequencies.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -250,13 +250,22 @@ class PiecewiseLinearFrequencyInterpolator: public StationFrequencyInterpolator
static_cast< double >( lookupTime ) ) + ", caught exception: " + exceptionText );
}

if( lookupTime > endTimes_.at( lowerNearestNeighbour ) || lookupTime < startTimes_.at ( lowerNearestNeighbour ) )

// if( lookupTime > endTimes_.at( lowerNearestNeighbour ) || lookupTime < startTimes_.at ( lowerNearestNeighbour ) )
// {
// throw std::runtime_error(
// "Error when interpolating ramp reference frequency: look up time (" + std::to_string(
// static_cast< double >( lookupTime ) ) +
// ") is outside the ramp table interval (" + std::to_string( double( startTimes_.at( 0 ) ) ) + " to " +
// std::to_string( double( startTimes_.back( ) ) ) + ")." );
// }
if ( lookupTime > endTimes_.at( lowerNearestNeighbour ) )
{
throw std::runtime_error(
"Error when interpolating ramp reference frequency: look up time (" + std::to_string(
static_cast< double >( lookupTime ) ) +
") is outside the ramp table interval (" + std::to_string( double( startTimes_.at( 0 ) ) ) + " to " +
std::to_string( double( startTimes_.back( ) ) ) + ")." );
lowerNearestNeighbour = endTimes_.size( ) - 1;
}
if ( lookupTime < startTimes_.at ( lowerNearestNeighbour ) )
{
lowerNearestNeighbour = 0;
}
else if ( invalidStartTimeLookupScheme_ != nullptr )
{
Expand Down
12 changes: 6 additions & 6 deletions include/tudat/astro/propagators/getZeroProperModeRotationalInitialState.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,7 @@ void integrateForwardWithDissipationAndBackwardsWithout(
* \param outputFolder Directory to which files will be written, if writeToFileInLoop is true
*/
template< typename TimeType, typename StateScalarType >
Eigen::VectorXd getZeroProperModeRotationalState(
Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > getZeroProperModeRotationalState(
const simulation_setup::SystemOfBodies& bodies,
const std::shared_ptr< SingleArcPropagatorSettings< StateScalarType, TimeType > > propagatorSettings,
const double bodyMeanRotationRate,
Expand Down Expand Up @@ -298,7 +298,7 @@ Eigen::VectorXd getZeroProperModeRotationalState(
template< typename TimeType = double, typename StateScalarType = double >
struct DampedInitialRotationalStateResults
{
Eigen::VectorXd initialState_;
Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > initialState_;
std::vector< std::pair< std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >,
std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > > > forwardBackwardPropagatedStates_;
std::vector< std::pair< std::map< TimeType, Eigen::Matrix< double, Eigen::Dynamic, 1 > >,
Expand All @@ -307,7 +307,7 @@ struct DampedInitialRotationalStateResults

template< typename TimeType = double, typename StateScalarType = double >
std::tuple<
Eigen::VectorXd,
Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >,
std::vector< std::pair< std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >,
std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > > >,
std::vector< std::pair< std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >,
Expand All @@ -324,7 +324,7 @@ std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > > >
std::vector< std::pair< std::map< TimeType, Eigen::Matrix< double, Eigen::Dynamic, 1 > >,
std::map< TimeType, Eigen::Matrix< double, Eigen::Dynamic, 1 > > > > dependentVariables;

Eigen::VectorXd initialState =
Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > initialState =
getZeroProperModeRotationalState(
bodies, propagatorSettings, bodyMeanRotationRate, dissipationTimes,
propagatedStates, dependentVariables,
Expand All @@ -350,7 +350,7 @@ std::shared_ptr< DampedInitialRotationalStateResults< TimeType, StateScalarType
std::vector< std::pair< std::map< TimeType, Eigen::Matrix< double, Eigen::Dynamic, 1 > >,
std::map< TimeType, Eigen::Matrix< double, Eigen::Dynamic, 1 > > > > dependentVariables;

Eigen::VectorXd initialState =
Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > initialState =
getZeroProperModeRotationalState(
bodies, propagatorSettings, bodyMeanRotationRate, dissipationTimes,
propagatedStates, dependentVariables,
Expand Down Expand Up @@ -378,7 +378,7 @@ std::shared_ptr< DampedInitialRotationalStateResults< TimeType, StateScalarType
*
*/
template< typename TimeType = double, typename StateScalarType = double >
Eigen::VectorXd getZeroProperModeRotationalState(
Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > getZeroProperModeRotationalState(
const simulation_setup::SystemOfBodies& bodies,
const std::shared_ptr< SingleArcPropagatorSettings< StateScalarType, TimeType > > propagatorSettings,
const double bodyMeanRotationRate,
Expand Down
137 changes: 128 additions & 9 deletions include/tudat/simulation/estimation_setup/observations.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -517,6 +517,36 @@ class SingleObservationSet
return residuals_.at( index );
}

Eigen::VectorXd getRmsResiduals( )
{
Eigen::VectorXd rmsResiduals = Eigen::VectorXd::Zero( singleObservationSize_ );
for ( unsigned int i = 0 ; i < singleObservationSize_ ; i++ )
{
// Calculate RMS of the residuals for each observation component
for( int j = 0; j < numberOfObservations_ ; j++ )
{
rmsResiduals[ i ] += residuals_[ j ]( i, 0 ) * residuals_[ j ]( i, 0 );
}
rmsResiduals[ i ] = std::sqrt( rmsResiduals[ i ] / numberOfObservations_ );
}

return rmsResiduals;
}

Eigen::VectorXd getMeanResiduals( )
{
Eigen::VectorXd meanResiduals = Eigen::VectorXd::Zero( singleObservationSize_ );
for ( unsigned int i = 0 ; i < singleObservationSize_ ; i++ )
{
// Calculate mean residual for each observation component
for ( unsigned int j = 0 ; j < numberOfObservations_ ; j++ )
{
meanResiduals[ i ] += residuals_[ j ]( i, 0 );
}
meanResiduals[ i ] /= numberOfObservations_;
}
return meanResiduals;
}

void setConstantWeight( const double weight )
{
Expand Down Expand Up @@ -884,6 +914,10 @@ class SingleObservationSet
void addDependentVariables( const std::vector< std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > > dependentVariableSettings,
const simulation_setup::SystemOfBodies& bodies )
{
if ( dependentVariableCalculator_ == nullptr )
{
dependentVariableCalculator_ = std::make_shared< ObservationDependentVariableCalculator >( observableType_, linkEnds_.linkEnds_ );
}
dependentVariableCalculator_->addDependentVariables( dependentVariableSettings, bodies );
}

Expand Down Expand Up @@ -1057,7 +1091,7 @@ class SingleObservationSet

std::vector< Eigen::VectorXd > observationsDependentVariables_;

const std::shared_ptr< simulation_setup::ObservationDependentVariableCalculator > dependentVariableCalculator_;
std::shared_ptr< simulation_setup::ObservationDependentVariableCalculator > dependentVariableCalculator_;

const std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > ancilliarySettings_;

Expand Down Expand Up @@ -1429,11 +1463,51 @@ class ObservationCollection
*std::max_element( concatenatedTimes_.begin( ), concatenatedTimes_.end( ) ) );
}

std::vector< std::pair< TimeType, TimeType > > getTimeBoundsPerSet(
std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const
{
std::vector< std::pair< TimeType, TimeType > > timeBounds;
for ( auto observableIt : getSingleObservationSetsIndices( observationParser ) )
{
for ( auto linkEndsIt : observableIt.second )
{
for ( auto index : linkEndsIt.second )
{
timeBounds.push_back( observationSetList_.at( observableIt.first ).at( linkEndsIt.first ).at( index )->getTimeBounds( ) );
}
}
}
return timeBounds;
}

std::vector< int > getConcatenatedLinkEndIds( )
{
return concatenatedLinkEndIds_;
}

std::vector< int > getConcatenatedLinkEndIds( std::shared_ptr< ObservationCollectionParser > observationParser )
{
std::vector< int > subsetConcatenatedLinkEndIds;
std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > observationSetsIndices = getSingleObservationSetsIndices( observationParser );

for ( auto obsIt : observationSetsIndices )
{
for ( auto linkEndsIt : obsIt.second )
{
std::vector< unsigned int > listRelevantSetIndices = linkEndsIt.second;
for ( auto index : listRelevantSetIndices )
{
std::pair< int, int > singleSetStartAndSize = observationSetStartAndSize_.at( obsIt.first ).at( linkEndsIt.first ).at( index );
for ( unsigned int k = singleSetStartAndSize.first ; k < ( singleSetStartAndSize.first + singleSetStartAndSize.second ) ; k++ )
{
subsetConcatenatedLinkEndIds.push_back( concatenatedLinkEndIds_[ k ] );
}
}
}
}
return subsetConcatenatedLinkEndIds;
}

std::map< observation_models::LinkEnds, int > getLinkEndIdentifierMap( )
{
return linkEndIds_;
Expand Down Expand Up @@ -1737,6 +1811,12 @@ class ObservationCollection
return concatenatedObservationsTimes;
}

std::vector< double > getConcatenatedDoubleObservationTimes(
std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) )
{
return utilities::staticCastVector< double, TimeType >( getConcatenatedObservationTimes( observationParser ) );
}

std::pair< std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >, std::vector< std::vector< TimeType > > > getObservationsAndTimes(
std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) )
{
Expand Down Expand Up @@ -1836,10 +1916,48 @@ class ObservationCollection
return concatenatedResiduals;
}

std::vector< Eigen::VectorXd > getComputedObservations(
std::vector< Eigen::VectorXd > getRmsResiduals(
std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const
{
std::vector< Eigen::VectorXd > computedObservations;
std::vector< Eigen::VectorXd > rmsResiduals;

std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > observationSetsIndices = getSingleObservationSetsIndices( observationParser );
for ( auto observableIt : observationSetsIndices )
{
for ( auto linkEndsIt : observableIt.second )
{
for ( auto index : linkEndsIt.second )
{
rmsResiduals.push_back( observationSetList_.at( observableIt.first ).at( linkEndsIt.first ).at( index )->getRmsResiduals( ) );
}
}
}
return rmsResiduals;
}

std::vector< Eigen::VectorXd > getMeanResiduals(
std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const
{
std::vector< Eigen::VectorXd > meanResiduals;

std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > observationSetsIndices = getSingleObservationSetsIndices( observationParser );
for ( auto observableIt : observationSetsIndices )
{
for ( auto linkEndsIt : observableIt.second )
{
for ( auto index : linkEndsIt.second )
{
meanResiduals.push_back( observationSetList_.at( observableIt.first ).at( linkEndsIt.first ).at( index )->getMeanResiduals( ) );
}
}
}
return meanResiduals;
}

std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > getComputedObservations(
std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const
{
std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > computedObservations;

std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > observationSetsIndices = getSingleObservationSetsIndices( observationParser );
for ( auto observableIt : observationSetsIndices )
Expand All @@ -1855,18 +1973,19 @@ class ObservationCollection
return computedObservations;
}

Eigen::VectorXd getConcatenatedComputedObservations(
Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > getConcatenatedComputedObservations(
std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const
{
std::vector< Eigen::VectorXd > computedObservations = getComputedObservations( observationParser );
std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > computedObservations = getComputedObservations( observationParser );

unsigned int totalObsSize = 0;
for ( auto obs : computedObservations )
{
totalObsSize += obs.size( );
}

Eigen::VectorXd concatenatedComputedObservations = Eigen::VectorXd::Zero( totalObsSize );
Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > concatenatedComputedObservations =
Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( totalObsSize );
unsigned int obsIndex = 0;
for ( unsigned int k = 0 ; k < computedObservations.size( ) ; k++ )
{
Expand Down Expand Up @@ -2214,7 +2333,7 @@ class ObservationCollection
unsigned int counterRemovedSets = 0;
for ( auto indexToRemove : linkEndsIt.second )
{
if ( indexToRemove > observationSetList_.at( observableIt.first ).at( linkEndsIt.first ).size( ) - 1 )
if ( indexToRemove - counterRemovedSets > observationSetList_.at( observableIt.first ).at( linkEndsIt.first ).size( ) - 1 )
{
throw std::runtime_error( "Error when removing single observation sets, set index exceeds number of observation sets"
" for given observation type and link ends." );
Expand Down Expand Up @@ -3095,7 +3214,7 @@ class ObservationCollection
}

// Concatenate dependent variables over all single observation sets
Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > concatenatedDependentVariables = Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( dependentVariablesRows, dependentVariableSize );
Eigen::MatrixXd concatenatedDependentVariables = Eigen::MatrixXd::Zero( dependentVariablesRows, dependentVariableSize );
unsigned int index = 0;
for ( unsigned int k = 0 ; k < dependentVariables.first.size( ) ; k++ )
{
Expand Down Expand Up @@ -3460,7 +3579,7 @@ inline std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType >
{
return std::make_shared< SingleObservationSet< ObservationScalarType, TimeType > >(
observableType, linkEnds, observations, observationTimes, referenceLinkEnd,
std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >( ), nullptr, ancilliarySettings );
std::vector< Eigen::VectorXd >( ), nullptr, ancilliarySettings );
}

//template< typename ObservationScalarType = double, typename TimeType = double,
Expand Down
7 changes: 7 additions & 0 deletions include/tudat/simulation/estimation_setup/observationsProcessing.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,6 +172,13 @@ inline std::shared_ptr< ObservationFilterBase > observationFilter(
return std::make_shared< ObservationFilter< std::pair< double, double > > >( filterType, filterValue, filterOut, useOppositeCondition );
}

inline std::shared_ptr< ObservationFilterBase > observationFilter(
const ObservationFilterType filterType, const double firstFilterValue, const double secondFilterValue, const bool filterOut = true, const bool useOppositeCondition = false )
{
std::pair< double, double > filterValue = std::make_pair( firstFilterValue, secondFilterValue );
return std::make_shared< ObservationFilter< std::pair< double, double > > >( filterType, filterValue, filterOut, useOppositeCondition );
}

inline std::shared_ptr< ObservationFilterBase > observationFilter(
const ObservationFilterType filterType, const Eigen::VectorXd& filterValue, const bool filterOut = true, const bool useOppositeCondition = false )
{
Expand Down
18 changes: 15 additions & 3 deletions src/astro/observation_models/corrections/atmosphereCorrection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -475,10 +475,22 @@ double TabulatedIonosphericCorrection::calculateLightTimeCorrectionWithMultiLegL
std::string( caughtException.what( ) ) );
}

// Convert times from TDB TO UTC. To speed up the conversion, we actually convert from TT to UTC. This approximation should be accurate enough for ionospheric delay computation.
double firstLegTransmissionTimeUtc = sofa_interface::convertTTtoUTC( firstLegTransmissionTime );
double stationTimeUtc = sofa_interface::convertTTtoUTC( stationTime );

// Compute light-time correction
double lightTimeCorrection = 0.0;
double transmittedFrequency = transmittedFrequencyFunction_( frequencyBands, firstLegTransmissionTimeUtc );
if ( !std::isnan( transmittedFrequency ) )
{
lightTimeCorrection = ( sign_ * referenceCorrectionCalculator_->computeMediaCorrection( stationTimeUtc ) *
std::pow( referenceFrequency_ / transmittedFrequencyFunction_( frequencyBands, firstLegTransmissionTimeUtc ), 2.0 ) ) /
physical_constants::getSpeedOfLight< double >( );
}

// Moyer (2000), eq. 10-11
return ( sign_ * referenceCorrectionCalculator_->computeMediaCorrection( stationTime ) *
std::pow( referenceFrequency_ / transmittedFrequencyFunction_( frequencyBands, firstLegTransmissionTime ), 2.0 ) ) /
physical_constants::getSpeedOfLight< double >( );
return lightTimeCorrection;
}

double JakowskiVtecCalculator::calculateVtec( const double time,
Expand Down
3 changes: 0 additions & 3 deletions src/simulation/estimation_setup/observationOutput.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -600,9 +600,6 @@ void ObservationDependentVariableCalculator::addDependentVariables(
const std::vector< std::shared_ptr< ObservationDependentVariableSettings > > settingsList,
const SystemOfBodies& bodies )
{
// Retrieve existing dependent variable settings
std::vector< std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > > existingSettingsList = settingsList_;

// Parse all settings to be added and check if they are already included
for ( auto settingsToAdd : settingsList )
{
Expand Down

0 comments on commit f5d0c94

Please sign in to comment.