From ef22038033516e347cf98534e57f7a25afa07fa5 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Mon, 9 Dec 2024 20:22:10 +0100 Subject: [PATCH 01/24] Reworked 2-way Doppler test --- .../observation_models/lightTimeSolution.h | 1 + .../oneWayDopplerObservationModel.h | 44 +++++++---- .../twoWayDopplerObservationModel.h | 70 +++++++---------- .../estimation_setup/createObservationModel.h | 77 +++++++++++++------ ...pplerMeasuredFrequencyObservationModel.cpp | 2 +- .../unitTestDopplerModels.cpp | 2 +- 6 files changed, 111 insertions(+), 85 deletions(-) diff --git a/include/tudat/astro/observation_models/lightTimeSolution.h b/include/tudat/astro/observation_models/lightTimeSolution.h index 6ca5d2840e..acb163a286 100644 --- a/include/tudat/astro/observation_models/lightTimeSolution.h +++ b/include/tudat/astro/observation_models/lightTimeSolution.h @@ -1021,6 +1021,7 @@ class MultiLegLightTimeCalculator const std::shared_ptr< ObservationAncilliarySimulationSettings > ancillarySettings = nullptr, bool computeLightTimeCorrections = true ) { + // Define objects to keep light times ObservationScalarType totalLightTime = 0.0; ObservationScalarType currentLightTime; diff --git a/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h b/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h index 550d9835aa..f62c7b7b71 100644 --- a/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h +++ b/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h @@ -104,10 +104,10 @@ ObservationScalarType calculateLineOfSightVelocityAsCFractionFromReceiverStateFu */ template< typename ObservationScalarType = double > ObservationScalarType computeOneWayFirstOrderDopplerTaylorSeriesExpansion( - Eigen::Matrix< ObservationScalarType, 6, 1 >& transmitterState, - Eigen::Matrix< ObservationScalarType, 6, 1 >& receiverState, - Eigen::Matrix< ObservationScalarType, 1, 3 >& lightTimeWrtTransmitterPositionPartial, - Eigen::Matrix< ObservationScalarType, 1, 3 >& lightTimeWrtReceiverPositionPartial, + const Eigen::Matrix< ObservationScalarType, 6, 1 >& transmitterState, + const Eigen::Matrix< ObservationScalarType, 6, 1 >& receiverState, + const Eigen::Matrix< ObservationScalarType, 1, 3 >& lightTimeWrtTransmitterPositionPartial, + const Eigen::Matrix< ObservationScalarType, 1, 3 >& lightTimeWrtReceiverPositionPartial, const int taylorSeriesOrder ) { // Compute projected velocity components @@ -539,29 +539,24 @@ class OneWayDopplerObservationModel: public ObservationModel< 1, ObservationScal TimeType transmissionTime = TUDAT_NAN, receptionTime = TUDAT_NAN; // Compute light time - switch( linkEndAssociatedWithTime ) + switch ( linkEndAssociatedWithTime ) { case receiver: lightTime = lightTimeCalculator_->calculateLightTimeWithLinkEndsStates( - receiverState_, transmitterState_, time, true, ancilliarySetings ); + receiverState_, transmitterState_, time, true, ancilliarySetings ); transmissionTime = time - lightTime; receptionTime = time; break; case transmitter: lightTime = lightTimeCalculator_->calculateLightTimeWithLinkEndsStates( - receiverState_, transmitterState_, time, false, ancilliarySetings ); + receiverState_, transmitterState_, time, false, ancilliarySetings ); transmissionTime = time; receptionTime = time + lightTime; break; default: throw std::runtime_error( - "Error when calculating one way Doppler observation, link end is not transmitter or receiver" ); - } - - if( ancilliarySetings != nullptr ) - { - throw std::runtime_error( "Error, calling one-way Doppler observable with ancilliary settings, but none are supported." ); + "Error when calculating one way Doppler observation, link end is not transmitter or receiver" ); } linkEndTimes.clear( ); @@ -574,6 +569,21 @@ class OneWayDopplerObservationModel: public ObservationModel< 1, ObservationScal linkEndStates.push_back( transmitterState_.template cast< double >( ) ); linkEndStates.push_back( receiverState_.template cast< double >( ) ); + return computeIdealDopplerWithLinkEndData( + linkEndAssociatedWithTime, linkEndTimes, linkEndStates, ancilliarySetings ); + } + + Eigen::Matrix< ObservationScalarType, 1, 1 > computeIdealDopplerWithLinkEndData( + const LinkEndType linkEndAssociatedWithTime, + const std::vector< double >& linkEndTimes, + const std::vector< Eigen::Matrix< double, 6, 1 > >& linkEndStates, + const std::shared_ptr< ObservationAncilliarySimulationSettings > ancilliarySetings = nullptr ) + { + if( ancilliarySetings != nullptr ) + { + throw std::runtime_error( "Error, calling one-way Doppler observable with ancilliary settings, but none are supported." ); + } + // Compute transmitter and receiver proper time rate ObservationScalarType transmitterProperTimeDifference = mathematical_constants::getFloatingInteger< ObservationScalarType >( 0 ); @@ -598,17 +608,17 @@ class OneWayDopplerObservationModel: public ObservationModel< 1, ObservationScal transmitterProperTimeDifference, receiverProperTimeDifference, taylorSeriesExpansionOrder_ ); - // Compute first-order (geometrical) one-way Doppler contribution + // Compute first-order (geometrical) one-way Doppler contribution lightTimePartialWrtReceiverPosition_ = lightTimeCalculator_->getPartialOfLightTimeWrtLinkEndPosition( - transmitterState_, receiverState_, transmissionTime, receptionTime, true ); + linkEndStates.at( 0 ), linkEndStates.at( 1 ), linkEndTimes.at( 0 ), linkEndTimes.at( 1 ), true ); lightTimePartialWrtTransmitterPosition_ = lightTimeCalculator_->getPartialOfLightTimeWrtLinkEndPosition( - transmitterState_, receiverState_, transmissionTime, receptionTime, false ); + linkEndStates.at( 0 ), linkEndStates.at( 1 ), linkEndTimes.at( 0 ), linkEndTimes.at( 1 ), false ); ObservationScalarType firstOrderDopplerObservable = computeOneWayFirstOrderDopplerTaylorSeriesExpansion< ObservationScalarType >( - transmitterState_, receiverState_, + linkEndStates.at( 0 ), linkEndStates.at( 1 ), lightTimePartialWrtTransmitterPosition_, lightTimePartialWrtReceiverPosition_, taylorSeriesExpansionOrder_ ); diff --git a/include/tudat/astro/observation_models/twoWayDopplerObservationModel.h b/include/tudat/astro/observation_models/twoWayDopplerObservationModel.h index 54d390ea2d..75b88fd178 100644 --- a/include/tudat/astro/observation_models/twoWayDopplerObservationModel.h +++ b/include/tudat/astro/observation_models/twoWayDopplerObservationModel.h @@ -46,6 +46,8 @@ class TwoWayDopplerObservationModel: public ObservationModel< 1, ObservationScal */ TwoWayDopplerObservationModel( const LinkEnds& linkEnds, + const std::shared_ptr< observation_models::MultiLegLightTimeCalculator< ObservationScalarType, TimeType > > + multiLegLightTimeCalculator, const std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > uplinkDopplerCalculator, const std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > @@ -53,10 +55,10 @@ class TwoWayDopplerObservationModel: public ObservationModel< 1, ObservationScal const std::shared_ptr< ObservationBias< 1 > > observationBiasCalculator = nullptr, const bool normalizeWithSpeedOfLight = false ): ObservationModel< 1, ObservationScalarType, TimeType >( two_way_doppler, linkEnds, observationBiasCalculator ), - uplinkDopplerCalculator_( uplinkDopplerCalculator ), - downlinkDopplerCalculator_( downlinkDopplerCalculator ) + multiLegLightTimeCalculator_( multiLegLightTimeCalculator ), + uplinkDopplerCalculator_( uplinkDopplerCalculator ), + downlinkDopplerCalculator_( downlinkDopplerCalculator ) { -// std::cout<<"Normalize: "<setNormalizeWithSpeedOfLight( true ); downlinkDopplerCalculator_->setNormalizeWithSpeedOfLight( true ); @@ -87,58 +89,44 @@ class TwoWayDopplerObservationModel: public ObservationModel< 1, ObservationScal std::vector< Eigen::Matrix< double, 6, 1 > >& linkEndStates, const std::shared_ptr< ObservationAncilliarySimulationSettings > ancilliarySetings = nullptr ) { - std::vector< double > uplinkLinkEndTimes; - std::vector< Eigen::Matrix< double, 6, 1 > > uplinkLinkEndStates; + linkEndTimes.clear( ); + linkEndStates.clear( ); + multiLegLightTimeCalculator_->calculateLightTimeWithLinkEndsStates( + time, linkEndAssociatedWithTime, linkEndTimes, linkEndStates, ancilliarySetings ); - std::vector< double > downlinkLinkEndTimes; - std::vector< Eigen::Matrix< double, 6, 1 > > downlinkLinkEndStates; + std::vector< double > uplinkLinkEndTimes = { linkEndTimes.at( 0 ), linkEndTimes.at( 1 ) }; + std::vector< Eigen::Matrix< double, 6, 1 > > uplinkLinkEndStates = { linkEndStates.at( 0 ), linkEndStates.at( 1 ) }; + + std::vector< double > downlinkLinkEndTimes = { linkEndTimes.at( 2 ), linkEndTimes.at( 3 ) }; + std::vector< Eigen::Matrix< double, 6, 1 > > downlinkLinkEndStates = { linkEndStates.at( 2 ), linkEndStates.at( 3 ) }; Eigen::Matrix< ObservationScalarType, 1, 1 > uplinkDoppler, downlinkDoppler; + LinkEndType uplinkReferenceLinkEnd, downlinkReferenceLinkEnd; switch( linkEndAssociatedWithTime ) { case receiver: - - downlinkDoppler = downlinkDopplerCalculator_->computeIdealObservationsWithLinkEndData( - time, receiver, downlinkLinkEndTimes, downlinkLinkEndStates ); - uplinkDoppler = uplinkDopplerCalculator_->computeIdealObservationsWithLinkEndData( - downlinkLinkEndTimes.at( 0 ), receiver, uplinkLinkEndTimes, uplinkLinkEndStates ); - + uplinkReferenceLinkEnd = receiver; + downlinkReferenceLinkEnd = receiver; break; case reflector1: - - uplinkDoppler = uplinkDopplerCalculator_->computeIdealObservationsWithLinkEndData( - time, receiver, uplinkLinkEndTimes, uplinkLinkEndStates ); - downlinkDoppler = downlinkDopplerCalculator_->computeIdealObservationsWithLinkEndData( - time, transmitter, downlinkLinkEndTimes, downlinkLinkEndStates ); - + uplinkReferenceLinkEnd = receiver; + downlinkReferenceLinkEnd = transmitter; break; case transmitter: - uplinkDoppler = uplinkDopplerCalculator_->computeIdealObservationsWithLinkEndData( - time, transmitter, uplinkLinkEndTimes, uplinkLinkEndStates ); - downlinkDoppler = downlinkDopplerCalculator_->computeIdealObservationsWithLinkEndData( - uplinkLinkEndTimes.at( 1 ), transmitter, downlinkLinkEndTimes, downlinkLinkEndStates ); + uplinkReferenceLinkEnd = transmitter; + downlinkReferenceLinkEnd = transmitter; break; default: throw std::runtime_error( "Error when calculating two way Doppler observation, link end is not transmitter or receiver" ); } - linkEndTimes.clear( ); - linkEndStates.clear( ); - - linkEndTimes.resize( 4 ); - linkEndStates.resize( 4 ); - - linkEndTimes[ 0 ] = uplinkLinkEndTimes.at( 0 ); - linkEndTimes[ 1 ] = uplinkLinkEndTimes.at( 1 ); - linkEndTimes[ 2 ] = downlinkLinkEndTimes.at( 0 ); - linkEndTimes[ 3 ] = downlinkLinkEndTimes.at( 1 ); + uplinkDoppler = uplinkDopplerCalculator_->computeIdealDopplerWithLinkEndData( + transmitter, uplinkLinkEndTimes, uplinkLinkEndStates ); + downlinkDoppler = downlinkDopplerCalculator_->computeIdealDopplerWithLinkEndData( + transmitter, downlinkLinkEndTimes, downlinkLinkEndStates ); - linkEndStates[ 0 ] = uplinkLinkEndStates.at( 0 ); - linkEndStates[ 1 ] = uplinkLinkEndStates.at( 1 ); - linkEndStates[ 2 ] = downlinkLinkEndStates.at( 0 ); - linkEndStates[ 3 ] = downlinkLinkEndStates.at( 1 ); return multiplicationTerm_ * ( Eigen::Matrix< ObservationScalarType, 1, 1 >( ) << downlinkDoppler( 0 ) * uplinkDoppler( 0 ) + downlinkDoppler( 0 ) + uplinkDoppler( 0 ) ).finished( ); @@ -185,13 +173,13 @@ class TwoWayDopplerObservationModel: public ObservationModel< 1, ObservationScal private: + std::shared_ptr< observation_models::MultiLegLightTimeCalculator< ObservationScalarType, TimeType > > multiLegLightTimeCalculator_; + //! Object that computes the one-way Doppler observable for the uplink - std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > - uplinkDopplerCalculator_; + std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > uplinkDopplerCalculator_; //! Object that computes the one-way Doppler observable for the downlink - std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > - downlinkDopplerCalculator_; + std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > downlinkDopplerCalculator_; ObservationScalarType multiplicationTerm_; diff --git a/include/tudat/simulation/estimation_setup/createObservationModel.h b/include/tudat/simulation/estimation_setup/createObservationModel.h index 992d487eea..35266ca113 100644 --- a/include/tudat/simulation/estimation_setup/createObservationModel.h +++ b/include/tudat/simulation/estimation_setup/createObservationModel.h @@ -713,7 +713,7 @@ class TwoWayDopplerObservationSettings: public ObservationModelSettings const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr ): ObservationModelSettings( two_way_doppler, mergeUpDownLink( uplinkOneWayDopplerSettings->linkEnds_, downlinkOneWayDopplerSettings->linkEnds_ ), - std::shared_ptr< LightTimeCorrectionSettings >( ), biasSettings, nullptr ), + std::shared_ptr< LightTimeCorrectionSettings >( ), biasSettings, std::make_shared< LightTimeConvergenceCriteria >( ) ), uplinkOneWayDopplerSettings_( uplinkOneWayDopplerSettings ), downlinkOneWayDopplerSettings_( downlinkOneWayDopplerSettings ) { @@ -1926,35 +1926,62 @@ class ObservationModelCreator< 1, ObservationScalarType, TimeType > std::shared_ptr< TwoWayDopplerObservationSettings > twoWayDopplerSettings = std::dynamic_pointer_cast< TwoWayDopplerObservationSettings >( observationSettings ); - - if( twoWayDopplerSettings == nullptr ) + // Create vector of convergence criteria and light time corrections + std::vector< std::shared_ptr< LightTimeConvergenceCriteria > > singleLegsLightTimeConvergenceCriteriaList; + std::shared_ptr< LightTimeConvergenceCriteria > multiLegLightTimeConvergenceCriteria = + observationSettings->lightTimeConvergenceCriteria_; + std::vector< std::vector< std::shared_ptr< LightTimeCorrectionSettings > > > lightTimeCorrectionsList; + if ( twoWayDopplerSettings != nullptr ) { - observationModel = std::make_shared< TwoWayDopplerObservationModel< - ObservationScalarType, TimeType > >( - linkEnds, - std::dynamic_pointer_cast< OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( - ObservationModelCreator< 1, ObservationScalarType, TimeType >::createObservationModel( - std::make_shared< ObservationModelSettings >( - one_way_doppler, uplinkLinkEnds, observationSettings->lightTimeCorrectionsList_ ), bodies, topLevelObservableType ) ), - std::dynamic_pointer_cast< OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( - ObservationModelCreator< 1, ObservationScalarType, TimeType >::createObservationModel( - std::make_shared< ObservationModelSettings >( - one_way_doppler, downlinkLinkEnds, observationSettings->lightTimeCorrectionsList_ ), bodies, topLevelObservableType ) ), - observationBias ); + lightTimeCorrectionsList.push_back( twoWayDopplerSettings->uplinkOneWayDopplerSettings_->lightTimeCorrectionsList_ ); + lightTimeCorrectionsList.push_back( twoWayDopplerSettings->downlinkOneWayDopplerSettings_->lightTimeCorrectionsList_ ); + + singleLegsLightTimeConvergenceCriteriaList.push_back( twoWayDopplerSettings->uplinkOneWayDopplerSettings_->lightTimeConvergenceCriteria_ ); + singleLegsLightTimeConvergenceCriteriaList.push_back( twoWayDopplerSettings->downlinkOneWayDopplerSettings_->lightTimeConvergenceCriteria_ ); } else { - observationModel = std::make_shared< TwoWayDopplerObservationModel< - ObservationScalarType, TimeType > >( - linkEnds, - std::dynamic_pointer_cast< OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( - ObservationModelCreator< 1, ObservationScalarType, TimeType >::createObservationModel( - twoWayDopplerSettings->uplinkOneWayDopplerSettings_, bodies, topLevelObservableType ) ), - std::dynamic_pointer_cast< OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( - ObservationModelCreator< 1, ObservationScalarType, TimeType >::createObservationModel( - twoWayDopplerSettings->downlinkOneWayDopplerSettings_, bodies, topLevelObservableType ) ), - observationBias, twoWayDopplerSettings->normalizeWithSpeedOfLight_ ); + for( unsigned int i = 0; i < 2; i++ ) + { + lightTimeCorrectionsList.push_back( observationSettings->lightTimeCorrectionsList_ ); + singleLegsLightTimeConvergenceCriteriaList.push_back( observationSettings->lightTimeConvergenceCriteria_ ); + } + } + + // Create multi-leg light time calculator + std::shared_ptr< observation_models::MultiLegLightTimeCalculator< ObservationScalarType, TimeType > > + multiLegLightTimeCalculator = createMultiLegLightTimeCalculator< ObservationScalarType, TimeType >( + linkEnds, bodies, topLevelObservableType, lightTimeCorrectionsList ); + + std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > uplinkDopplerCalculator; + std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > downlinkDopplerCalculator; + if ( twoWayDopplerSettings != nullptr ) + { + uplinkDopplerCalculator = std::dynamic_pointer_cast< OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( + ObservationModelCreator< 1, ObservationScalarType, TimeType >::createObservationModel( + twoWayDopplerSettings->uplinkOneWayDopplerSettings_, bodies, topLevelObservableType ) ); + + downlinkDopplerCalculator = std::dynamic_pointer_cast< OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( + ObservationModelCreator< 1, ObservationScalarType, TimeType >::createObservationModel( + twoWayDopplerSettings->downlinkOneWayDopplerSettings_, bodies, topLevelObservableType ) ); } + else + { + uplinkDopplerCalculator = std::dynamic_pointer_cast< OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( + ObservationModelCreator< 1, ObservationScalarType, TimeType >::createObservationModel( + std::make_shared< ObservationModelSettings >( + one_way_doppler, uplinkLinkEnds, observationSettings->lightTimeCorrectionsList_ ), bodies, topLevelObservableType ) ); + downlinkDopplerCalculator = std::dynamic_pointer_cast< OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( + ObservationModelCreator< 1, ObservationScalarType, TimeType >::createObservationModel( + std::make_shared< ObservationModelSettings >( + one_way_doppler, downlinkLinkEnds, observationSettings->lightTimeCorrectionsList_ ), bodies, topLevelObservableType ) ); + } + + bool normalizeWithSpeedOfLight = false; + observationModel = std::make_shared< TwoWayDopplerObservationModel< + ObservationScalarType, TimeType > >( + linkEnds, multiLegLightTimeCalculator, uplinkDopplerCalculator, downlinkDopplerCalculator, + observationBias, normalizeWithSpeedOfLight ); break; } diff --git a/tests/src/astro/observation_models/unitTestDopplerMeasuredFrequencyObservationModel.cpp b/tests/src/astro/observation_models/unitTestDopplerMeasuredFrequencyObservationModel.cpp index df4d7012f4..768736ac82 100644 --- a/tests/src/astro/observation_models/unitTestDopplerMeasuredFrequencyObservationModel.cpp +++ b/tests/src/astro/observation_models/unitTestDopplerMeasuredFrequencyObservationModel.cpp @@ -131,7 +131,7 @@ BOOST_AUTO_TEST_CASE(testJuiceMeasuredFrequency) std::shared_ptr > dopplerFrequencyObservationModel = std::dynamic_pointer_cast>( ObservationModelCreator<1, double, Time>::createObservationModel( - std::make_shared( doppler_measured_frequency, linkEnds, lightTimeCorrectionsList ), bodies )); + std::make_shared( doppler_measured_frequency, linkEnds, lightTimeCorrectionsList ), bodies )); // Define link end LinkEndType referenceLinkEnd = receiver; diff --git a/tests/src/astro/observation_models/unitTestDopplerModels.cpp b/tests/src/astro/observation_models/unitTestDopplerModels.cpp index 8fce84c88d..9007f88cf8 100644 --- a/tests/src/astro/observation_models/unitTestDopplerModels.cpp +++ b/tests/src/astro/observation_models/unitTestDopplerModels.cpp @@ -373,7 +373,7 @@ BOOST_AUTO_TEST_CASE( testTwoWayDoppplerModel ) for( unsigned testCase = 0; testCase < 3; testCase++ ) { - for( unsigned int normalizeObservable = 0; normalizeObservable < 2 ; normalizeObservable++ ) + for( unsigned int normalizeObservable = 0; normalizeObservable < 2; normalizeObservable++ ) { if( normalizeObservable == 0 ) { From 27f6b471c6d70d26bc340c969cc530295cc6da7e Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Tue, 10 Dec 2024 17:27:34 +0100 Subject: [PATCH 02/24] Working on model --- .../oneWayDopplerObservationModel.h | 6 +++--- .../estimation_setup/createObservationModel.h | 17 ++++++++++++----- ...DopplerMeasuredFrequencyObservationModel.cpp | 6 +++--- 3 files changed, 18 insertions(+), 11 deletions(-) diff --git a/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h b/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h index f62c7b7b71..befda707c7 100644 --- a/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h +++ b/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h @@ -286,13 +286,13 @@ class DirectFirstOrderDopplerProperTimeRateInterface: const std::function< double( ) > gravitationalParameterFunction, const std::string& referenceBody, const LinkEndType referencePointLinkEndType = unidentified_link_end, - const std::function< Eigen::Vector6d( const double ) > referencePointStateFunction = - std::function< Eigen::Vector6d( const double ) >( ) ): + const std::vector< std::function< Eigen::Vector6d( const double ) > > referencePointStateFunction = + std::vector< std::function< Eigen::Vector6d( const double ) > >( ) ): DopplerProperTimeRateInterface( computationPointLinkEndType ), gravitationalParameterFunction_( gravitationalParameterFunction ), referenceBody_( referenceBody ), referencePointLinkEndType_( referencePointLinkEndType ), - referencePointStateFunction_( referencePointStateFunction ) + referencePointStateFunctions_( referencePointStateFunctions ) { // Check input consistency if( this->computationPointLinkEndType_ == referencePointLinkEndType ) diff --git a/include/tudat/simulation/estimation_setup/createObservationModel.h b/include/tudat/simulation/estimation_setup/createObservationModel.h index 35266ca113..14231fc9fc 100644 --- a/include/tudat/simulation/estimation_setup/createObservationModel.h +++ b/include/tudat/simulation/estimation_setup/createObservationModel.h @@ -613,9 +613,14 @@ class DirectFirstOrderDopplerProperTimeRateSettings: public DopplerProperTimeRat * and w.r.t. which the velocity of the point at which proper time rate is computed is taken */ DirectFirstOrderDopplerProperTimeRateSettings( - const std::string centralBodyName ): + const std::string& centralBodyName ): DopplerProperTimeRateSettings( direct_first_order_doppler_proper_time_rate ), - centralBodyName_( centralBodyName ){ } + centralBodyNames_( { centralBodyName } ){ } + + DirectFirstOrderDopplerProperTimeRateSettings( + const std::vector< std::string >& centralBodyNames ): + DopplerProperTimeRateSettings( direct_first_order_doppler_proper_time_rate ), + centralBodyNames_( centralBodyNames ){ } //! Destructor. ~DirectFirstOrderDopplerProperTimeRateSettings( ){ } @@ -625,7 +630,7 @@ class DirectFirstOrderDopplerProperTimeRateSettings: public DopplerProperTimeRat * Name of central body, fromw which the mass monopole is retrieved to compute the proper time rate, * and w.r.t. which the velocity of the point at which proper time rate is computed is taken */ - std::string centralBodyName_; + std::vector< std::string > centralBodyNames_; }; //! Class to define the settings for one-way Doppler observable @@ -1282,6 +1287,8 @@ std::shared_ptr< DopplerProperTimeRateInterface > createOneWayDopplerProperTimeC } else { + std::vector< std::function< double( ) > > gravitationalParameterFunctions; + if( bodies.at( directFirstOrderDopplerProperTimeRateSettings->centralBodyName_ )->getGravityFieldModel( ) == nullptr ) { throw std::runtime_error( "Error when making DirectFirstOrderDopplerProperTimeRateInterface, no gravity field found for " + @@ -2291,11 +2298,11 @@ class ObservationModelCreator< 1, ObservationScalarType, TimeType > { auto uplinkOneWaySettings = std::make_shared< OneWayDopplerObservationSettings >( getUplinkFromTwoWayLinkEnds( linkEnds ), observationSettings->lightTimeCorrectionsList_, - std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), nullptr ); + std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Sun" ), std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Sun" ) ); uplinkOneWaySettings->normalizeWithSpeedOfLight_ = false; auto downlinkOneWaySettings = std::make_shared< OneWayDopplerObservationSettings >( getDownlinkFromTwoWayLinkEnds( linkEnds ), observationSettings->lightTimeCorrectionsList_, - nullptr, std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ) ); + std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Sun" ), std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Sun" ) ); downlinkOneWaySettings->normalizeWithSpeedOfLight_ = false; auto twoWaySettings = std::make_shared< TwoWayDopplerObservationSettings >( uplinkOneWaySettings, downlinkOneWaySettings ); diff --git a/tests/src/astro/observation_models/unitTestDopplerMeasuredFrequencyObservationModel.cpp b/tests/src/astro/observation_models/unitTestDopplerMeasuredFrequencyObservationModel.cpp index 768736ac82..5e8350c1ba 100644 --- a/tests/src/astro/observation_models/unitTestDopplerMeasuredFrequencyObservationModel.cpp +++ b/tests/src/astro/observation_models/unitTestDopplerMeasuredFrequencyObservationModel.cpp @@ -198,9 +198,9 @@ BOOST_AUTO_TEST_CASE(testJuiceMeasuredFrequency) // std::cout<( observationTimes ) ), "pride_times.dat", 16 ); + input_output::writeMatrixToFile( observableVector, "pride_doppler.dat", 16 ); + input_output::writeMatrixToFile( residualVector, "pride_residual.dat", 16 ); + input_output::writeMatrixToFile( utilities::convertStlVectorToEigenVector( utilities::staticCastVector< double, Time >( observationTimes ) ), "pride_times.dat", 16 ); } } From 9bd08451cc74ab76fffae45c152f68e031baf328 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Fri, 13 Dec 2024 08:31:29 +0100 Subject: [PATCH 03/24] Moving to develop for test --- .../oneWayDopplerObservationModel.h | 50 +++++++++---------- .../estimation_setup/createObservationModel.h | 11 +--- 2 files changed, 27 insertions(+), 34 deletions(-) diff --git a/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h b/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h index befda707c7..63ab9c6bb3 100644 --- a/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h +++ b/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h @@ -283,7 +283,7 @@ class DirectFirstOrderDopplerProperTimeRateInterface: */ DirectFirstOrderDopplerProperTimeRateInterface( const LinkEndType computationPointLinkEndType, - const std::function< double( ) > gravitationalParameterFunction, + const std::vector< std::function< double( ) > > gravitationalParameterFunction, const std::string& referenceBody, const LinkEndType referencePointLinkEndType = unidentified_link_end, const std::vector< std::function< Eigen::Vector6d( const double ) > > referencePointStateFunction = @@ -386,44 +386,44 @@ class DirectFirstOrderDopplerProperTimeRateInterface: return ( ( referencePointLinkEndType_ == transmitter ) ? linkEndStates.at( 0 ) : linkEndStates.at( 1 ) ); } } - - //! Function to retrieve central body gravitational parameter - /*! - * Function to retrieve central body gravitational parameter - * \return Central body gravitational parameter - */ - double getGravitationalParameter( ) - { - return gravitationalParameterFunction_( ); - } - - //! Function to return the name of body generating the gravity field. - /*! - * Function to return the name of body generating the gravity field - * \return Name of body generating the gravity field - */ - std::string getCentralBody( ) - { - return referenceBody_; - } +// +// //! Function to retrieve central body gravitational parameter +// /*! +// * Function to retrieve central body gravitational parameter +// * \return Central body gravitational parameter +// */ +// double getGravitationalParameter( ) +// { +// return gravitationalParameterFunction_( ); +// } +// +// //! Function to return the name of body generating the gravity field. +// /*! +// * Function to return the name of body generating the gravity field +// * \return Name of body generating the gravity field +// */ +// std::string getCentralBody( ) +// { +// return referenceBodies_; +// } private: //! Function that returns the gravitational parameter of the central body. - std::function< double( ) > gravitationalParameterFunction_; + std::vector< std::function< double( ) > > gravitationalParameterFunctions_; //! Name of body generating the gravity field. - std::string referenceBody_; + std::vector< std::string > referenceBodies_; //! Link end type of central body (unidentified_link_end if central body is not one of the link ends) - LinkEndType referencePointLinkEndType_; + std::vector< LinkEndType > referencePointLinkEndType_; //! Function that returns the state of the central body as a function of time. /*! * Function that returns the state of the central body as a function of time, must be provided if * referencePointLinkEndType equals unidentified_link_end. */ - std::function< Eigen::Vector6d( const double ) > referencePointStateFunction_; + std::vector< std::function< Eigen::Vector6d( const double ) > > referencePointStateFunctions_; }; diff --git a/include/tudat/simulation/estimation_setup/createObservationModel.h b/include/tudat/simulation/estimation_setup/createObservationModel.h index 98ada8c4b3..d26a539cc8 100644 --- a/include/tudat/simulation/estimation_setup/createObservationModel.h +++ b/include/tudat/simulation/estimation_setup/createObservationModel.h @@ -2499,7 +2499,6 @@ class ObservationModelCreator< 1, ObservationScalarType, TimeType > "Error when making n-way differenced range observation model, input " "type inconsistent" ); } -<<<<<<< HEAD } std::shared_ptr< ground_stations::GroundStationState > receivingStationState = @@ -2534,13 +2533,6 @@ class ObservationModelCreator< 1, ObservationScalarType, TimeType > ObservationModelCreator< 1, ObservationScalarType, TimeType >::createObservationModel( twoWaySettings, bodies ) ); if( twoWayDopplerModel == nullptr ) -======= - std::shared_ptr< NWayRangeObservationModel< ObservationScalarType, TimeType > > - arcStartObservationModel; - std::shared_ptr< NWayRangeObservationModel< ObservationScalarType, TimeType > > - arcEndObservationModel; - try ->>>>>>> develop { std::shared_ptr< ObservationModelSettings > undifferencedObservationSettings = nWayDifferencedRangeObservationSettings @@ -2586,7 +2578,8 @@ class ObservationModelCreator< 1, ObservationScalarType, TimeType > observationBias ); break; } - case dsn_n_way_averaged_doppler: { + case dsn_n_way_averaged_doppler: + { std::shared_ptr< DsnNWayAveragedDopplerObservationSettings > dsnNWayAveragedDopplerObservationSettings = std::dynamic_pointer_cast< DsnNWayAveragedDopplerObservationSettings >( observationSettings ); From 29ac26006570682e20478a96c6c9b63051c4182e Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Fri, 13 Dec 2024 11:54:15 +0100 Subject: [PATCH 04/24] Working on refining Doppler partial --- .../observation_models/lightTimeSolution.h | 12 +- .../oneWayDopplerObservationModel.h | 105 +- .../twoWayDopplerObservationModel.h | 4 +- .../relativity/relativisticTimeConversion.h | 10 +- .../estimation_setup/createObservationModel.h | 1555 ++++++++--------- .../createPositionPartialScaling.h | 7 +- .../oneWayDopplerPartial.cpp | 38 +- .../relativisticLightTimeCorrection.cpp | 2 +- .../relativity/relativisticTimeConversion.cpp | 21 +- ...pplerMeasuredFrequencyObservationModel.cpp | 3 + .../unitTestOneWayDopplerPartials.cpp | 3 +- 11 files changed, 891 insertions(+), 869 deletions(-) diff --git a/include/tudat/astro/observation_models/lightTimeSolution.h b/include/tudat/astro/observation_models/lightTimeSolution.h index 6bcf5eaff4..e5f3551437 100644 --- a/include/tudat/astro/observation_models/lightTimeSolution.h +++ b/include/tudat/astro/observation_models/lightTimeSolution.h @@ -645,11 +645,15 @@ class LightTimeCalculator Eigen::Matrix< ObservationScalarType, 3, 1 > relativePosition = receiverState.segment( 0, 3 ) - transmitterState.segment( 0, 3 ); - return ( relativePosition.normalized( ) ).transpose( ) * -// ( mathematical_constants::getFloatingInteger< ObservationScalarType >( 1 ) + -// currentCorrection_ / relativePosition.norm( ) ) * - ( isPartialWrtReceiver ? mathematical_constants::getFloatingInteger< ObservationScalarType >( 1 ) : + Eigen::Matrix< ObservationScalarType, 1, 3 > partialWrtLinkEndPosition = + ( relativePosition.normalized( ) ).transpose( ) * ( isPartialWrtReceiver ? mathematical_constants::getFloatingInteger< ObservationScalarType >( 1 ) : mathematical_constants::getFloatingInteger< ObservationScalarType >( -1 ) ); + for( unsigned int i = 0; i < correctionFunctions_.size( ); i++ ) + { + partialWrtLinkEndPosition += correctionFunctions_.at( i )->calculateLightTimeCorrectionPartialDerivativeWrtLinkEndPosition( + transmitterState, receiverState, transmitterTime, receiverTime, isPartialWrtReceiver ? receiver : transmitter ); + } + return partialWrtLinkEndPosition; } //! Function to get list of light-time correction functions diff --git a/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h b/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h index 63ab9c6bb3..a7fc8d3913 100644 --- a/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h +++ b/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h @@ -283,40 +283,42 @@ class DirectFirstOrderDopplerProperTimeRateInterface: */ DirectFirstOrderDopplerProperTimeRateInterface( const LinkEndType computationPointLinkEndType, - const std::vector< std::function< double( ) > > gravitationalParameterFunction, - const std::string& referenceBody, - const LinkEndType referencePointLinkEndType = unidentified_link_end, - const std::vector< std::function< Eigen::Vector6d( const double ) > > referencePointStateFunction = - std::vector< std::function< Eigen::Vector6d( const double ) > >( ) ): + const std::vector< std::function< double( ) > >& gravitationalParameterFunctions, + const std::vector< std::function< Eigen::Vector6d( const double ) > >& perturbingBodyStateFunctions, + const std::vector< LinkEndType >& perturbingBodyMatchLinkEnds, + const std::vector< std::string >& perturbingBodyNames ): DopplerProperTimeRateInterface( computationPointLinkEndType ), - gravitationalParameterFunction_( gravitationalParameterFunction ), - referenceBody_( referenceBody ), - referencePointLinkEndType_( referencePointLinkEndType ), - referencePointStateFunctions_( referencePointStateFunctions ) + gravitationalParameterFunctions_( gravitationalParameterFunctions ), + perturbingBodyStateFunctions_( perturbingBodyStateFunctions ), + perturbingBodyMatchLinkEnds_( perturbingBodyMatchLinkEnds ), + perturbingBodyNames_( perturbingBodyNames ) { - // Check input consistency - if( this->computationPointLinkEndType_ == referencePointLinkEndType ) + if( ( gravitationalParameterFunctions_.size( ) != perturbingBodyStateFunctions_.size( ) ) || + ( gravitationalParameterFunctions_.size( ) != perturbingBodyMatchLinkEnds_.size( ) ) || + ( gravitationalParameterFunctions_.size( ) != perturbingBodyNames_.size( ) ) ) { - throw std::runtime_error( "Error when creating DirectFirstOrderDopplerProperTimeRateInterface, input link end types must be different" ); + throw std::runtime_error( "Error when creating DirectFirstOrderDopplerProperTimeRateInterface, input sizes are inconsistent" ); } - else if( ( this->computationPointLinkEndType_ != receiver ) && + + currentPerturbedStates_.resize( gravitationalParameterFunctions_.size( ) ); + currentCentralBodyGravitationalParameters_.resize( gravitationalParameterFunctions_.size( ) ); + + // Check input consistency + if( ( this->computationPointLinkEndType_ != receiver ) && ( this->computationPointLinkEndType_ != transmitter ) ) { throw std::runtime_error( "Error when creating DirectFirstOrderDopplerProperTimeRateInterface, computation point must be receiver or transmitter" ); } - else if( ( this->computationPointLinkEndType_ != receiver ) && - ( this->computationPointLinkEndType_ != transmitter ) && - ( this->computationPointLinkEndType_ != unidentified_link_end ) ) - { - throw std::runtime_error( "Error when creating DirectFirstOrderDopplerProperTimeRateInterface, reference point must be receiver, transmitter or unidentified" ); - } - else if( ( referencePointLinkEndType == unidentified_link_end ) && ( referencePointStateFunction == nullptr ) ) - { - throw std::runtime_error( "Error when creating DirectFirstOrderDopplerProperTimeRateInterface, reference point must have state information" ); - } - else if( ( referencePointLinkEndType != unidentified_link_end ) && !( referencePointStateFunction == nullptr ) ) + + for( unsigned int i = 0; i < perturbingBodyMatchLinkEnds.size( ); i++ ) { - throw std::runtime_error( "Error when creating DirectFirstOrderDopplerProperTimeRateInterface, reference point must have unambiguous state information" ); + if ( ( this->perturbingBodyMatchLinkEnds_.at( i ) != receiver ) && + ( this->perturbingBodyMatchLinkEnds_.at( i ) != transmitter ) && + ( this->perturbingBodyMatchLinkEnds_.at( i ) != unidentified_link_end ) ) + { + throw std::runtime_error( + "Error when creating DirectFirstOrderDopplerProperTimeRateInterface, perturbing body link end id must be receiver, transmitter or unidentified" ); + } } } @@ -341,12 +343,12 @@ class DirectFirstOrderDopplerProperTimeRateInterface: } // Compute central body state w.r.t. computation point - Eigen::Vector6d computationPointRelativeState = - getComputationPointRelativeState( linkEndTimes, linkEndStates ); + Eigen::Vector6d computationPointRelativeState = getComputationPointState( linkEndTimes, linkEndStates ); + setPerturbedProperties( linkEndTimes, linkEndStates ); // Compute proper time rate return relativity::calculateFirstCentralBodyProperTimeRateDifference( - computationPointRelativeState, gravitationalParameterFunction_( ), + computationPointRelativeState, currentPerturbedStates_, currentCentralBodyGravitationalParameters_, relativity::equivalencePrincipleLpiViolationParameter ); } @@ -357,12 +359,11 @@ class DirectFirstOrderDopplerProperTimeRateInterface: * \param linkEndStates Link end Cartesian states for one-way Doppler observale for which proper time rate is to be computed. * \return The state of the computation point w.r.t. the central body */ - Eigen::Vector6d getComputationPointRelativeState( + Eigen::Vector6d getComputationPointState( const std::vector< double >& linkEndTimes, const std::vector< Eigen::Matrix< double, 6, 1 > >& linkEndStates ) { - return ( ( this->computationPointLinkEndType_ == transmitter ) ? linkEndStates.at( 0 ) : linkEndStates.at( 1 ) ) - - getReferencePointState( linkEndTimes, linkEndStates ); + return ( ( this->computationPointLinkEndType_ == transmitter ) ? linkEndStates.at( 0 ) : linkEndStates.at( 1 ) ); } //! Function to compute the state of the central body (e.g. origin for positions and velocities in proper time calculations) @@ -372,18 +373,23 @@ class DirectFirstOrderDopplerProperTimeRateInterface: * \param linkEndStates Link end Cartesian states for one-way Doppler observale for which proper time rate is to be computed. * \return The state of the central body */ - Eigen::Vector6d getReferencePointState( + void setPerturbedProperties( const std::vector< double >& linkEndTimes, const std::vector< Eigen::Matrix< double, 6, 1 > >& linkEndStates ) { - if( referencePointLinkEndType_ == unidentified_link_end ) + for( unsigned int i = 0; i < perturbingBodyNames_.size( ); i++ ) { - return referencePointStateFunction_( - ( ( this->computationPointLinkEndType_ == transmitter ) ? linkEndTimes.at( 0 ) : linkEndTimes.at( 1 ) ) ); - } - else - { - return ( ( referencePointLinkEndType_ == transmitter ) ? linkEndStates.at( 0 ) : linkEndStates.at( 1 ) ); + double evaluationTime = 0.0; + if( perturbingBodyMatchLinkEnds_.at( i ) == unidentified_link_end ) + { + evaluationTime = ( linkEndTimes.at( 0 ) + linkEndTimes.at( 1 ) ) / 2.0; + } + else + { + evaluationTime = ( this->perturbingBodyMatchLinkEnds_.at( i ) == transmitter ) ? linkEndTimes.at( 0 ) : linkEndTimes.at( 1 ); + } + currentPerturbedStates_[ i ] = perturbingBodyStateFunctions_.at( i )( evaluationTime ); + currentCentralBodyGravitationalParameters_[ i ] = gravitationalParameterFunctions_.at( i )( ); } } // @@ -409,21 +415,20 @@ class DirectFirstOrderDopplerProperTimeRateInterface: private: - //! Function that returns the gravitational parameter of the central body. std::vector< std::function< double( ) > > gravitationalParameterFunctions_; - //! Name of body generating the gravity field. - std::vector< std::string > referenceBodies_; + std::vector< std::function< Eigen::Vector6d( const double ) > > perturbingBodyStateFunctions_; - //! Link end type of central body (unidentified_link_end if central body is not one of the link ends) - std::vector< LinkEndType > referencePointLinkEndType_; + std::vector< LinkEndType > perturbingBodyMatchLinkEnds_; + + std::vector< std::string > perturbingBodyNames_; + + + + std::vector< Eigen::Vector6d > currentPerturbedStates_; + + std::vector< double > currentCentralBodyGravitationalParameters_; - //! Function that returns the state of the central body as a function of time. - /*! - * Function that returns the state of the central body as a function of time, must be provided if - * referencePointLinkEndType equals unidentified_link_end. - */ - std::vector< std::function< Eigen::Vector6d( const double ) > > referencePointStateFunctions_; }; diff --git a/include/tudat/astro/observation_models/twoWayDopplerObservationModel.h b/include/tudat/astro/observation_models/twoWayDopplerObservationModel.h index 75b88fd178..38ae5d3176 100644 --- a/include/tudat/astro/observation_models/twoWayDopplerObservationModel.h +++ b/include/tudat/astro/observation_models/twoWayDopplerObservationModel.h @@ -123,9 +123,9 @@ class TwoWayDopplerObservationModel: public ObservationModel< 1, ObservationScal } uplinkDoppler = uplinkDopplerCalculator_->computeIdealDopplerWithLinkEndData( - transmitter, uplinkLinkEndTimes, uplinkLinkEndStates ); + uplinkReferenceLinkEnd, uplinkLinkEndTimes, uplinkLinkEndStates ); downlinkDoppler = downlinkDopplerCalculator_->computeIdealDopplerWithLinkEndData( - transmitter, downlinkLinkEndTimes, downlinkLinkEndStates ); + downlinkReferenceLinkEnd, downlinkLinkEndTimes, downlinkLinkEndStates ); return multiplicationTerm_ * ( Eigen::Matrix< ObservationScalarType, 1, 1 >( ) << downlinkDoppler( 0 ) * uplinkDoppler( 0 ) + diff --git a/include/tudat/astro/relativity/relativisticTimeConversion.h b/include/tudat/astro/relativity/relativisticTimeConversion.h index 113e935c95..8bb890b1e0 100644 --- a/include/tudat/astro/relativity/relativisticTimeConversion.h +++ b/include/tudat/astro/relativity/relativisticTimeConversion.h @@ -12,6 +12,7 @@ #define TUDAT_RELATIVISTIVTIMECONVERSION_H #include +#include #include "tudat/basics/basicTypedefs.h" namespace tudat @@ -30,7 +31,8 @@ namespace relativity * \return Proper time rate minus one (d tau/dt - 1.0) */ double calculateFirstCentralBodyProperTimeRateDifference( - const double relativeSpeed, const double gravitationalScalarPotential, + const double computationPointSpeed, + const double gravitationalScalarPotential, const double equivalencePrincipleLpiViolationParameter = 0.0 ); //! Function to compute proper-time rate w.r.t. coordinate time, minus 1.0, for a 1/c^2 potential from a static mass monopole @@ -38,13 +40,15 @@ double calculateFirstCentralBodyProperTimeRateDifference( * Function to compute proper-time rate w.r.t. coordinate time, minus 1.0, for a 1/c^2 potential from a static mass monopole * located at the origin of the reference frame of which the coordinate time is used., * allowing for the possibility of EP violation (nominally none) - * \param relativeStateVector Cartesian state of point where proper time rate is to be computed + * \param computationPointState Cartesian state of point where proper time rate is to be computed * \param centralBodyGravitationalParameter Newtonian gravitational potnetial of central body. * \param equivalencePrincipleLpiViolationParameter Violation parameter of equivalence principle (default to GR value of 0.0 * \return Proper time rate minus one (d tau/dt - 1.0) */ double calculateFirstCentralBodyProperTimeRateDifference( - const Eigen::Vector6d relativeStateVector, const double centralBodyGravitationalParameter, + const Eigen::Vector6d& computationPointState, + const std::vector< Eigen::Vector6d >& perturbedStates, + const std::vector< double >& centralBodyGravitationalParameters, const double equivalencePrincipleLpiViolationParameter = 0.0 ); } diff --git a/include/tudat/simulation/estimation_setup/createObservationModel.h b/include/tudat/simulation/estimation_setup/createObservationModel.h index d26a539cc8..148761caef 100644 --- a/include/tudat/simulation/estimation_setup/createObservationModel.h +++ b/include/tudat/simulation/estimation_setup/createObservationModel.h @@ -714,13 +714,24 @@ class TwoWayDopplerObservationSettings : public ObservationModelSettings TwoWayDopplerObservationSettings( const std::shared_ptr< OneWayDopplerObservationSettings > uplinkOneWayDopplerSettings, const std::shared_ptr< OneWayDopplerObservationSettings > downlinkOneWayDopplerSettings, + const ObservableType observableType, const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr ): - ObservationModelSettings( two_way_doppler, mergeUpDownLink( + ObservationModelSettings( observableType, mergeUpDownLink( uplinkOneWayDopplerSettings->linkEnds_, downlinkOneWayDopplerSettings->linkEnds_ ), std::shared_ptr< LightTimeCorrectionSettings >( ), biasSettings, std::make_shared< LightTimeConvergenceCriteria >( ) ), uplinkOneWayDopplerSettings_( uplinkOneWayDopplerSettings ), downlinkOneWayDopplerSettings_( downlinkOneWayDopplerSettings ) { + if( observableType != two_way_doppler && observableType != doppler_measured_frequency ) + { + throw std::runtime_error( "Error when creating 2-way Doppler settings, input type is inconsistent" ); + } + + if( observableType == doppler_measured_frequency ) + { + uplinkOneWayDopplerSettings->normalizeWithSpeedOfLight_ = false; + downlinkOneWayDopplerSettings->normalizeWithSpeedOfLight_ = false; + } if( uplinkOneWayDopplerSettings->normalizeWithSpeedOfLight_ != downlinkOneWayDopplerSettings->normalizeWithSpeedOfLight_ ) { @@ -1192,7 +1203,7 @@ inline std::shared_ptr< ObservationModelSettings > twoWayOpenLoopDoppler( const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr ) { return std::make_shared< TwoWayDopplerObservationSettings >( - uplinkOneWayDopplerSettings, downlinkOneWayDopplerSettings, biasSettings ); + uplinkOneWayDopplerSettings, downlinkOneWayDopplerSettings, two_way_doppler, biasSettings ); } inline std::shared_ptr< ObservationModelSettings > twoWayOpenLoopDoppler( @@ -1228,6 +1239,35 @@ inline std::shared_ptr< ObservationModelSettings > dopplerMeasuredFrequencyObser lightTimeConvergenceCriteria ); } +inline std::shared_ptr< ObservationModelSettings > dopplerMeasuredFrequencyObservationSettings( + const LinkDefinition& linkEnds, + const std::shared_ptr< DopplerProperTimeRateSettings > properTimeRateSettings, + const std::vector< std::shared_ptr< LightTimeCorrectionSettings > >& lightTimeCorrectionsList = std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr, + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = std::make_shared< LightTimeConvergenceCriteria >( ) ) +{ + if( properTimeRateSettings != nullptr ) + { + std::shared_ptr< OneWayDopplerObservationSettings > uplinkOneWaySettings = std::make_shared< OneWayDopplerObservationSettings >( + getUplinkFromTwoWayLinkEnds( linkEnds ), lightTimeCorrectionsList, properTimeRateSettings, properTimeRateSettings, nullptr, lightTimeConvergenceCriteria ); + uplinkOneWaySettings->normalizeWithSpeedOfLight_ = false; + + std::shared_ptr< OneWayDopplerObservationSettings > downlinkOneWaySettings = std::make_shared< OneWayDopplerObservationSettings >( + getDownlinkFromTwoWayLinkEnds( linkEnds ), lightTimeCorrectionsList, properTimeRateSettings, properTimeRateSettings, nullptr, lightTimeConvergenceCriteria ); + downlinkOneWaySettings->normalizeWithSpeedOfLight_ = false; + + return std::make_shared< TwoWayDopplerObservationSettings >( uplinkOneWaySettings, downlinkOneWaySettings, doppler_measured_frequency, biasSettings ); + } + else + { + return std::make_shared( doppler_measured_frequency, + linkEnds, + lightTimeCorrectionsList, + biasSettings, + lightTimeConvergenceCriteria ); + } +} + inline std::shared_ptr< ObservationModelSettings > oneWayClosedLoopDoppler( const LinkDefinition& linkEnds, const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > @@ -1457,76 +1497,82 @@ std::shared_ptr< DopplerProperTimeRateInterface > createOneWayDopplerProperTimeC } else { - std::vector< std::function< double( ) > > gravitationalParameterFunctions; + std::vector > gravitationalParameterFunctions; + std::vector > perturbingBodyStateFunctions; + std::vector perturbingBodyMatchLinkEnds; + std::vector perturbingBodyNames; - if( bodies.at( directFirstOrderDopplerProperTimeRateSettings->centralBodyName_ )->getGravityFieldModel( ) == nullptr ) - { - throw std::runtime_error( - "Error when making DopplerProperTimeRateInterface, input type " - "(direct_first_order_doppler_proper_time_rate) is inconsistent" ); - } - else if( linkEnds.count( linkEndForCalculator ) == 0 ) + if ( linkEnds.count( linkEndForCalculator ) == 0 ) { std::string errorMessage = - "Error when creating one-way Doppler proper time calculator, did not find " - "link end " + - std::to_string( linkEndForCalculator ); + "Error when creating one-way Doppler proper time calculator, did not find " + "link end " + + std::to_string( linkEndForCalculator ); throw std::runtime_error( errorMessage ); } - else + + for ( unsigned int i = 0; i < directFirstOrderDopplerProperTimeRateSettings->centralBodyNames_.size( ); i++ ) { - if( bodies.at( directFirstOrderDopplerProperTimeRateSettings->centralBodyName_ ) - ->getGravityFieldModel( ) == nullptr ) + std::string currentBodyName = directFirstOrderDopplerProperTimeRateSettings->centralBodyNames_.at( i ); + LinkEndId perturberLinkEndId = LinkEndId( + currentBodyName, "" ); + if (( linkEnds.at( receiver ) == perturberLinkEndId ) || ( linkEnds.at( transmitter ) == perturberLinkEndId )) + { + throw std::runtime_error( + "Error, proper time reference point is identical perturbed point; DopplerProperTimeRateInterface will yield Inf dtau/dt" ); + } + + // Set perturbed evaluation time + if ( linkEnds.at( transmitter ).bodyName_ == currentBodyName ) + { + perturbingBodyMatchLinkEnds.push_back( transmitter ); + } + else if ( linkEnds.at( receiver ).bodyName_ == currentBodyName ) + { + perturbingBodyMatchLinkEnds.push_back( receiver ); + } + else + { + perturbingBodyMatchLinkEnds.push_back( unidentified_link_end ); + } + + // Set perturber gravitational parameter + if ( bodies.at( currentBodyName )->getGravityFieldModel( ) == nullptr ) { throw std::runtime_error( - "Error when making DirectFirstOrderDopplerProperTimeRateInterface, no " - "gravity field found for " + - directFirstOrderDopplerProperTimeRateSettings->centralBodyName_ ); + "Error when making DopplerProperTimeRateInterface, body has not gravity field " + + directFirstOrderDopplerProperTimeRateSettings->centralBodyNames_.at( i ) ); } else { // Retrieve gravitational parameter - std::function< double( ) > gravitationalParameterFunction = - std::bind( &gravitation::GravityFieldModel::getGravitationalParameter, - bodies.at( directFirstOrderDopplerProperTimeRateSettings - ->centralBodyName_ ) - ->getGravityFieldModel( ) ); - - // Create calculation object. - LinkEndId referencePointId = LinkEndId( - directFirstOrderDopplerProperTimeRateSettings->centralBodyName_, "" ); - if( ( linkEnds.at( receiver ) != referencePointId ) && - ( linkEnds.at( transmitter ) != referencePointId ) ) - { - properTimeRateInterface = std::make_shared< - DirectFirstOrderDopplerProperTimeRateInterface >( - linkEndForCalculator, - gravitationalParameterFunction, - directFirstOrderDopplerProperTimeRateSettings->centralBodyName_, - unidentified_link_end, - simulation_setup::getLinkEndCompleteEphemerisFunction< double, - double >( - LinkEndId( directFirstOrderDopplerProperTimeRateSettings - ->centralBodyName_, - "" ), - bodies ) ); - } - else - { - throw std::runtime_error( - "Error, proper time reference point as link end not yet " - "implemented for DopplerProperTimeRateInterface creation" ); - } + gravitationalParameterFunctions.push_back( + std::bind( &gravitation::GravityFieldModel::getGravitationalParameter, + bodies.at( currentBodyName )->getGravityFieldModel( ))); } + + perturbingBodyStateFunctions.push_back( + std::bind( &simulation_setup::Body::getStateInBaseFrameFromEphemeris, + bodies.at( currentBodyName ), std::placeholders::_1 )); + } - break; + properTimeRateInterface = std::make_shared< + DirectFirstOrderDopplerProperTimeRateInterface>( + linkEndForCalculator, + gravitationalParameterFunctions, + perturbingBodyStateFunctions, + perturbingBodyMatchLinkEnds, + directFirstOrderDopplerProperTimeRateSettings->centralBodyNames_ ); + } - default: - std::string errorMessage = - "Error when creating one-way Doppler proper time calculator, did not recognize " - "type " + - std::to_string( properTimeRateSettings->dopplerProperTimeRateType_ ); - throw std::runtime_error( errorMessage ); + break; + } + default: + std::string errorMessage = + "Error when creating one-way Doppler proper time calculator, did not recognize " + "type " + + std::to_string( properTimeRateSettings->dopplerProperTimeRateType_ ); + throw std::runtime_error( errorMessage ); } return properTimeRateInterface; } @@ -2067,840 +2113,787 @@ class ObservationModelCreator< 1, ObservationScalarType, TimeType > // Check type of observation model. switch( observationSettings->observableType_ ) { - case one_way_range: { - // Check consistency input. - if( linkEnds.size( ) != 2 ) - { - std::string errorMessage = "Error when making 1 way range model, " + - std::to_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" ); - } + case one_way_range: { + // Check consistency input. + if( linkEnds.size( ) != 2 ) + { + std::string errorMessage = "Error when making 1 way range model, " + + std::to_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" ); + } - std::shared_ptr< ObservationBias< 1 > > observationBias; - if( observationSettings->biasSettings_ != nullptr ) - { - observationBias = - createObservationBiasCalculator( linkEnds, - observationSettings->observableType_, - observationSettings->biasSettings_, - bodies ); - } + std::shared_ptr< ObservationBias< 1 > > observationBias; + if( observationSettings->biasSettings_ != nullptr ) + { + observationBias = + createObservationBiasCalculator( linkEnds, + observationSettings->observableType_, + observationSettings->biasSettings_, + bodies ); + } + + // Create observation model + observationModel = std::make_shared< + OneWayRangeObservationModel< ObservationScalarType, TimeType > >( + linkEnds, + createLightTimeCalculator< ObservationScalarType, TimeType >( + linkEnds, + transmitter, + receiver, + bodies, + topLevelObservableType, + observationSettings->lightTimeCorrectionsList_, + observationSettings->lightTimeConvergenceCriteria_ ), + observationBias ); + + break; + } + case one_way_doppler: { + // Check consistency input. + if( linkEnds.size( ) != 2 ) + { + std::string errorMessage = "Error when making 1 way Doppler model, " + + std::to_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 Doppler model, no receiver found" ); + } + if( linkEnds.count( transmitter ) == 0 ) + { + throw std::runtime_error( + "Error when making 1 way Doppler model, no transmitter found" ); + } + + std::shared_ptr< ObservationBias< 1 > > observationBias; + if( observationSettings->biasSettings_ != nullptr ) + { + observationBias = + createObservationBiasCalculator( linkEnds, + observationSettings->observableType_, + observationSettings->biasSettings_, + bodies ); + } + if( std::dynamic_pointer_cast< OneWayDopplerObservationSettings >( + observationSettings ) == nullptr ) + { // Create observation model observationModel = std::make_shared< - OneWayRangeObservationModel< ObservationScalarType, TimeType > >( + OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( + linkEnds, + createLightTimeCalculator< ObservationScalarType, TimeType >( linkEnds, - createLightTimeCalculator< ObservationScalarType, TimeType >( - linkEnds, - transmitter, - receiver, - bodies, - topLevelObservableType, - observationSettings->lightTimeCorrectionsList_, - observationSettings->lightTimeConvergenceCriteria_ ), - observationBias ); - - break; + transmitter, + receiver, + bodies, + topLevelObservableType, + observationSettings->lightTimeCorrectionsList_, + observationSettings->lightTimeConvergenceCriteria_ ), + observationBias, + std::function< ObservationScalarType( const TimeType ) >( ), + std::function< ObservationScalarType( const TimeType ) >( ), + false ); } - case one_way_doppler: { - // Check consistency input. - if( linkEnds.size( ) != 2 ) - { - std::string errorMessage = "Error when making 1 way Doppler model, " + - std::to_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 Doppler model, no receiver found" ); - } - if( linkEnds.count( transmitter ) == 0 ) - { - throw std::runtime_error( - "Error when making 1 way Doppler model, no transmitter found" ); - } - - std::shared_ptr< ObservationBias< 1 > > observationBias; - if( observationSettings->biasSettings_ != nullptr ) - { - observationBias = - createObservationBiasCalculator( linkEnds, - observationSettings->observableType_, - observationSettings->biasSettings_, - bodies ); + else + { + std::shared_ptr< OneWayDopplerObservationSettings > oneWayDopplerSettings = + std::dynamic_pointer_cast< OneWayDopplerObservationSettings >( + observationSettings ); + std::shared_ptr< DopplerProperTimeRateInterface > transmitterProperTimeRate = + nullptr; + std::shared_ptr< DopplerProperTimeRateInterface > receiverProperTimeRate = + nullptr; + if( oneWayDopplerSettings->transmitterProperTimeRateSettings_ != nullptr ) + { + transmitterProperTimeRate = + createOneWayDopplerProperTimeCalculator< ObservationScalarType, + TimeType >( + oneWayDopplerSettings->transmitterProperTimeRateSettings_, + linkEnds, + bodies, + transmitter ); } - if( std::dynamic_pointer_cast< OneWayDopplerObservationSettings >( - observationSettings ) == nullptr ) + if( oneWayDopplerSettings->receiverProperTimeRateSettings_ != nullptr ) { - // Create observation model - observationModel = std::make_shared< - OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( + receiverProperTimeRate = + createOneWayDopplerProperTimeCalculator< ObservationScalarType, + TimeType >( + oneWayDopplerSettings->receiverProperTimeRateSettings_, linkEnds, - createLightTimeCalculator< ObservationScalarType, TimeType >( - linkEnds, - transmitter, - receiver, - bodies, - topLevelObservableType, - observationSettings->lightTimeCorrectionsList_, - observationSettings->lightTimeConvergenceCriteria_ ), - observationBias, - std::function< ObservationScalarType( const TimeType ) >( ), - std::function< ObservationScalarType( const TimeType ) >( ), - false ); + bodies, + receiver ); } - else - { - std::shared_ptr< OneWayDopplerObservationSettings > oneWayDopplerSettings = - std::dynamic_pointer_cast< OneWayDopplerObservationSettings >( - observationSettings ); - std::shared_ptr< DopplerProperTimeRateInterface > transmitterProperTimeRate = - nullptr; - std::shared_ptr< DopplerProperTimeRateInterface > receiverProperTimeRate = - nullptr; - if( oneWayDopplerSettings->transmitterProperTimeRateSettings_ != nullptr ) - { - transmitterProperTimeRate = - createOneWayDopplerProperTimeCalculator< ObservationScalarType, - TimeType >( - oneWayDopplerSettings->transmitterProperTimeRateSettings_, - linkEnds, - bodies, - transmitter ); - } - if( oneWayDopplerSettings->receiverProperTimeRateSettings_ != nullptr ) - { - receiverProperTimeRate = - createOneWayDopplerProperTimeCalculator< ObservationScalarType, - TimeType >( - oneWayDopplerSettings->receiverProperTimeRateSettings_, - linkEnds, - bodies, - receiver ); - } + // Create observation model + observationModel = std::make_shared< + OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( + linkEnds, + createLightTimeCalculator< ObservationScalarType, TimeType >( + linkEnds, + transmitter, + receiver, + bodies, + topLevelObservableType, + observationSettings->lightTimeCorrectionsList_, + observationSettings->lightTimeConvergenceCriteria_ ), + transmitterProperTimeRate, + receiverProperTimeRate, + observationBias, + oneWayDopplerSettings->normalizeWithSpeedOfLight_ ); + } - // Create observation model - observationModel = std::make_shared< - OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( - linkEnds, - createLightTimeCalculator< ObservationScalarType, TimeType >( - linkEnds, - transmitter, - receiver, - bodies, - topLevelObservableType, - observationSettings->lightTimeCorrectionsList_, - observationSettings->lightTimeConvergenceCriteria_ ), - transmitterProperTimeRate, - receiverProperTimeRate, - observationBias, - oneWayDopplerSettings->normalizeWithSpeedOfLight_ ); - } + break; + } - break; + case two_way_doppler: + { + // Check consistency input. + if( linkEnds.size( ) != 3 ) + { + std::string errorMessage = "Error when making 2 way Doppler model, " + + std::to_string( linkEnds.size( ) ) + " link ends found"; + throw std::runtime_error( errorMessage ); + } + if( linkEnds.count( receiver ) == 0 ) + { + throw std::runtime_error( + "Error when making 2 way Doppler model, no receiver found" ); } - case two_way_doppler: { - // Check consistency input. - if( linkEnds.size( ) != 3 ) - { - std::string errorMessage = "Error when making 2 way Doppler model, " + - std::to_string( linkEnds.size( ) ) + " link ends found"; - throw std::runtime_error( errorMessage ); - } - if( linkEnds.count( receiver ) == 0 ) - { - throw std::runtime_error( - "Error when making 2 way Doppler model, no receiver found" ); - } + if( linkEnds.count( reflector1 ) == 0 ) + { + throw std::runtime_error( + "Error when making 2 way Doppler model, no retransmitter found" ); + } - if( linkEnds.count( reflector1 ) == 0 ) - { - throw std::runtime_error( - "Error when making 2 way Doppler model, no retransmitter found" ); - } + if( linkEnds.count( transmitter ) == 0 ) + { + throw std::runtime_error( + "Error when making 2 way Doppler model, no transmitter found" ); + } - if( linkEnds.count( transmitter ) == 0 ) - { - throw std::runtime_error( - "Error when making 2 way Doppler model, no transmitter found" ); - } + std::shared_ptr< ObservationBias< 1 > > observationBias; + if( observationSettings->biasSettings_ != nullptr ) + { + observationBias = + createObservationBiasCalculator( linkEnds, + observationSettings->observableType_, + observationSettings->biasSettings_, + bodies ); + } - std::shared_ptr< ObservationBias< 1 > > observationBias; - if( observationSettings->biasSettings_ != nullptr ) + // Create observation model + LinkDefinition uplinkLinkEnds; + uplinkLinkEnds[ transmitter ] = linkEnds.at( transmitter ); + uplinkLinkEnds[ receiver ] = linkEnds.at( reflector1 ); + + LinkDefinition downlinkLinkEnds; + downlinkLinkEnds[ transmitter ] = linkEnds.at( reflector1 ); + downlinkLinkEnds[ receiver ] = linkEnds.at( receiver ); + + std::shared_ptr< TwoWayDopplerObservationSettings > twoWayDopplerSettings = + std::dynamic_pointer_cast< TwoWayDopplerObservationSettings >( observationSettings ); + + // Create vector of convergence criteria and light time corrections + std::vector< std::shared_ptr< LightTimeConvergenceCriteria > > singleLegsLightTimeConvergenceCriteriaList; + std::shared_ptr< LightTimeConvergenceCriteria > multiLegLightTimeConvergenceCriteria = + observationSettings->lightTimeConvergenceCriteria_; + std::vector< std::vector< std::shared_ptr< LightTimeCorrectionSettings > > > lightTimeCorrectionsList; + if ( twoWayDopplerSettings != nullptr ) + { + lightTimeCorrectionsList.push_back( twoWayDopplerSettings->uplinkOneWayDopplerSettings_->lightTimeCorrectionsList_ ); + lightTimeCorrectionsList.push_back( twoWayDopplerSettings->downlinkOneWayDopplerSettings_->lightTimeCorrectionsList_ ); + + singleLegsLightTimeConvergenceCriteriaList.push_back( twoWayDopplerSettings->uplinkOneWayDopplerSettings_->lightTimeConvergenceCriteria_ ); + singleLegsLightTimeConvergenceCriteriaList.push_back( twoWayDopplerSettings->downlinkOneWayDopplerSettings_->lightTimeConvergenceCriteria_ ); + } + else + { + for( unsigned int i = 0; i < 2; i++ ) { - observationBias = - createObservationBiasCalculator( linkEnds, - observationSettings->observableType_, - observationSettings->biasSettings_, - bodies ); + lightTimeCorrectionsList.push_back( observationSettings->lightTimeCorrectionsList_ ); + singleLegsLightTimeConvergenceCriteriaList.push_back( observationSettings->lightTimeConvergenceCriteria_ ); } + } - // Create observation model - LinkDefinition uplinkLinkEnds; - uplinkLinkEnds[ transmitter ] = linkEnds.at( transmitter ); - uplinkLinkEnds[ receiver ] = linkEnds.at( reflector1 ); - - LinkDefinition downlinkLinkEnds; - downlinkLinkEnds[ transmitter ] = linkEnds.at( reflector1 ); - downlinkLinkEnds[ receiver ] = linkEnds.at( receiver ); + // Create multi-leg light time calculator + std::shared_ptr< observation_models::MultiLegLightTimeCalculator< ObservationScalarType, TimeType > > + multiLegLightTimeCalculator = createMultiLegLightTimeCalculator< ObservationScalarType, TimeType >( + linkEnds, bodies, topLevelObservableType, lightTimeCorrectionsList ); - std::shared_ptr< TwoWayDopplerObservationSettings > twoWayDopplerSettings = - std::dynamic_pointer_cast< TwoWayDopplerObservationSettings >( - observationSettings ); + std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > uplinkDopplerCalculator; + std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > downlinkDopplerCalculator; + if ( twoWayDopplerSettings != nullptr ) + { + uplinkDopplerCalculator = std::dynamic_pointer_cast< OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( + ObservationModelCreator< 1, ObservationScalarType, TimeType >::createObservationModel( + twoWayDopplerSettings->uplinkOneWayDopplerSettings_, bodies, topLevelObservableType ) ); - std::shared_ptr< TwoWayDopplerObservationSettings > twoWayDopplerSettings = - std::dynamic_pointer_cast< TwoWayDopplerObservationSettings >( observationSettings ); + downlinkDopplerCalculator = std::dynamic_pointer_cast< OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( + ObservationModelCreator< 1, ObservationScalarType, TimeType >::createObservationModel( + twoWayDopplerSettings->downlinkOneWayDopplerSettings_, bodies, topLevelObservableType ) ); + } + else + { + uplinkDopplerCalculator = std::dynamic_pointer_cast< OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( + ObservationModelCreator< 1, ObservationScalarType, TimeType >::createObservationModel( + std::make_shared< ObservationModelSettings >( + one_way_doppler, uplinkLinkEnds, observationSettings->lightTimeCorrectionsList_ ), bodies, topLevelObservableType ) ); + downlinkDopplerCalculator = std::dynamic_pointer_cast< OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( + ObservationModelCreator< 1, ObservationScalarType, TimeType >::createObservationModel( + std::make_shared< ObservationModelSettings >( + one_way_doppler, downlinkLinkEnds, observationSettings->lightTimeCorrectionsList_ ), bodies, topLevelObservableType ) ); + } - // Create vector of convergence criteria and light time corrections - std::vector< std::shared_ptr< LightTimeConvergenceCriteria > > singleLegsLightTimeConvergenceCriteriaList; - std::shared_ptr< LightTimeConvergenceCriteria > multiLegLightTimeConvergenceCriteria = - observationSettings->lightTimeConvergenceCriteria_; - std::vector< std::vector< std::shared_ptr< LightTimeCorrectionSettings > > > lightTimeCorrectionsList; - if ( twoWayDopplerSettings != nullptr ) - { - lightTimeCorrectionsList.push_back( twoWayDopplerSettings->uplinkOneWayDopplerSettings_->lightTimeCorrectionsList_ ); - lightTimeCorrectionsList.push_back( twoWayDopplerSettings->downlinkOneWayDopplerSettings_->lightTimeCorrectionsList_ ); + bool normalizeWithSpeedOfLight = false; + observationModel = std::make_shared< TwoWayDopplerObservationModel< + ObservationScalarType, TimeType > >( + linkEnds, multiLegLightTimeCalculator, uplinkDopplerCalculator, downlinkDopplerCalculator, + observationBias, normalizeWithSpeedOfLight ); - singleLegsLightTimeConvergenceCriteriaList.push_back( twoWayDopplerSettings->uplinkOneWayDopplerSettings_->lightTimeConvergenceCriteria_ ); - singleLegsLightTimeConvergenceCriteriaList.push_back( twoWayDopplerSettings->downlinkOneWayDopplerSettings_->lightTimeConvergenceCriteria_ ); - } - else - { - for( unsigned int i = 0; i < 2; i++ ) - { - lightTimeCorrectionsList.push_back( observationSettings->lightTimeCorrectionsList_ ); - singleLegsLightTimeConvergenceCriteriaList.push_back( observationSettings->lightTimeConvergenceCriteria_ ); - } - } + break; + } + case one_way_differenced_range: + { + if( linkEnds.size( ) != 2 ) + { + std::string errorMessage = "Error when making 1 way range model, " + + std::to_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 multi-leg light time calculator - std::shared_ptr< observation_models::MultiLegLightTimeCalculator< ObservationScalarType, TimeType > > - multiLegLightTimeCalculator = createMultiLegLightTimeCalculator< ObservationScalarType, TimeType >( - linkEnds, bodies, topLevelObservableType, lightTimeCorrectionsList ); + std::shared_ptr< ObservationBias< 1 > > observationBias; + if( observationSettings->biasSettings_ != nullptr ) + { + observationBias = + createObservationBiasCalculator( linkEnds, + observationSettings->observableType_, + observationSettings->biasSettings_, + bodies ); + } - std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > uplinkDopplerCalculator; - std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > downlinkDopplerCalculator; - if ( twoWayDopplerSettings != nullptr ) - { - uplinkDopplerCalculator = std::dynamic_pointer_cast< OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( - ObservationModelCreator< 1, ObservationScalarType, TimeType >::createObservationModel( - twoWayDopplerSettings->uplinkOneWayDopplerSettings_, bodies, topLevelObservableType ) ); + // Create observation model + observationModel = std::make_shared< + OneWayDifferencedRangeObservationModel< ObservationScalarType, TimeType > >( + linkEnds, + createLightTimeCalculator< ObservationScalarType, TimeType >( + linkEnds, + transmitter, + receiver, + bodies, + topLevelObservableType, + observationSettings->lightTimeCorrectionsList_, + observationSettings->lightTimeConvergenceCriteria_ ), + createLightTimeCalculator< ObservationScalarType, TimeType >( + linkEnds, + transmitter, + receiver, + bodies, + topLevelObservableType, + observationSettings->lightTimeCorrectionsList_, + observationSettings->lightTimeConvergenceCriteria_ ), + observationBias ); - downlinkDopplerCalculator = std::dynamic_pointer_cast< OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( - ObservationModelCreator< 1, ObservationScalarType, TimeType >::createObservationModel( - twoWayDopplerSettings->downlinkOneWayDopplerSettings_, bodies, topLevelObservableType ) ); - } - else - { - uplinkDopplerCalculator = std::dynamic_pointer_cast< OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( - ObservationModelCreator< 1, ObservationScalarType, TimeType >::createObservationModel( - std::make_shared< ObservationModelSettings >( - one_way_doppler, uplinkLinkEnds, observationSettings->lightTimeCorrectionsList_ ), bodies, topLevelObservableType ) ); - downlinkDopplerCalculator = std::dynamic_pointer_cast< OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( - ObservationModelCreator< 1, ObservationScalarType, TimeType >::createObservationModel( - std::make_shared< ObservationModelSettings >( - one_way_doppler, downlinkLinkEnds, observationSettings->lightTimeCorrectionsList_ ), bodies, topLevelObservableType ) ); - } + break; + } + case n_way_range: { + // Check consistency input. + if( linkEnds.size( ) < 2 ) + { + std::string errorMessage = "Error when making n way range model, " + + std::to_string( linkEnds.size( ) ) + " link ends found"; + throw std::runtime_error( errorMessage ); + } - bool normalizeWithSpeedOfLight = false; - observationModel = std::make_shared< TwoWayDopplerObservationModel< - ObservationScalarType, TimeType > >( - linkEnds, multiLegLightTimeCalculator, uplinkDopplerCalculator, downlinkDopplerCalculator, - observationBias, normalizeWithSpeedOfLight ); + // Create observation bias object + std::shared_ptr< ObservationBias< 1 > > observationBias; + if( observationSettings->biasSettings_ != nullptr ) + { + observationBias = + createObservationBiasCalculator( linkEnds, + observationSettings->observableType_, + observationSettings->biasSettings_, + bodies ); + } - break; + std::shared_ptr< NWayRangeObservationSettings > nWayRangeObservationSettings = + std::dynamic_pointer_cast< NWayRangeObservationSettings >( + observationSettings ); + if( nWayRangeObservationSettings != nullptr && + nWayRangeObservationSettings->oneWayRangeObservationSettings_.size( ) != + linkEnds.size( ) - 1 ) + { + throw std::runtime_error( + "Error when making n-way range, input data is inconsistent" ); } - case one_way_differenced_range: - { - // std::shared_ptr< OneWayDifferencedRangeRateObservationSettings > - // rangeRateObservationSettings = - // std::dynamic_pointer_cast< - // OneWayDifferencedRangeRateObservationSettings >( - // observationSettings ); - // if( rangeRateObservationSettings == nullptr ) - // { - // throw std::runtime_error( "Error when making differenced one-way - // range rate, input type is inconsistent" ); - // } - // Check consistency input. - if( linkEnds.size( ) != 2 ) - { - std::string errorMessage = "Error when making 1 way range model, " + - std::to_string( linkEnds.size( ) ) + " link ends found"; - throw std::runtime_error( errorMessage ); - } - if( linkEnds.count( receiver ) == 0 ) + + // Create vector of convergence criteria and light time corrections + std::vector< std::shared_ptr< LightTimeConvergenceCriteria > > + singleLegsLightTimeConvergenceCriteriaList; + std::shared_ptr< LightTimeConvergenceCriteria > + multiLegLightTimeConvergenceCriteria; + std::vector< std::vector< std::shared_ptr< LightTimeCorrectionSettings > > > + lightTimeCorrectionsList; + if( nWayRangeObservationSettings != nullptr ) + { + multiLegLightTimeConvergenceCriteria = + nWayRangeObservationSettings->multiLegLightTimeConvergenceCriteria_; + for( unsigned int i = 0; i < linkEnds.size( ) - 1; i++ ) { - throw std::runtime_error( - "Error when making 1 way range model, no receiver found" ); + if( nWayRangeObservationSettings->oneWayRangeObservationSettings_.at( i ) + ->observableType_ != one_way_range ) + { + throw std::runtime_error( + "Error in n-way observable creation, constituent link is not " + "of type 1-way." ); + } + lightTimeCorrectionsList.push_back( + nWayRangeObservationSettings->oneWayRangeObservationSettings_ + .at( i ) + ->lightTimeCorrectionsList_ ); + singleLegsLightTimeConvergenceCriteriaList.push_back( + nWayRangeObservationSettings->oneWayRangeObservationSettings_ + .at( i ) + ->lightTimeConvergenceCriteria_ ); + if( nWayRangeObservationSettings->oneWayRangeObservationSettings_.at( i ) + ->biasSettings_ != nullptr ) + { + throw std::runtime_error( + "Error when creating n-way range observable, cannot process " + "single-leg bias, but setting is not null" ); + } } - if( linkEnds.count( transmitter ) == 0 ) + } + else + { + multiLegLightTimeConvergenceCriteria = + observationSettings->lightTimeConvergenceCriteria_; + for( unsigned int i = 0; i < linkEnds.size( ) - 1; i++ ) { - throw std::runtime_error( - "Error when making 1 way range model, no transmitter found" ); + lightTimeCorrectionsList.push_back( + observationSettings->lightTimeCorrectionsList_ ); + singleLegsLightTimeConvergenceCriteriaList.push_back( + observationSettings->lightTimeConvergenceCriteria_ ); } + } - std::shared_ptr< ObservationBias< 1 > > observationBias; - if( observationSettings->biasSettings_ != nullptr ) - { - observationBias = - createObservationBiasCalculator( linkEnds, - observationSettings->observableType_, - observationSettings->biasSettings_, - bodies ); - } + // Create multi-leg light time calculator + std::shared_ptr< + observation_models::MultiLegLightTimeCalculator< ObservationScalarType, + TimeType > > + multiLegLightTimeCalculator = + createMultiLegLightTimeCalculator< ObservationScalarType, + TimeType >( + linkEnds, + bodies, + topLevelObservableType, + lightTimeCorrectionsList, + singleLegsLightTimeConvergenceCriteriaList, + multiLegLightTimeConvergenceCriteria ); - // Create observation model - observationModel = std::make_shared< - OneWayDifferencedRangeObservationModel< ObservationScalarType, TimeType > >( - linkEnds, - createLightTimeCalculator< ObservationScalarType, TimeType >( - linkEnds, - transmitter, - receiver, - bodies, - topLevelObservableType, - observationSettings->lightTimeCorrectionsList_, - observationSettings->lightTimeConvergenceCriteria_ ), - createLightTimeCalculator< ObservationScalarType, TimeType >( - linkEnds, - transmitter, - receiver, - bodies, - topLevelObservableType, - observationSettings->lightTimeCorrectionsList_, - observationSettings->lightTimeConvergenceCriteria_ ), - observationBias ); + // Create observation model + observationModel = std::make_shared< + NWayRangeObservationModel< ObservationScalarType, TimeType > >( + linkEnds, multiLegLightTimeCalculator, observationBias ); + break; + } + case n_way_differenced_range: { + std::shared_ptr< NWayDifferencedRangeObservationSettings > + nWayDifferencedRangeObservationSettings = std::dynamic_pointer_cast< + NWayDifferencedRangeObservationSettings >( observationSettings ); + if( nWayDifferencedRangeObservationSettings == nullptr ) + { + throw std::runtime_error( + "Error when making n-way differenced range observation model, input " + "type inconsistent" ); + } + std::shared_ptr< NWayRangeObservationModel< ObservationScalarType, TimeType > > + arcStartObservationModel; + std::shared_ptr< NWayRangeObservationModel< ObservationScalarType, TimeType > > + arcEndObservationModel; + try + { + std::shared_ptr< ObservationModelSettings > undifferencedObservationSettings = + nWayDifferencedRangeObservationSettings + ->getUndifferencedObservationSettings( ); + + arcStartObservationModel = std::dynamic_pointer_cast< + NWayRangeObservationModel< ObservationScalarType, TimeType > >( + ObservationModelCreator< 1, ObservationScalarType, TimeType >:: + createObservationModel( undifferencedObservationSettings, + bodies, + topLevelObservableType ) ); + arcEndObservationModel = std::dynamic_pointer_cast< + NWayRangeObservationModel< ObservationScalarType, TimeType > >( + ObservationModelCreator< 1, ObservationScalarType, TimeType >:: + createObservationModel( undifferencedObservationSettings, + bodies, + topLevelObservableType ) ); + } + catch( const std::exception& caughtException ) + { + std::string exceptionText = std::string( caughtException.what( ) ); + throw std::runtime_error( + "Error when creating n-way differenced range observation model, " + "error: " + + exceptionText ); + } - break; + std::shared_ptr< ObservationBias< 1 > > observationBias; + if( observationSettings->biasSettings_ != nullptr ) + { + observationBias = + createObservationBiasCalculator( linkEnds, + observationSettings->observableType_, + observationSettings->biasSettings_, + bodies ); } - case n_way_range: { - // Check consistency input. - if( linkEnds.size( ) < 2 ) - { - std::string errorMessage = "Error when making n way range model, " + - std::to_string( linkEnds.size( ) ) + " link ends found"; - throw std::runtime_error( errorMessage ); - } - // Create observation bias object - std::shared_ptr< ObservationBias< 1 > > observationBias; - if( observationSettings->biasSettings_ != nullptr ) - { - observationBias = - createObservationBiasCalculator( linkEnds, - observationSettings->observableType_, - observationSettings->biasSettings_, - bodies ); - } + observationModel = std::make_shared< + NWayDifferencedRangeObservationModel< ObservationScalarType, TimeType > >( + linkEnds, + arcStartObservationModel, + arcEndObservationModel, + observationBias ); + break; + } + case dsn_n_way_averaged_doppler: { + std::shared_ptr< DsnNWayAveragedDopplerObservationSettings > + dsnNWayAveragedDopplerObservationSettings = std::dynamic_pointer_cast< + DsnNWayAveragedDopplerObservationSettings >( observationSettings ); + if( dsnNWayAveragedDopplerObservationSettings == nullptr ) + { + throw std::runtime_error( + "Error when creating DSN N-way averaged Doppler observation model, " + "input type " + "inconsistent." ); + } + + std::shared_ptr< NWayRangeObservationModel< ObservationScalarType, TimeType > > + arcStartObservationModel; + std::shared_ptr< NWayRangeObservationModel< ObservationScalarType, TimeType > > + arcEndObservationModel; + try + { + std::shared_ptr< ObservationModelSettings > nWayRangeObservationSettings = + dsnNWayAveragedDopplerObservationSettings + ->getNWayRangeObservationSettings( ); + + arcStartObservationModel = std::dynamic_pointer_cast< + NWayRangeObservationModel< ObservationScalarType, TimeType > >( + ObservationModelCreator< 1, ObservationScalarType, TimeType >:: + createObservationModel( nWayRangeObservationSettings, + bodies, + topLevelObservableType ) ); + arcEndObservationModel = std::dynamic_pointer_cast< + NWayRangeObservationModel< ObservationScalarType, TimeType > >( + ObservationModelCreator< 1, ObservationScalarType, TimeType >:: + createObservationModel( nWayRangeObservationSettings, + bodies, + topLevelObservableType ) ); + } + catch( const std::exception& caughtException ) + { + std::string exceptionText = std::string( caughtException.what( ) ); + throw std::runtime_error( + "Error when creating DSN N-way averaged Doppler observation model, " + "error: " + + exceptionText ); + } - std::shared_ptr< NWayRangeObservationSettings > nWayRangeObservationSettings = - std::dynamic_pointer_cast< NWayRangeObservationSettings >( - observationSettings ); - if( nWayRangeObservationSettings != nullptr && - nWayRangeObservationSettings->oneWayRangeObservationSettings_.size( ) != - linkEnds.size( ) - 1 ) + std::shared_ptr< ObservationBias< 1 > > observationBias; + if( observationSettings->biasSettings_ != nullptr ) + { + observationBias = + createObservationBiasCalculator( linkEnds, + observationSettings->observableType_, + observationSettings->biasSettings_, + bodies ); + } + + std::function< double( observation_models::FrequencyBands, + observation_models::FrequencyBands ) > + turnaroundRatioFunction = getTurnaroundFunction( bodies, linkEnds ); + + std::shared_ptr< ground_stations::StationFrequencyInterpolator > + transmittingFrequencyInterpolator = + getTransmittingFrequencyInterpolator( bodies, linkEnds ); + + std::map< LinkEndType, std::shared_ptr< ground_stations::GroundStationState > > + stationStates; + for( auto it: linkEnds ) + { + if( bodies.at( linkEnds.at( it.first ).bodyName_ ) + ->getGroundStationMap( ) + .count( linkEnds.at( it.first ).stationName_ ) > 0 ) { - throw std::runtime_error( - "Error when making n-way range, input data is inconsistent" ); + stationStates[ it.first ] = + bodies.at( linkEnds.at( it.first ).bodyName_ ) + ->getGroundStation( linkEnds.at( it.first ).stationName_ ) + ->getNominalStationState( ); } + } + + std::shared_ptr< ground_stations::GroundStationState > receivingStationState = + bodies.at( linkEnds.at( receiver ).bodyName_ ) + ->getGroundStation( linkEnds.at( receiver ).stationName_ ) + ->getNominalStationState( ); + observationModel = std::make_shared< + DsnNWayAveragedDopplerObservationModel< ObservationScalarType, TimeType > >( + linkEnds, + arcStartObservationModel, + arcEndObservationModel, + bodies.getBody( linkEnds.at( observation_models::transmitter ).bodyName_ ) + ->getGroundStation( linkEnds.at( observation_models::transmitter ) + .stationName_ ) + ->getTransmittingFrequencyCalculator( ), + turnaroundRatioFunction, + observationBias, + stationStates, + dsnNWayAveragedDopplerObservationSettings->getSubtractDopplerSignature( ) ); + break; + } + case dsn_n_way_range: + { + std::shared_ptr< DsnNWayRangeObservationSettings > dsnNWayRangeObservationSettings = + std::dynamic_pointer_cast< DsnNWayRangeObservationSettings >( + observationSettings ); + if( dsnNWayRangeObservationSettings != nullptr && + dsnNWayRangeObservationSettings->oneWayRangeObservationSettings_.size( ) != + linkEnds.size( ) - 1 ) + { + throw std::runtime_error( + "Error when making DSN N-way range, input data is inconsistent" ); + } - // Create vector of convergence criteria and light time corrections + try + { std::vector< std::shared_ptr< LightTimeConvergenceCriteria > > - singleLegsLightTimeConvergenceCriteriaList; + singleLegsLightTimeConvergenceCriteriaList; std::shared_ptr< LightTimeConvergenceCriteria > - multiLegLightTimeConvergenceCriteria; + multiLegLightTimeConvergenceCriteria; std::vector< std::vector< std::shared_ptr< LightTimeCorrectionSettings > > > - lightTimeCorrectionsList; - if( nWayRangeObservationSettings != nullptr ) + lightTimeCorrectionsList; + + if( dsnNWayRangeObservationSettings != nullptr ) { multiLegLightTimeConvergenceCriteria = - nWayRangeObservationSettings->multiLegLightTimeConvergenceCriteria_; + dsnNWayRangeObservationSettings + ->multiLegLightTimeConvergenceCriteria_; for( unsigned int i = 0; i < linkEnds.size( ) - 1; i++ ) { - if( nWayRangeObservationSettings->oneWayRangeObservationSettings_.at( i ) - ->observableType_ != one_way_range ) + if( dsnNWayRangeObservationSettings->oneWayRangeObservationSettings_ + .at( i ) + ->observableType_ != one_way_range ) { throw std::runtime_error( - "Error in n-way observable creation, constituent link is not " - "of type 1-way." ); + "Error in DSN N-way observable creation, constituent link " + "is " + "not of type 1-way." ); } lightTimeCorrectionsList.push_back( - nWayRangeObservationSettings->oneWayRangeObservationSettings_ - .at( i ) - ->lightTimeCorrectionsList_ ); + dsnNWayRangeObservationSettings->oneWayRangeObservationSettings_ + .at( i ) + ->lightTimeCorrectionsList_ ); singleLegsLightTimeConvergenceCriteriaList.push_back( - nWayRangeObservationSettings->oneWayRangeObservationSettings_ - .at( i ) - ->lightTimeConvergenceCriteria_ ); - if( nWayRangeObservationSettings->oneWayRangeObservationSettings_.at( i ) - ->biasSettings_ != nullptr ) + dsnNWayRangeObservationSettings->oneWayRangeObservationSettings_ + .at( i ) + ->lightTimeConvergenceCriteria_ ); + if( dsnNWayRangeObservationSettings->oneWayRangeObservationSettings_ + .at( i ) + ->biasSettings_ != nullptr ) { throw std::runtime_error( - "Error when creating n-way range observable, cannot process " - "single-leg bias, but setting is not null" ); + "Error when creating DSN N-way range observable, cannot " + "process single-leg bias, but setting is not null" ); } } } else { multiLegLightTimeConvergenceCriteria = - observationSettings->lightTimeConvergenceCriteria_; + observationSettings->lightTimeConvergenceCriteria_; for( unsigned int i = 0; i < linkEnds.size( ) - 1; i++ ) { lightTimeCorrectionsList.push_back( - observationSettings->lightTimeCorrectionsList_ ); + observationSettings->lightTimeCorrectionsList_ ); singleLegsLightTimeConvergenceCriteriaList.push_back( - observationSettings->lightTimeConvergenceCriteria_ ); + observationSettings->lightTimeConvergenceCriteria_ ); } } - // Create multi-leg light time calculator std::shared_ptr< - observation_models::MultiLegLightTimeCalculator< ObservationScalarType, - TimeType > > - multiLegLightTimeCalculator = - createMultiLegLightTimeCalculator< ObservationScalarType, - TimeType >( - linkEnds, - bodies, - topLevelObservableType, - lightTimeCorrectionsList, - singleLegsLightTimeConvergenceCriteriaList, - multiLegLightTimeConvergenceCriteria ); - - // Create observation model - observationModel = std::make_shared< - NWayRangeObservationModel< ObservationScalarType, TimeType > >( - linkEnds, multiLegLightTimeCalculator, observationBias ); - break; - } - case n_way_differenced_range: { - std::shared_ptr< NWayDifferencedRangeObservationSettings > - nWayDifferencedRangeObservationSettings = std::dynamic_pointer_cast< - NWayDifferencedRangeObservationSettings >( observationSettings ); - if( nWayDifferencedRangeObservationSettings == nullptr ) - { - throw std::runtime_error( - "Error when making n-way differenced range observation model, input " - "type inconsistent" ); - } - } - - std::shared_ptr< ground_stations::GroundStationState > receivingStationState = - bodies.at( linkEnds.at( receiver ).bodyName_ )->getGroundStation( linkEnds.at( receiver ).stationName_ )->getNominalStationState( ); - observationModel = std::make_shared< - DsnNWayRangeObservationModel< ObservationScalarType, TimeType > >( - linkEnds, multiLegLightTimeCalculator, - bodies.getBody( linkEnds.at( observation_models::transmitter ).bodyName_ )->getGroundStation( - linkEnds.at( observation_models::transmitter ).stationName_ )->getTransmittingFrequencyCalculator( ), - observationBias, stationStates ); - break; - } - case doppler_measured_frequency: - { - - std::shared_ptr< TwoWayDopplerObservationModel< ObservationScalarType, TimeType > > twoWayDopplerModel; - try - { - auto uplinkOneWaySettings = std::make_shared< OneWayDopplerObservationSettings >( - getUplinkFromTwoWayLinkEnds( linkEnds ), observationSettings->lightTimeCorrectionsList_, - std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Sun" ), std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Sun" ) ); - uplinkOneWaySettings->normalizeWithSpeedOfLight_ = false; - auto downlinkOneWaySettings = std::make_shared< OneWayDopplerObservationSettings >( - getDownlinkFromTwoWayLinkEnds( linkEnds ), observationSettings->lightTimeCorrectionsList_, - std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Sun" ), std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Sun" ) ); - downlinkOneWaySettings->normalizeWithSpeedOfLight_ = false; - - auto twoWaySettings = std::make_shared< TwoWayDopplerObservationSettings >( uplinkOneWaySettings, downlinkOneWaySettings ); - - twoWayDopplerModel = - std::dynamic_pointer_cast< TwoWayDopplerObservationModel< ObservationScalarType, TimeType > >( - ObservationModelCreator< 1, ObservationScalarType, TimeType >::createObservationModel( - twoWaySettings, bodies ) ); - if( twoWayDopplerModel == nullptr ) - { - std::shared_ptr< ObservationModelSettings > undifferencedObservationSettings = - nWayDifferencedRangeObservationSettings - ->getUndifferencedObservationSettings( ); - - arcStartObservationModel = std::dynamic_pointer_cast< - NWayRangeObservationModel< ObservationScalarType, TimeType > >( - ObservationModelCreator< 1, ObservationScalarType, TimeType >:: - createObservationModel( undifferencedObservationSettings, - bodies, - topLevelObservableType ) ); - arcEndObservationModel = std::dynamic_pointer_cast< - NWayRangeObservationModel< ObservationScalarType, TimeType > >( - ObservationModelCreator< 1, ObservationScalarType, TimeType >:: - createObservationModel( undifferencedObservationSettings, - bodies, - topLevelObservableType ) ); - } - catch( const std::exception& caughtException ) - { - std::string exceptionText = std::string( caughtException.what( ) ); - throw std::runtime_error( - "Error when creating n-way differenced range observation model, " - "error: " + - exceptionText ); - } - - std::shared_ptr< ObservationBias< 1 > > observationBias; - if( observationSettings->biasSettings_ != nullptr ) - { - observationBias = - createObservationBiasCalculator( linkEnds, - observationSettings->observableType_, - observationSettings->biasSettings_, - bodies ); - } - - observationModel = std::make_shared< - NWayDifferencedRangeObservationModel< ObservationScalarType, TimeType > >( + observation_models::MultiLegLightTimeCalculator< ObservationScalarType, + TimeType > > + multiLegLightTimeCalculator = + createMultiLegLightTimeCalculator< ObservationScalarType, + TimeType >( linkEnds, - arcStartObservationModel, - arcEndObservationModel, - observationBias ); - break; - } - case dsn_n_way_averaged_doppler: - { - std::shared_ptr< DsnNWayAveragedDopplerObservationSettings > - dsnNWayAveragedDopplerObservationSettings = std::dynamic_pointer_cast< - DsnNWayAveragedDopplerObservationSettings >( observationSettings ); - if( dsnNWayAveragedDopplerObservationSettings == nullptr ) - { - throw std::runtime_error( - "Error when creating DSN N-way averaged Doppler observation model, " - "input type " - "inconsistent." ); - } - - std::shared_ptr< NWayRangeObservationModel< ObservationScalarType, TimeType > > - arcStartObservationModel; - std::shared_ptr< NWayRangeObservationModel< ObservationScalarType, TimeType > > - arcEndObservationModel; - try - { - std::shared_ptr< ObservationModelSettings > nWayRangeObservationSettings = - dsnNWayAveragedDopplerObservationSettings - ->getNWayRangeObservationSettings( ); - - arcStartObservationModel = std::dynamic_pointer_cast< - NWayRangeObservationModel< ObservationScalarType, TimeType > >( - ObservationModelCreator< 1, ObservationScalarType, TimeType >:: - createObservationModel( nWayRangeObservationSettings, - bodies, - topLevelObservableType ) ); - arcEndObservationModel = std::dynamic_pointer_cast< - NWayRangeObservationModel< ObservationScalarType, TimeType > >( - ObservationModelCreator< 1, ObservationScalarType, TimeType >:: - createObservationModel( nWayRangeObservationSettings, - bodies, - topLevelObservableType ) ); - } - catch( const std::exception& caughtException ) - { - std::string exceptionText = std::string( caughtException.what( ) ); - throw std::runtime_error( - "Error when creating DSN N-way averaged Doppler observation model, " - "error: " + - exceptionText ); - } + bodies, + topLevelObservableType, + lightTimeCorrectionsList, + singleLegsLightTimeConvergenceCriteriaList, + multiLegLightTimeConvergenceCriteria ); std::shared_ptr< ObservationBias< 1 > > observationBias; if( observationSettings->biasSettings_ != nullptr ) { - observationBias = - createObservationBiasCalculator( linkEnds, - observationSettings->observableType_, - observationSettings->biasSettings_, - bodies ); + observationBias = createObservationBiasCalculator( + linkEnds, + observationSettings->observableType_, + observationSettings->biasSettings_, + bodies ); } - std::function< double( observation_models::FrequencyBands, - observation_models::FrequencyBands ) > - turnaroundRatioFunction = getTurnaroundFunction( bodies, linkEnds ); - std::shared_ptr< ground_stations::StationFrequencyInterpolator > - transmittingFrequencyInterpolator = - getTransmittingFrequencyInterpolator( bodies, linkEnds ); + transmittingFrequencyInterpolator = + getTransmittingFrequencyInterpolator( bodies, linkEnds ); std::map< LinkEndType, std::shared_ptr< ground_stations::GroundStationState > > - stationStates; + stationStates; for( auto it: linkEnds ) { if( bodies.at( linkEnds.at( it.first ).bodyName_ ) - ->getGroundStationMap( ) - .count( linkEnds.at( it.first ).stationName_ ) > 0 ) + ->getGroundStationMap( ) + .count( linkEnds.at( it.first ).stationName_ ) > 0 ) { stationStates[ it.first ] = - bodies.at( linkEnds.at( it.first ).bodyName_ ) - ->getGroundStation( linkEnds.at( it.first ).stationName_ ) - ->getNominalStationState( ); + bodies.at( linkEnds.at( it.first ).bodyName_ ) + ->getGroundStation( + linkEnds.at( it.first ).stationName_ ) + ->getNominalStationState( ); } } - std::shared_ptr< ground_stations::GroundStationState > receivingStationState = - bodies.at( linkEnds.at( receiver ).bodyName_ ) - ->getGroundStation( linkEnds.at( receiver ).stationName_ ) - ->getNominalStationState( ); observationModel = std::make_shared< - DsnNWayAveragedDopplerObservationModel< ObservationScalarType, TimeType > >( - linkEnds, - arcStartObservationModel, - arcEndObservationModel, - bodies.getBody( linkEnds.at( observation_models::transmitter ).bodyName_ ) - ->getGroundStation( linkEnds.at( observation_models::transmitter ) - .stationName_ ) - ->getTransmittingFrequencyCalculator( ), - turnaroundRatioFunction, - observationBias, - stationStates, - dsnNWayAveragedDopplerObservationSettings->getSubtractDopplerSignature( ) ); - break; + DsnNWayRangeObservationModel< ObservationScalarType, TimeType > >( + linkEnds, + multiLegLightTimeCalculator, + transmittingFrequencyInterpolator, + observationBias, + stationStates ); } - case dsn_n_way_range: { - std::shared_ptr< DsnNWayRangeObservationSettings > dsnNWayRangeObservationSettings = - std::dynamic_pointer_cast< DsnNWayRangeObservationSettings >( - observationSettings ); - if( dsnNWayRangeObservationSettings != nullptr && - dsnNWayRangeObservationSettings->oneWayRangeObservationSettings_.size( ) != - linkEnds.size( ) - 1 ) - { - throw std::runtime_error( - "Error when making DSN N-way range, input data is inconsistent" ); - } - - try - { - std::vector< std::shared_ptr< LightTimeConvergenceCriteria > > - singleLegsLightTimeConvergenceCriteriaList; - std::shared_ptr< LightTimeConvergenceCriteria > - multiLegLightTimeConvergenceCriteria; - std::vector< std::vector< std::shared_ptr< LightTimeCorrectionSettings > > > - lightTimeCorrectionsList; - - if( dsnNWayRangeObservationSettings != nullptr ) - { - multiLegLightTimeConvergenceCriteria = - dsnNWayRangeObservationSettings - ->multiLegLightTimeConvergenceCriteria_; - for( unsigned int i = 0; i < linkEnds.size( ) - 1; i++ ) - { - if( dsnNWayRangeObservationSettings->oneWayRangeObservationSettings_ - .at( i ) - ->observableType_ != one_way_range ) - { - throw std::runtime_error( - "Error in DSN N-way observable creation, constituent link " - "is " - "not of type 1-way." ); - } - lightTimeCorrectionsList.push_back( - dsnNWayRangeObservationSettings->oneWayRangeObservationSettings_ - .at( i ) - ->lightTimeCorrectionsList_ ); - singleLegsLightTimeConvergenceCriteriaList.push_back( - dsnNWayRangeObservationSettings->oneWayRangeObservationSettings_ - .at( i ) - ->lightTimeConvergenceCriteria_ ); - if( dsnNWayRangeObservationSettings->oneWayRangeObservationSettings_ - .at( i ) - ->biasSettings_ != nullptr ) - { - throw std::runtime_error( - "Error when creating DSN N-way range observable, cannot " - "process single-leg bias, but setting is not null" ); - } - } - } - else - { - multiLegLightTimeConvergenceCriteria = - observationSettings->lightTimeConvergenceCriteria_; - for( unsigned int i = 0; i < linkEnds.size( ) - 1; i++ ) - { - lightTimeCorrectionsList.push_back( - observationSettings->lightTimeCorrectionsList_ ); - singleLegsLightTimeConvergenceCriteriaList.push_back( - observationSettings->lightTimeConvergenceCriteria_ ); - } - } - - std::shared_ptr< - observation_models::MultiLegLightTimeCalculator< ObservationScalarType, - TimeType > > - multiLegLightTimeCalculator = - createMultiLegLightTimeCalculator< ObservationScalarType, - TimeType >( - linkEnds, - bodies, - topLevelObservableType, - lightTimeCorrectionsList, - singleLegsLightTimeConvergenceCriteriaList, - multiLegLightTimeConvergenceCriteria ); - - std::shared_ptr< ObservationBias< 1 > > observationBias; - if( observationSettings->biasSettings_ != nullptr ) - { - observationBias = createObservationBiasCalculator( - linkEnds, - observationSettings->observableType_, - observationSettings->biasSettings_, - bodies ); - } + catch( const std::exception& caughtException ) + { + std::string exceptionText = std::string( caughtException.what( ) ); + throw std::runtime_error( + "Error when creating DSN N-way range observation model, error: " + + exceptionText ); + } + break; + } + case doppler_measured_frequency: + { - std::shared_ptr< ground_stations::StationFrequencyInterpolator > - transmittingFrequencyInterpolator = - getTransmittingFrequencyInterpolator( bodies, linkEnds ); + std::shared_ptr< TwoWayDopplerObservationModel< ObservationScalarType, TimeType > > twoWayDopplerModel; + try + { - std::map< LinkEndType, std::shared_ptr< ground_stations::GroundStationState > > - stationStates; - for( auto it: linkEnds ) - { - if( bodies.at( linkEnds.at( it.first ).bodyName_ ) - ->getGroundStationMap( ) - .count( linkEnds.at( it.first ).stationName_ ) > 0 ) - { - stationStates[ it.first ] = - bodies.at( linkEnds.at( it.first ).bodyName_ ) - ->getGroundStation( - linkEnds.at( it.first ).stationName_ ) - ->getNominalStationState( ); - } - } + std::shared_ptr< OneWayDopplerObservationSettings > uplinkOneWayDopplerSettings; + std::shared_ptr< OneWayDopplerObservationSettings > downlinkOneWayDopplerSettings; - observationModel = std::make_shared< - DsnNWayRangeObservationModel< ObservationScalarType, TimeType > >( - linkEnds, - multiLegLightTimeCalculator, - transmittingFrequencyInterpolator, - observationBias, - stationStates ); - } - catch( const std::exception& caughtException ) + if( std::dynamic_pointer_cast< TwoWayDopplerObservationSettings >( observationSettings ) != nullptr ) { - std::string exceptionText = std::string( caughtException.what( ) ); - throw std::runtime_error( - "Error when creating DSN N-way range observation model, error: " + - exceptionText ); - } - break; - } - case doppler_measured_frequency: { - std::shared_ptr< TwoWayDopplerObservationModel< ObservationScalarType, TimeType > > - twoWayDopplerModel; - try - { - auto uplinkOneWaySettings = std::make_shared< - OneWayDopplerObservationSettings >( - getUplinkFromTwoWayLinkEnds( linkEnds ), - observationSettings->lightTimeCorrectionsList_, - std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( - "Earth" ), - nullptr ); - uplinkOneWaySettings->normalizeWithSpeedOfLight_ = false; - auto downlinkOneWaySettings = std::make_shared< - OneWayDopplerObservationSettings >( - getDownlinkFromTwoWayLinkEnds( linkEnds ), - observationSettings->lightTimeCorrectionsList_, - nullptr, - std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( - "Earth" ) ); - downlinkOneWaySettings->normalizeWithSpeedOfLight_ = false; - - auto twoWaySettings = std::make_shared< TwoWayDopplerObservationSettings >( - uplinkOneWaySettings, downlinkOneWaySettings ); - - twoWayDopplerModel = std::dynamic_pointer_cast< - TwoWayDopplerObservationModel< ObservationScalarType, TimeType > >( - ObservationModelCreator< 1, ObservationScalarType, TimeType >:: - createObservationModel( twoWaySettings, bodies ) ); - if( twoWayDopplerModel == nullptr ) - { - throw std::runtime_error( - "Error when getting two-way Doppler model for measured frequency " - "observable, model in null." ); - } + std::shared_ptr< TwoWayDopplerObservationSettings > twoWayFrequencySettings = + std::dynamic_pointer_cast< TwoWayDopplerObservationSettings >( observationSettings ); + uplinkOneWayDopplerSettings = twoWayFrequencySettings->uplinkOneWayDopplerSettings_; + downlinkOneWayDopplerSettings = twoWayFrequencySettings->downlinkOneWayDopplerSettings_; } - catch( const std::exception& caughtException ) + else { - std::string exceptionText = std::string( caughtException.what( ) ); - throw std::runtime_error( - "Error when creating Doppler Measured Frequency observation model, " - "error: " + - exceptionText ); + uplinkOneWayDopplerSettings = std::make_shared< OneWayDopplerObservationSettings >( + getUplinkFromTwoWayLinkEnds( linkEnds ), observationSettings->lightTimeCorrectionsList_, nullptr, nullptr ); + uplinkOneWayDopplerSettings->normalizeWithSpeedOfLight_ = false; + downlinkOneWayDopplerSettings = std::make_shared< OneWayDopplerObservationSettings >( + getDownlinkFromTwoWayLinkEnds( linkEnds ), observationSettings->lightTimeCorrectionsList_, nullptr, nullptr ); + downlinkOneWayDopplerSettings->normalizeWithSpeedOfLight_ = false; + } + std::shared_ptr< ObservationModelSettings > twoWaySettings = + std::make_shared< TwoWayDopplerObservationSettings >( uplinkOneWayDopplerSettings, downlinkOneWayDopplerSettings, two_way_doppler ); - // Determine the bias settings - std::shared_ptr< ObservationBias< 1 > > observationBias; - if( observationSettings->biasSettings_ != nullptr ) + + twoWayDopplerModel = + std::dynamic_pointer_cast< TwoWayDopplerObservationModel< ObservationScalarType, TimeType > >( + ObservationModelCreator< 1, ObservationScalarType, TimeType >::createObservationModel( + twoWaySettings, bodies ) ); + if( twoWayDopplerModel == nullptr ) { - observationBias = - createObservationBiasCalculator( linkEnds, - observationSettings->observableType_, - observationSettings->biasSettings_, - bodies ); + throw std::runtime_error( "Error when getting two-way Doppler model for measured frequency observable, model in null." ); } + } + catch( const std::exception& caughtException ) + { + std::string exceptionText = std::string( caughtException.what( ) ); + throw std::runtime_error( "Error when creating Doppler Measured Frequency observation model, error: " + + exceptionText ); + } - // Determine the turnaround ratio function - std::function< double( observation_models::FrequencyBands, - observation_models::FrequencyBands ) > - turnaroundRatioFunction = getTurnaroundFunction( bodies, linkEnds ); + // Determine the bias settings + std::shared_ptr< ObservationBias< 1 > > observationBias; + if( observationSettings->biasSettings_ != nullptr ) + { + observationBias = createObservationBiasCalculator( + linkEnds, observationSettings->observableType_, observationSettings->biasSettings_, bodies ); + } - // Check if transmitter has frequency calculator - std::shared_ptr< ground_stations::StationFrequencyInterpolator > - transmittingFrequencyInterpolator = - getTransmittingFrequencyInterpolator( bodies, linkEnds ); + // Determine the turnaround ratio function + std::function< double ( observation_models::FrequencyBands, observation_models::FrequencyBands ) > turnaroundRatioFunction = + getTurnaroundFunction( bodies, linkEnds ); - std::map< LinkEndType, std::shared_ptr< ground_stations::GroundStationState > > - stationStates; - for( auto it: linkEnds ) + // Check if transmitter has frequency calculator + std::shared_ptr< ground_stations::StationFrequencyInterpolator > transmittingFrequencyInterpolator = + getTransmittingFrequencyInterpolator( bodies, linkEnds ); + + std::map< LinkEndType, std::shared_ptr< ground_stations::GroundStationState > > stationStates; + for( auto it : linkEnds ) + { + if( bodies.at( linkEnds.at( it.first ).bodyName_ )->getGroundStationMap( ).count( linkEnds.at( it.first ).stationName_ ) > 0 ) { - if( bodies.at( linkEnds.at( it.first ).bodyName_ ) - ->getGroundStationMap( ) - .count( linkEnds.at( it.first ).stationName_ ) > 0 ) - { - stationStates[ it.first ] = - bodies.at( linkEnds.at( it.first ).bodyName_ ) - ->getGroundStation( linkEnds.at( it.first ).stationName_ ) - ->getNominalStationState( ); - } + stationStates[ it.first ] = + bodies.at( linkEnds.at( it.first ).bodyName_ )->getGroundStation( linkEnds.at( it.first ).stationName_ )->getNominalStationState( ); } + } - observationModel = std::make_shared< - DopplerMeasuredFrequencyObservationModel< ObservationScalarType, - TimeType > >( - linkEnds, - twoWayDopplerModel, - transmittingFrequencyInterpolator, - turnaroundRatioFunction, - observationBias, - stationStates ); + observationModel = std::make_shared< + DopplerMeasuredFrequencyObservationModel< ObservationScalarType, TimeType > >( + linkEnds, twoWayDopplerModel, + transmittingFrequencyInterpolator, + turnaroundRatioFunction, + observationBias, + stationStates ); - break; - } - default: - std::string errorMessage = "Error, observable " + - std::to_string( observationSettings->observableType_ ) + - " not recognized when making size 1 observation model."; - throw std::runtime_error( errorMessage ); + break; + } + default: + std::string errorMessage = "Error, observable " + + std::to_string( observationSettings->observableType_ ) + + " not recognized when making size 1 observation model."; + throw std::runtime_error( errorMessage ); } return observationModel; diff --git a/include/tudat/simulation/estimation_setup/createPositionPartialScaling.h b/include/tudat/simulation/estimation_setup/createPositionPartialScaling.h index bd96bd4061..59a7d506c6 100644 --- a/include/tudat/simulation/estimation_setup/createPositionPartialScaling.h +++ b/include/tudat/simulation/estimation_setup/createPositionPartialScaling.h @@ -49,9 +49,10 @@ inline std::shared_ptr< OneWayDopplerProperTimeComponentScaling > createDopplerP else if( std::dynamic_pointer_cast< observation_models::DirectFirstOrderDopplerProperTimeRateInterface >( dopplerProperTimeInterface ) != nullptr ) { - bool computeStatePartials = ( oneWayDopplerLinkEnds.at( linkEndAtWhichPartialIsComputed ).bodyName_ != - std::dynamic_pointer_cast< observation_models::DirectFirstOrderDopplerProperTimeRateInterface >( - dopplerProperTimeInterface )->getCentralBody( ) ); + std::cerr<<"Warning, disconnected Doppler partials"<( +// dopplerProperTimeInterface )->getCentralBody( ) ); properTimeRateDopplerPartial = std::make_shared< OneWayDopplerDirectFirstOrderProperTimeComponentScaling >( std::dynamic_pointer_cast< observation_models::DirectFirstOrderDopplerProperTimeRateInterface >( dopplerProperTimeInterface ), linkEndAtWhichPartialIsComputed, computeStatePartials ); diff --git a/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp b/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp index 0261fa285a..0c02aeccaa 100644 --- a/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp +++ b/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp @@ -50,24 +50,26 @@ void OneWayDopplerDirectFirstOrderProperTimeComponentScaling::update( const std: const observation_models::LinkEndType fixedLinkEnd, const Eigen::VectorXd currentObservation ) { - // Get relative state - Eigen::Vector6d relativeState = properTimeRateModel_->getComputationPointRelativeState( - times, linkEndStates ); - currentDistance_ = relativeState.segment( 0, 3 ).norm( ); - currentGravitationalParameter_ = properTimeRateModel_->getGravitationalParameter( ); - - currentLinkEndTime_ == ( linkEndWithPartial_ == observation_models::transmitter ) ? ( times.at( 0 ) ) : ( times.at( 1 ) ); - - // Compute partials w.r.t. position and velocity - if( computeStatePartials_ ) - { - partialWrPosition_ = -physical_constants::INVERSE_SQUARE_SPEED_OF_LIGHT * - ( 1.0 + relativity::equivalencePrincipleLpiViolationParameter ) * - currentGravitationalParameter_ / ( currentDistance_ * currentDistance_ ) * - ( relativeState.segment( 0, 3 ).normalized( ) ).transpose( ); - partialWrtVelocity_ = physical_constants::INVERSE_SQUARE_SPEED_OF_LIGHT * - ( relativeState.segment( 3, 3 ) ).transpose( ); - } +// // Get relative state +// Eigen::Vector6d relativeState = properTimeRateModel_->getComputationPointRelativeState( +// times, linkEndStates ); +// currentDistance_ = relativeState.segment( 0, 3 ).norm( ); +// currentGravitationalParameter_ = properTimeRateModel_->getGravitationalParameter( ); +// +// currentLinkEndTime_ == ( linkEndWithPartial_ == observation_models::transmitter ) ? ( times.at( 0 ) ) : ( times.at( 1 ) ); +// +// // Compute partials w.r.t. position and velocity +// if( computeStatePartials_ ) +// { +// partialWrPosition_ = -physical_constants::INVERSE_SQUARE_SPEED_OF_LIGHT * +// ( 1.0 + relativity::equivalencePrincipleLpiViolationParameter ) * +// currentGravitationalParameter_ / ( currentDistance_ * currentDistance_ ) * +// ( relativeState.segment( 0, 3 ).normalized( ) ).transpose( ); +// partialWrtVelocity_ = physical_constants::INVERSE_SQUARE_SPEED_OF_LIGHT * +// ( relativeState.segment( 3, 3 ) ).transpose( ); +// } + partialWrPosition_.setZero( ); + partialWrtVelocity_.setZero( ); } //! Function to retrieve the scaling factor for the derivative w.r.t. the position of a given link end diff --git a/src/astro/relativity/relativisticLightTimeCorrection.cpp b/src/astro/relativity/relativisticLightTimeCorrection.cpp index 8b46f314ed..a4e8550ece 100644 --- a/src/astro/relativity/relativisticLightTimeCorrection.cpp +++ b/src/astro/relativity/relativisticLightTimeCorrection.cpp @@ -60,7 +60,7 @@ Eigen::Matrix< double, 1, 3 > calculateFirstOrderCentralBodyLightTimeCorrectionG gradient += relativePositionVector.norm( ) * ( transmitterPosition.normalized( ) ).transpose( );\ } - return 2.0 * ppnParameterGamma * bodyGravitationalParameter * physical_constants::INVERSE_CUBIC_SPEED_OF_LIGHT * gradient / + return 2.0 * ( 1.0 + ppnParameterGamma ) * bodyGravitationalParameter * physical_constants::INVERSE_CUBIC_SPEED_OF_LIGHT * gradient / ( ( receiverDistance + transmitterDistance ) * ( receiverDistance + transmitterDistance ) - linkEndDistance * linkEndDistance ); diff --git a/src/astro/relativity/relativisticTimeConversion.cpp b/src/astro/relativity/relativisticTimeConversion.cpp index 1d527fdfb8..64d54192e2 100644 --- a/src/astro/relativity/relativisticTimeConversion.cpp +++ b/src/astro/relativity/relativisticTimeConversion.cpp @@ -19,21 +19,30 @@ namespace relativity //! Function to compute proper-time rate w.r.t. coordinate time, minus 1.0, from a speed and scalar potential double calculateFirstCentralBodyProperTimeRateDifference( - const double relativeSpeed, const double gravitationalScalarPotential, + const double computationPointSpeed, + double gravitationalScalarPotential, const double equivalencePrincipleLpiViolationParameter ) { - return ( -( 0.5 * relativeSpeed * relativeSpeed + ( 1.0 + equivalencePrincipleLpiViolationParameter ) * + return ( -( 0.5 * computationPointSpeed * computationPointSpeed + ( 1.0 + equivalencePrincipleLpiViolationParameter ) * gravitationalScalarPotential ) ) * physical_constants::INVERSE_SQUARE_SPEED_OF_LIGHT; } //! Function to compute proper-time rate w.r.t. coordinate time, minus 1.0, from a speed and scalar potential double calculateFirstCentralBodyProperTimeRateDifference( - const Eigen::Vector6d relativeStateVector, const double centralBodyGravitationalParameter, - const double equivalencePrincipleLpiViolationParameter ) + const Eigen::Vector6d& computationPointState, + const std::vector< Eigen::Vector6d >& perturbedInertialStates, + const std::vector< double >& centralBodyGravitationalParameters, + const double equivalencePrincipleLpiViolationParameter ) { + double gravitationalScalarPotential = 0.0; + for( unsigned int i = 0; i < perturbedInertialStates.size( ); i++ ) + { + gravitationalScalarPotential += + centralBodyGravitationalParameters.at( i ) / + ( perturbedInertialStates.at( i ).segment( 0, 3 ) - computationPointState.segment( 0, 3 ) ).norm( ); + } return calculateFirstCentralBodyProperTimeRateDifference( - relativeStateVector.segment( 3, 3 ).norm( ), centralBodyGravitationalParameter / - relativeStateVector.segment( 0, 3 ).norm( ), equivalencePrincipleLpiViolationParameter ); + computationPointState.segment( 3, 3 ).norm( ), gravitationalScalarPotential, equivalencePrincipleLpiViolationParameter ); } } diff --git a/tests/src/astro/observation_models/unitTestDopplerMeasuredFrequencyObservationModel.cpp b/tests/src/astro/observation_models/unitTestDopplerMeasuredFrequencyObservationModel.cpp index 5e8350c1ba..62258b2afe 100644 --- a/tests/src/astro/observation_models/unitTestDopplerMeasuredFrequencyObservationModel.cpp +++ b/tests/src/astro/observation_models/unitTestDopplerMeasuredFrequencyObservationModel.cpp @@ -128,6 +128,9 @@ BOOST_AUTO_TEST_CASE(testJuiceMeasuredFrequency) // Create observation settings std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList; lightTimeCorrectionsList.push_back( std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( std::vector< std::string >( { "Sun", "Moon", "Earth" } ) ) ); + + std::shared_ptr< ObservationModelSettings > observationModelSettings = dopplerMeasuredFrequencyObservationSettings( + linkEnds, std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( std::vector< std::string >{ "Sun", "Moon", "Earth" } ) , lightTimeCorrectionsList ); std::shared_ptr > dopplerFrequencyObservationModel = std::dynamic_pointer_cast>( ObservationModelCreator<1, double, Time>::createObservationModel( diff --git a/tests/src/astro/orbit_determination/observation_partials/unitTestOneWayDopplerPartials.cpp b/tests/src/astro/orbit_determination/observation_partials/unitTestOneWayDopplerPartials.cpp index 9fa4d15ae7..ab3765da94 100644 --- a/tests/src/astro/orbit_determination/observation_partials/unitTestOneWayDopplerPartials.cpp +++ b/tests/src/astro/orbit_determination/observation_partials/unitTestOneWayDopplerPartials.cpp @@ -219,6 +219,7 @@ BOOST_AUTO_TEST_CASE( testOneWayDopplerPartials ) for( unsigned int estimationCase = 0; estimationCase < 3; estimationCase ++ ) { + std::cout<<"ESTIMATION CASE *********************************** "< > oneWayDopplerModel; std::vector< std::string > perturbingBodies; @@ -280,7 +281,7 @@ BOOST_AUTO_TEST_CASE( testOneWayDopplerPartials ) for( unsigned int estimationCase = 0; estimationCase < 3; estimationCase ++ ) { - std::cout << "Rates: " << estimationCase << std::endl; + std::cout<<"ESTIMATION CASE *********************************** "<<" "< > oneWayDopplerModel; std::vector< std::string > perturbingBodies; From 935e68cc51b53cb8a08231e4ceb67f341870aff7 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Sat, 14 Dec 2024 11:33:09 +0100 Subject: [PATCH 05/24] Corrected build --- .../tudat/simulation/estimation_setup/createObservationModel.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/tudat/simulation/estimation_setup/createObservationModel.h b/include/tudat/simulation/estimation_setup/createObservationModel.h index 148761caef..d02510218c 100644 --- a/include/tudat/simulation/estimation_setup/createObservationModel.h +++ b/include/tudat/simulation/estimation_setup/createObservationModel.h @@ -714,7 +714,7 @@ class TwoWayDopplerObservationSettings : public ObservationModelSettings TwoWayDopplerObservationSettings( const std::shared_ptr< OneWayDopplerObservationSettings > uplinkOneWayDopplerSettings, const std::shared_ptr< OneWayDopplerObservationSettings > downlinkOneWayDopplerSettings, - const ObservableType observableType, + const ObservableType observableType = two_way_doppler, const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr ): ObservationModelSettings( observableType, mergeUpDownLink( uplinkOneWayDopplerSettings->linkEnds_, downlinkOneWayDopplerSettings->linkEnds_ ), From b2a20725c358eef393c9e30f9baae6507630a840 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Sat, 14 Dec 2024 12:29:16 +0100 Subject: [PATCH 06/24] Compilation correction --- .../observation_models/lightTimeSolution.h | 20 +++++++++++------- .../oneWayDopplerObservationModel.h | 17 ++++++++++----- .../createPositionPartialScaling.h | 7 +++---- .../oneWayDopplerPartial.cpp | 4 ++-- .../unitTestDopplerModels.cpp | 21 ++++++++++--------- 5 files changed, 40 insertions(+), 29 deletions(-) diff --git a/include/tudat/astro/observation_models/lightTimeSolution.h b/include/tudat/astro/observation_models/lightTimeSolution.h index e5f3551437..39eae8737e 100644 --- a/include/tudat/astro/observation_models/lightTimeSolution.h +++ b/include/tudat/astro/observation_models/lightTimeSolution.h @@ -633,25 +633,29 @@ class LightTimeCalculator * \param receiverTime Time at reiver. * \param isPartialWrtReceiver If partial is to be calculated w.r.t. receiver or transmitter. */ - Eigen::Matrix< ObservationScalarType, 1, 3 > getPartialOfLightTimeWrtLinkEndPosition( - const StateType& transmitterState, - const StateType& receiverState, - const TimeType transmitterTime, - const TimeType receiverTime, + Eigen::Matrix< ObservationScalarType, 1, 3 > getPartialOfLightTimeWrtLinkEndPosition( + const Eigen::Vector6d& transmitterState, + const Eigen::Vector6d& receiverState, + const double transmitterTime, + const double receiverTime, const bool isPartialWrtReceiver, const std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > ancillarySettings = nullptr ) { - setTotalLightTimeCorrection( transmitterState, receiverState, transmitterTime, receiverTime, ancillarySettings ); + setTotalLightTimeCorrection( + transmitterState.template cast< ObservationScalarType >( ), + receiverState.template cast< ObservationScalarType >( ), transmitterTime, receiverTime, ancillarySettings ); Eigen::Matrix< ObservationScalarType, 3, 1 > relativePosition = - receiverState.segment( 0, 3 ) - transmitterState.segment( 0, 3 ); + ( receiverState.segment( 0, 3 ) - transmitterState.segment( 0, 3 ) ).template cast< ObservationScalarType >( ); Eigen::Matrix< ObservationScalarType, 1, 3 > partialWrtLinkEndPosition = ( relativePosition.normalized( ) ).transpose( ) * ( isPartialWrtReceiver ? mathematical_constants::getFloatingInteger< ObservationScalarType >( 1 ) : mathematical_constants::getFloatingInteger< ObservationScalarType >( -1 ) ); for( unsigned int i = 0; i < correctionFunctions_.size( ); i++ ) { partialWrtLinkEndPosition += correctionFunctions_.at( i )->calculateLightTimeCorrectionPartialDerivativeWrtLinkEndPosition( - transmitterState, receiverState, transmitterTime, receiverTime, isPartialWrtReceiver ? receiver : transmitter ); + transmitterState.template cast< double >( ), + receiverState.template cast< double >( ), + transmitterTime, receiverTime, isPartialWrtReceiver ? receiver : transmitter ).template cast< ObservationScalarType >( ); } return partialWrtLinkEndPosition; } diff --git a/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h b/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h index a7fc8d3913..efb84346ff 100644 --- a/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h +++ b/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h @@ -408,10 +408,17 @@ class DirectFirstOrderDopplerProperTimeRateInterface: // * Function to return the name of body generating the gravity field // * \return Name of body generating the gravity field // */ -// std::string getCentralBody( ) -// { -// return referenceBodies_; -// } + bool matchWithBody( const std::string bodyName ) + { + for( unsigned int i = 0; i < perturbingBodyNames_.size( ); i++ ) + { + if( bodyName == perturbingBodyNames_.at( i ) ) + { + return true; + } + } + return false; + } private: @@ -623,7 +630,7 @@ class OneWayDopplerObservationModel: public ObservationModel< 1, ObservationScal ObservationScalarType firstOrderDopplerObservable = computeOneWayFirstOrderDopplerTaylorSeriesExpansion< ObservationScalarType >( - linkEndStates.at( 0 ), linkEndStates.at( 1 ), + linkEndStates.at( 0 ).template cast< ObservationScalarType >( ), linkEndStates.at( 1 ).template cast< ObservationScalarType >( ), lightTimePartialWrtTransmitterPosition_, lightTimePartialWrtReceiverPosition_, taylorSeriesExpansionOrder_ ); diff --git a/include/tudat/simulation/estimation_setup/createPositionPartialScaling.h b/include/tudat/simulation/estimation_setup/createPositionPartialScaling.h index 59a7d506c6..d54b4ae85a 100644 --- a/include/tudat/simulation/estimation_setup/createPositionPartialScaling.h +++ b/include/tudat/simulation/estimation_setup/createPositionPartialScaling.h @@ -49,10 +49,9 @@ inline std::shared_ptr< OneWayDopplerProperTimeComponentScaling > createDopplerP else if( std::dynamic_pointer_cast< observation_models::DirectFirstOrderDopplerProperTimeRateInterface >( dopplerProperTimeInterface ) != nullptr ) { - std::cerr<<"Warning, disconnected Doppler partials"<( -// dopplerProperTimeInterface )->getCentralBody( ) ); + bool computeStatePartials = + !( std::dynamic_pointer_cast< observation_models::DirectFirstOrderDopplerProperTimeRateInterface >( + dopplerProperTimeInterface )->matchWithBody( oneWayDopplerLinkEnds.at( linkEndAtWhichPartialIsComputed ).bodyName_ ) ); properTimeRateDopplerPartial = std::make_shared< OneWayDopplerDirectFirstOrderProperTimeComponentScaling >( std::dynamic_pointer_cast< observation_models::DirectFirstOrderDopplerProperTimeRateInterface >( dopplerProperTimeInterface ), linkEndAtWhichPartialIsComputed, computeStatePartials ); diff --git a/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp b/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp index 0c02aeccaa..38b3b5b411 100644 --- a/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp +++ b/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp @@ -50,8 +50,8 @@ void OneWayDopplerDirectFirstOrderProperTimeComponentScaling::update( const std: const observation_models::LinkEndType fixedLinkEnd, const Eigen::VectorXd currentObservation ) { -// // Get relative state -// Eigen::Vector6d relativeState = properTimeRateModel_->getComputationPointRelativeState( + // Get relative state +// Eigen::Vector6d relativeState = properTimeRateModel_->getComputationPointState( // times, linkEndStates ); // currentDistance_ = relativeState.segment( 0, 3 ).norm( ); // currentGravitationalParameter_ = properTimeRateModel_->getGravitationalParameter( ); diff --git a/tests/src/astro/observation_models/unitTestDopplerModels.cpp b/tests/src/astro/observation_models/unitTestDopplerModels.cpp index 9007f88cf8..c11a9cb8e8 100644 --- a/tests/src/astro/observation_models/unitTestDopplerModels.cpp +++ b/tests/src/astro/observation_models/unitTestDopplerModels.cpp @@ -247,23 +247,24 @@ BOOST_AUTO_TEST_CASE( testOneWayDoppplerModel ) std::shared_ptr< RotationalEphemeris > earthRotationModel = bodies.at( "Earth" )->getRotationalEphemeris( ); - Eigen::Vector3d groundStationVelocityVector = - earthRotationModel->getDerivativeOfRotationToTargetFrame( observationTime ) * + Eigen::Vector6d groundStationGeocentricState; + groundStationGeocentricState.segment( 0, 3 ) = stationCartesianPosition; + groundStationGeocentricState.segment( 3, 3 ) = earthRotationModel->getDerivativeOfRotationToTargetFrame( observationTime ) * ( earthRotationModel->getRotationToBaseFrame( observationTime ) * stationCartesianPosition ); + Eigen::Vector6d groundStationState = groundStationGeocentricState; + groundStationState += bodies.at( "Earth" )->getStateInBaseFrameFromEphemeris( observationTime ); - std::shared_ptr< Ephemeris > spacecraftEphemeris = - bodies.at( "Spacecraft" )->getEphemeris( ); - Eigen::Vector6d spacecraftState = - spacecraftEphemeris->getCartesianState( observationTime ); - + Eigen::Vector6d spacecraftState = bodies.at( "Spacecraft" )->getStateInBaseFrameFromEphemeris( observationTime ); + Eigen::Vector6d spacecraftGeocentricState = bodies.at( "Spacecraft" )->getStateInBaseFrameFromEphemeris( observationTime ) - + bodies.at( "Earth" )->getStateInBaseFrameFromEphemeris( observationTime ); long double groundStationProperTimeRate = 1.0L - static_cast< long double >( physical_constants::INVERSE_SQUARE_SPEED_OF_LIGHT * - ( 0.5 * std::pow( groundStationVelocityVector.norm( ), 2 ) + - earthGravitationalParameter / stationCartesianPosition.norm( ) ) ); + ( 0.5 * std::pow( groundStationState.segment( 3, 3 ).norm( ), 2 ) + + earthGravitationalParameter / groundStationGeocentricState.segment( 0, 3 ).norm( ) ) ); long double spacecraftProperTimeRate = 1.0L - static_cast< long double >( physical_constants::INVERSE_SQUARE_SPEED_OF_LIGHT * ( 0.5 * std::pow( spacecraftState.segment( 3, 3 ).norm( ), 2 ) + - earthGravitationalParameter / spacecraftState.segment( 0, 3 ).norm( ) ) ); + earthGravitationalParameter / spacecraftGeocentricState.segment( 0, 3 ).norm( ) ) ); long double manualDopplerValue = ( groundStationProperTimeRate * From 7249b2e1c8f2b4ff350668a63e45e75d230aa573 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Mon, 16 Dec 2024 11:49:31 +0100 Subject: [PATCH 07/24] Fixed one-way doppler --- .../oneWayDopplerObservationModel.h | 14 ++++++++++- .../relativity/relativisticTimeConversion.cpp | 3 ++- .../unitTestDopplerModels.cpp | 25 ++++++++++++------- 3 files changed, 31 insertions(+), 11 deletions(-) diff --git a/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h b/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h index efb84346ff..5fbf2a2011 100644 --- a/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h +++ b/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h @@ -346,6 +346,10 @@ class DirectFirstOrderDopplerProperTimeRateInterface: Eigen::Vector6d computationPointRelativeState = getComputationPointState( linkEndTimes, linkEndStates ); setPerturbedProperties( linkEndTimes, linkEndStates ); + std::cout<<"COMPUTATION POINT STATE "<perturbingBodyMatchLinkEnds_.at( i ) == transmitter ) ? linkEndTimes.at( 0 ) : linkEndTimes.at( 1 ); } + else + { + evaluationTime = ( this->perturbingBodyMatchLinkEnds_.at( i ) == transmitter ) ? linkEndTimes.at( 1 ) : linkEndTimes.at( 0 ); + } + + std::cout<<"LINK ENDS "<perturbingBodyMatchLinkEnds_.at( i )<<" "< #include "tudat/astro/basic_astro/physicalConstants.h" #include "tudat/astro/relativity/relativisticTimeConversion.h" @@ -37,6 +37,7 @@ double calculateFirstCentralBodyProperTimeRateDifference( double gravitationalScalarPotential = 0.0; for( unsigned int i = 0; i < perturbedInertialStates.size( ); i++ ) { + std::cout<<"Perturber relative state "<<( perturbedInertialStates.at( i ).segment( 0, 3 ) - computationPointState.segment( 0, 3 ) ).transpose( )<setEphemeris( createBodyEphemeris( std::make_shared< KeplerEphemerisSettings >( - spacecraftOrbitalElements, 0.0, earthGravitationalParameter, "Earth" ), "Spacecraft" ) ); + spacecraftOrbitalElements, 0.0, earthGravitationalParameter, "Earth", "ECLIPJ2000" ), "Spacecraft" ) ); bodies.processBodyFrameDefinitions( ); @@ -242,21 +242,27 @@ BOOST_AUTO_TEST_CASE( testOneWayDoppplerModel ) double observationWithoutCorrections = observationModelWithoutCorrections->computeIdealObservations( observationTime, receiver ).x( ); + std::cout<<"************* COMPUTING WITH CORRECTIONS "<computeIdealObservations( observationTime, receiver ).x( ); std::shared_ptr< RotationalEphemeris > earthRotationModel = bodies.at( "Earth" )->getRotationalEphemeris( ); - Eigen::Vector6d groundStationGeocentricState; - groundStationGeocentricState.segment( 0, 3 ) = stationCartesianPosition; - groundStationGeocentricState.segment( 3, 3 ) = earthRotationModel->getDerivativeOfRotationToTargetFrame( observationTime ) * - ( earthRotationModel->getRotationToBaseFrame( observationTime ) * stationCartesianPosition ); + Eigen::Vector6d groundStationEarthFixedState = Eigen::Vector6d::Zero( ); + groundStationEarthFixedState.segment( 0, 3 ) = stationCartesianPosition; + Eigen::Vector6d groundStationGeocentricState = ephemerides::transformStateToInertialOrientation( + groundStationEarthFixedState, observationTime, earthRotationModel ); + Eigen::Vector6d groundStationState = groundStationGeocentricState; groundStationState += bodies.at( "Earth" )->getStateInBaseFrameFromEphemeris( observationTime ); Eigen::Vector6d spacecraftState = bodies.at( "Spacecraft" )->getStateInBaseFrameFromEphemeris( observationTime ); Eigen::Vector6d spacecraftGeocentricState = bodies.at( "Spacecraft" )->getStateInBaseFrameFromEphemeris( observationTime ) - bodies.at( "Earth" )->getStateInBaseFrameFromEphemeris( observationTime ); + + std::cout<<"States "<( physical_constants::INVERSE_SQUARE_SPEED_OF_LIGHT * ( 0.5 * std::pow( groundStationState.segment( 3, 3 ).norm( ), 2 ) + @@ -271,8 +277,9 @@ BOOST_AUTO_TEST_CASE( testOneWayDoppplerModel ) ( 1.0L + static_cast< long double >( observationWithoutCorrections ) / physical_constants::SPEED_OF_LIGHT ) / spacecraftProperTimeRate - 1.0L ) * physical_constants::SPEED_OF_LIGHT; - BOOST_CHECK_SMALL( std::fabs( static_cast< double >( manualDopplerValue ) - observationWithCorrections ), - static_cast< double >( std::numeric_limits< long double >::epsilon( ) * physical_constants::SPEED_OF_LIGHT ) ); + std::cout<<"PROPER TIME RATES "<( manualDopplerValue ) - observationWithCorrections ), 1.0E-6 ); } } @@ -329,7 +336,7 @@ BOOST_AUTO_TEST_CASE( testTwoWayDoppplerModel ) spacecraftOrbitalElements, 0.0, earthGravitationalParameter, "Earth" ), "Spacecraft" ) ); bodies.processBodyFrameDefinitions( ); - + { // Define link ends for observations. From 181d025ae89d6c148b8ab6338ebd3b6fe4891a76 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Mon, 16 Dec 2024 16:15:37 +0100 Subject: [PATCH 08/24] Test progress --- .../oneWayDopplerObservationModel.h | 7 - .../relativity/relativisticTimeConversion.cpp | 1 - .../support/observationPartialTestFunctions.h | 6 +- .../unitTestDopplerModels.cpp | 45 +- .../unitTestOneWayDopplerPartials.cpp | 590 +++++++++--------- 5 files changed, 322 insertions(+), 327 deletions(-) diff --git a/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h b/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h index 5fbf2a2011..59858e8b1b 100644 --- a/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h +++ b/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h @@ -346,10 +346,6 @@ class DirectFirstOrderDopplerProperTimeRateInterface: Eigen::Vector6d computationPointRelativeState = getComputationPointState( linkEndTimes, linkEndStates ); setPerturbedProperties( linkEndTimes, linkEndStates ); - std::cout<<"COMPUTATION POINT STATE "<perturbingBodyMatchLinkEnds_.at( i ) == transmitter ) ? linkEndTimes.at( 1 ) : linkEndTimes.at( 0 ); } - std::cout<<"LINK ENDS "<perturbingBodyMatchLinkEnds_.at( i )<<" "<( spacecraftOrbitalElements, 0.0, earthGravitationalParameter, "Earth", "ECLIPJ2000" ), "Spacecraft" ) ); bodies.processBodyFrameDefinitions( ); - + // Define link ends for observations. LinkEnds linkEnds; @@ -242,7 +242,6 @@ BOOST_AUTO_TEST_CASE( testOneWayDoppplerModel ) double observationWithoutCorrections = observationModelWithoutCorrections->computeIdealObservations( observationTime, receiver ).x( ); - std::cout<<"************* COMPUTING WITH CORRECTIONS "<computeIdealObservations( observationTime, receiver ).x( ); @@ -260,9 +259,6 @@ BOOST_AUTO_TEST_CASE( testOneWayDoppplerModel ) Eigen::Vector6d spacecraftGeocentricState = bodies.at( "Spacecraft" )->getStateInBaseFrameFromEphemeris( observationTime ) - bodies.at( "Earth" )->getStateInBaseFrameFromEphemeris( observationTime ); - std::cout<<"States "<( physical_constants::INVERSE_SQUARE_SPEED_OF_LIGHT * ( 0.5 * std::pow( groundStationState.segment( 3, 3 ).norm( ), 2 ) + @@ -277,8 +273,6 @@ BOOST_AUTO_TEST_CASE( testOneWayDoppplerModel ) ( 1.0L + static_cast< long double >( observationWithoutCorrections ) / physical_constants::SPEED_OF_LIGHT ) / spacecraftProperTimeRate - 1.0L ) * physical_constants::SPEED_OF_LIGHT; - std::cout<<"PROPER TIME RATES "<( manualDopplerValue ) - observationWithCorrections ), 1.0E-6 ); } } @@ -305,7 +299,7 @@ BOOST_AUTO_TEST_CASE( testTwoWayDoppplerModel ) // Create bodies settings needed in simulation BodyListSettings defaultBodySettings = getDefaultBodySettings( - bodiesToCreate, initialEphemerisTime - buffer, finalEphemerisTime + buffer ); + bodiesToCreate, "SSB" ); // Create bodies SystemOfBodies bodies = createSystemOfBodies( defaultBodySettings ); @@ -584,33 +578,39 @@ BOOST_AUTO_TEST_CASE( testTwoWayDoppplerModel ) std::shared_ptr< RotationalEphemeris > earthRotationModel = bodies.at( "Earth" )->getRotationalEphemeris( ); - Eigen::Vector3d groundStationVelocityVectorAtTransmission = - earthRotationModel->getDerivativeOfRotationToTargetFrame( linkEndTimes.at( 0 ) ) * - ( earthRotationModel->getRotationToBaseFrame( linkEndTimes.at( 0 ) ) * stationCartesianPosition ); + Eigen::Vector6d groundStationEarthFixedStateTransmission = Eigen::Vector6d::Zero( ); + groundStationEarthFixedStateTransmission.segment( 0, 3 ) = stationCartesianPosition; + Eigen::Vector6d groundStationGeocentricStateTransmission = ephemerides::transformStateToInertialOrientation( + groundStationEarthFixedStateTransmission, linkEndTimes.at( 0 ), earthRotationModel ); + Eigen::Vector6d groundStationStateTransmission = groundStationGeocentricStateTransmission; + groundStationStateTransmission += bodies.at( "Earth" )->getStateInBaseFrameFromEphemeris( linkEndTimes.at( 0 ) ); - Eigen::Vector3d groundStationVelocityVectorAtReception = - earthRotationModel->getDerivativeOfRotationToTargetFrame( linkEndTimes.at( 2 ) ) * - ( earthRotationModel->getRotationToBaseFrame( linkEndTimes.at( 2 ) ) * receivingStationPosition ); + Eigen::Vector6d groundStationEarthFixedStateReception = Eigen::Vector6d::Zero( ); + groundStationEarthFixedStateReception.segment( 0, 3 ) = receivingStationPosition; + Eigen::Vector6d groundStationGeocentricStateReception = ephemerides::transformStateToInertialOrientation( + groundStationEarthFixedStateReception, linkEndTimes.at( 2 ), earthRotationModel ); + Eigen::Vector6d groundStationStateReception = groundStationGeocentricStateReception; + groundStationStateReception += bodies.at( "Earth" )->getStateInBaseFrameFromEphemeris( linkEndTimes.at( 2 ) ); long double groundStationProperTimeRateAtTransmission = 1.0L - static_cast< long double >( physical_constants::INVERSE_SQUARE_SPEED_OF_LIGHT * - ( 0.5 * std::pow( groundStationVelocityVectorAtTransmission.norm( ), 2 ) + - earthGravitationalParameter / stationCartesianPosition.norm( ) ) ); + ( 0.5 * std::pow( groundStationStateTransmission.segment( 3, 3 ).norm( ), 2 ) + + earthGravitationalParameter / groundStationGeocentricStateTransmission.segment( 0, 3 ).norm( ) ) ); long double groundStationProperTimeRateAtReception = 1.0L - static_cast< long double >( physical_constants::INVERSE_SQUARE_SPEED_OF_LIGHT * - ( 0.5 * std::pow( groundStationVelocityVectorAtReception.norm( ), 2 ) + - earthGravitationalParameter / receivingStationPosition.norm( ) ) ); + ( 0.5 * std::pow( groundStationStateReception.segment( 3, 3 ).norm( ), 2 ) + + earthGravitationalParameter / groundStationGeocentricStateReception.segment( 0, 3 ).norm( ) ) ); + if( test == 0 ) { - BOOST_CHECK_SMALL( std::fabs( observationWithCorrections - observationWithoutCorrections ), - static_cast< double >( std::numeric_limits< long double >::epsilon( ) * physical_constants::SPEED_OF_LIGHT ) ); + BOOST_CHECK_SMALL( std::fabs( observationWithCorrections - observationWithoutCorrections ), 1.0E-6 ); BOOST_CHECK_SMALL( std::fabs( static_cast< double >( groundStationProperTimeRateAtTransmission - groundStationProperTimeRateAtReception ) ), - static_cast< double >( std::numeric_limits< long double >::epsilon( ) ) ); + 10.0 * static_cast< double >( std::numeric_limits< double >::epsilon( ) ) ); } else @@ -621,8 +621,7 @@ BOOST_AUTO_TEST_CASE( testTwoWayDoppplerModel ) observationWithCorrections / physical_constants::SPEED_OF_LIGHT - ( observationWithoutCorrections / physical_constants::SPEED_OF_LIGHT + properTimeRatioDeviation + observationWithoutCorrections / physical_constants::SPEED_OF_LIGHT * properTimeRatioDeviation ); - BOOST_CHECK_SMALL( std::fabs( static_cast< double >( observableDifference ) ), - static_cast< double >( std::numeric_limits< long double >::epsilon( ) ) ); + BOOST_CHECK_SMALL( std::fabs( static_cast< double >( observableDifference ) ), 1.0E-6 ); } diff --git a/tests/src/astro/orbit_determination/observation_partials/unitTestOneWayDopplerPartials.cpp b/tests/src/astro/orbit_determination/observation_partials/unitTestOneWayDopplerPartials.cpp index ab3765da94..1d97c1f0ba 100644 --- a/tests/src/astro/orbit_determination/observation_partials/unitTestOneWayDopplerPartials.cpp +++ b/tests/src/astro/orbit_determination/observation_partials/unitTestOneWayDopplerPartials.cpp @@ -207,7 +207,7 @@ BOOST_AUTO_TEST_CASE( testOneWayDopplerPartials ) std::cout<<"A ****************************************** "< > oneWayDopplerModel; - std::vector< std::string > perturbingBodies; - perturbingBodies.push_back( "Earth" ); - if( estimationCase == 0 ) - { - oneWayDopplerModel = - observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( - std::make_shared< observation_models::OneWayDopplerObservationSettings >( - linkEnds, - std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( perturbingBodies ), - nullptr, nullptr, nullptr, - std::make_shared< LightTimeConvergenceCriteria >( ), - static_cast< bool >( normalizeObservable ) ), bodies ); - } - else - { - oneWayDopplerModel = - observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( - std::make_shared< OneWayDopplerObservationSettings > - ( linkEnds, std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( - perturbingBodies ), - std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Mars" ), - std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), - nullptr, - std::make_shared< LightTimeConvergenceCriteria >( ), - static_cast< bool >( normalizeObservable ) ), bodies ); - } - // Create parameter objects. - std::shared_ptr< EstimatableParameterSet< double > > fullEstimatableParameterSet; - Eigen::VectorXd parameterPerturbationMultipliers = Eigen::Vector4d::Constant( 1.0 ); - if( estimationCase < 2 ) - { - fullEstimatableParameterSet = createEstimatableParameters( bodies, 1.1E7 ); - } - else - { - fullEstimatableParameterSet = createEstimatableParameters( bodies, 1.1E7, true ); - parameterPerturbationMultipliers( 2 ) = 1.0E-4; - } - - testObservationPartials< 1 >( - oneWayDopplerModel, bodies, fullEstimatableParameterSet, linkEnds, one_way_doppler, 1.0E-4, false, true, - 1.0, parameterPerturbationMultipliers ); - } - } - - std::cout<<"C ****************************************** "< > oneWayDopplerModel = - std::dynamic_pointer_cast< OneWayDopplerObservationModel< > >( - observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( - std::make_shared< OneWayDopplerObservationSettings > - ( linkEnds, std::shared_ptr< LightTimeCorrectionSettings >( ), - std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), - std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Mars" ), - nullptr, - std::make_shared< LightTimeConvergenceCriteria >( ), - static_cast< bool >( normalizeObservable ) ), bodies ) ); - - - // Extract proper time calculators - std::shared_ptr< DopplerProperTimeRateInterface > receiverProperTimeRateCalculator = - oneWayDopplerModel->getReceiverProperTimeRateCalculator( ); - std::shared_ptr< DopplerProperTimeRateInterface > transmitterProperTimeRateCalculator = - oneWayDopplerModel->getTransmitterProperTimeRateCalculator( ); - - // Create parameter objects. - std::shared_ptr< EstimatableParameterSet< double > > fullEstimatableParameterSet = - createEstimatableParameters( bodies, 1.1E7 ); - - // Create partials for Doppler with proper time rates - std::pair< SingleLinkObservationPartialList, std::shared_ptr< PositionPartialScaling > > dopplerPartials = - ObservationPartialCreator<1, double, double>::createObservationPartials( - oneWayDopplerModel, bodies, fullEstimatableParameterSet ); - - // Retrieve scaling objects and partials with proper time - std::shared_ptr< OneWayDopplerScaling > partialScalingObject = - std::dynamic_pointer_cast< OneWayDopplerScaling >( dopplerPartials.second ); - - std::shared_ptr< OneWayDopplerProperTimeComponentScaling > transmitterProperTimePartials = - partialScalingObject->getTransmitterProperTimePartials( ); - std::shared_ptr< OneWayDopplerProperTimeComponentScaling > receiverProperTimePartials = - partialScalingObject->getReceiverProperTimePartials( ); - - std::shared_ptr< DirectObservationPartial< 1 > > earthStatePartial = - std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( - ( dopplerPartials.first ).begin( )->second ); - std::shared_ptr< DirectObservationPartial< 1 > > marsStatePartial = - std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( - ( ++( ( dopplerPartials.first ).begin( ) ) )->second ); - - // Compute nominal observation with proper time - double observationTime = 1.1E7; - std::vector< double > linkEndTimes; - std::vector< Eigen::Vector6d > linkEndStates; - LinkEndType referenceLinkEnd = transmitter; - Eigen::VectorXd nominalObservable = oneWayDopplerModel->computeIdealObservationsWithLinkEndData( - observationTime, referenceLinkEnd, linkEndTimes, linkEndStates ); - - std::cout<<"Nominal: "<update( - linkEndStates, linkEndTimes, referenceLinkEnd, nominalObservable ); - - std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > earthStatePartialOutput = - earthStatePartial->calculatePartial( linkEndStates, linkEndTimes, referenceLinkEnd, {}, nominalObservable ); - - std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > marsStatePartialOutput = - marsStatePartial->calculatePartial( linkEndStates, linkEndTimes, referenceLinkEnd, {}, nominalObservable ); - - // Compute numerical proper time rate partials and compare to analytical results - { - - std::function< Eigen::VectorXd( const double ) > transmitterProperTimeRateFunction = - std::bind( &getProperTimeRateInVectorForm, - transmitterProperTimeRateCalculator, - linkEndTimes, linkEndStates, referenceLinkEnd ); - Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalTransmitterProperTimePartialsWrtMarsPosition = - calculatePartialWrtConstantBodyState( - "Earth", bodies, Eigen::Vector3d::Constant( 1000.0E3 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); - Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalTransmitterProperTimePartialsWrtEarthPosition = - calculatePartialWrtConstantBodyState( - "Mars", bodies, Eigen::Vector3d::Constant( 1000.0E3 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); - Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalTransmitterProperTimePartialsWrtMarsVelocity = - calculatePartialWrtConstantBodyVelocity( - "Earth", bodies, Eigen::Vector3d::Constant( 1.0E0 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); - Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalTransmitterProperTimePartialsWrtEarthVelocity = - calculatePartialWrtConstantBodyVelocity( - "Mars", bodies, Eigen::Vector3d::Constant( 1.0E0 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); - - std::cout<<"Trans. scaling factor: "<getPositionScalingFactor( transmitter )<getPositionScalingFactor( receiver )<getPositionScalingFactor( transmitter ) ), - ( numericalTransmitterProperTimePartialsWrtMarsPosition ), 1.0E-6 ); - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( - ( transmitterProperTimePartials->getPositionScalingFactor( receiver ) ), - ( numericalTransmitterProperTimePartialsWrtEarthPosition ), 1.0E-6 ); - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( - ( transmitterProperTimePartials->getVelocityScalingFactor( transmitter ) ), - ( numericalTransmitterProperTimePartialsWrtMarsVelocity ), 1.0E-6 ); - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( - ( transmitterProperTimePartials->getVelocityScalingFactor( receiver ) ), - ( numericalTransmitterProperTimePartialsWrtEarthVelocity ), 1.0E-6 ); - - std::function< Eigen::VectorXd( const double ) > receiverProperTimeRateFunction = - std::bind( &getProperTimeRateInVectorForm, - receiverProperTimeRateCalculator, - linkEndTimes, linkEndStates, referenceLinkEnd ); - Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtMarsPosition = - calculatePartialWrtConstantBodyState( - "Earth", bodies, Eigen::Vector3d::Constant( 10000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); - Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtEarthPosition = - calculatePartialWrtConstantBodyState( - "Mars", bodies, Eigen::Vector3d::Constant( 10000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); - Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtMarsVelocity = - calculatePartialWrtConstantBodyVelocity( - "Earth", bodies, Eigen::Vector3d::Constant( 1000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); - Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtEarthVelocity = - calculatePartialWrtConstantBodyVelocity( - "Mars", bodies, Eigen::Vector3d::Constant( 1000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); - - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( - ( receiverProperTimePartials->getPositionScalingFactor( receiver ) ), - ( numericalReceiverProperTimePartialsWrtEarthPosition ), 1.0E-6 ); - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( - ( receiverProperTimePartials->getPositionScalingFactor( transmitter ) ), - ( numericalReceiverProperTimePartialsWrtMarsPosition ), 1.0E-6 ); - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( - ( receiverProperTimePartials->getVelocityScalingFactor( transmitter ) ), - ( numericalReceiverProperTimePartialsWrtMarsVelocity ), 1.0E-6 ); - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( - ( receiverProperTimePartials->getVelocityScalingFactor( receiver ) ), - ( numericalReceiverProperTimePartialsWrtEarthVelocity ), 1.0E-6 ); - } - - std::cout<<"D ****************************************** "< > oneWayDopplerModelWithoutProperTime = - std::dynamic_pointer_cast< OneWayDopplerObservationModel< > >( - observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( - std::make_shared< observation_models::OneWayDopplerObservationSettings >( - linkEnds, nullptr, nullptr, nullptr, nullptr, - std::make_shared< LightTimeConvergenceCriteria >( ), - static_cast< bool >( normalizeObservable ) ), bodies ) ); - - // Create partials for Doppler without proper time rates - std::pair< SingleLinkObservationPartialList, std::shared_ptr< PositionPartialScaling > > dopplerPartialsWithoutProperTime = - ObservationPartialCreator<1, double, double>::createObservationPartials( - oneWayDopplerModelWithoutProperTime, bodies, fullEstimatableParameterSet ); - - // Retrieve partial object without proper time - std::shared_ptr< OneWayDopplerScaling > partialScalingObjectWithoutProperTime = - std::dynamic_pointer_cast< OneWayDopplerScaling >( dopplerPartialsWithoutProperTime.second ); - std::shared_ptr< DirectObservationPartial< 1 > > earthStatePartialWithoutProperTime = - std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( - ( dopplerPartialsWithoutProperTime.first ).begin( )->second ); - std::shared_ptr< DirectObservationPartial< 1 > > marsStatePartialWithoutProperTime = - std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( - ( ++( ( dopplerPartialsWithoutProperTime.first ).begin( ) ) )->second ); - - // Compute nominal observation without proper time - std::vector< double > linkEndTimesWithoutProperTime; - std::vector< Eigen::Vector6d > linkEndStatesWithoutProperTime; - Eigen::VectorXd nominalObservableWithoutProperTime = oneWayDopplerModelWithoutProperTime->computeIdealObservationsWithLinkEndData( - observationTime, referenceLinkEnd, linkEndTimesWithoutProperTime, linkEndStatesWithoutProperTime ); - - std::cout<<"Nominal without proper time: "<update( - linkEndStatesWithoutProperTime, linkEndTimesWithoutProperTime, - referenceLinkEnd, nominalObservableWithoutProperTime ); - std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > earthStatePartialOutputWithoutProperTime = - earthStatePartialWithoutProperTime->calculatePartial( - linkEndStates, linkEndTimes, referenceLinkEnd, {}, nominalObservable ); - std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > marsStatePartialOutputWithoutProperTime = - marsStatePartialWithoutProperTime->calculatePartial( - linkEndStates, linkEndTimes, referenceLinkEnd, {}, nominalObservable ); - - Eigen::MatrixXd partialWrtEarthState = earthStatePartialOutput.at( 0 ).first; - Eigen::MatrixXd partialWrtEarthStateWithoutProperTime = earthStatePartialOutputWithoutProperTime.at( 0 ).first; - - Eigen::MatrixXd partialWrtMarsState = marsStatePartialOutput.at( 0 ).first; - Eigen::MatrixXd partialWrtMarsStateWithoutProperTime = marsStatePartialOutputWithoutProperTime.at( 0 ).first; - - Eigen::MatrixXd properTimePartialWrtMarsPosition = transmitterProperTimePartials->getPositionScalingFactor( transmitter ) * partialScalingTerm; - Eigen::MatrixXd properTimePartialWrtEarthPosition = receiverProperTimePartials->getPositionScalingFactor( receiver ) * partialScalingTerm; - - Eigen::MatrixXd properTimePartialWrtMarsVelocity = transmitterProperTimePartials->getVelocityScalingFactor( transmitter ) * partialScalingTerm; - Eigen::MatrixXd properTimePartialWrtEarthVelocity = receiverProperTimePartials->getVelocityScalingFactor( receiver ) * partialScalingTerm; - - std::cout<<"Partial w.r.t. Earth state "< > oneWayDopplerModel; +// std::vector< std::string > perturbingBodies; +// perturbingBodies.push_back( "Earth" ); +// if( estimationCase == 0 ) +// { +// oneWayDopplerModel = +// observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( +// std::make_shared< observation_models::OneWayDopplerObservationSettings >( +// linkEnds, +// std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( perturbingBodies ), +// nullptr, nullptr, nullptr, +// std::make_shared< LightTimeConvergenceCriteria >( ), +// static_cast< bool >( normalizeObservable ) ), bodies ); +// } +// else +// { +// oneWayDopplerModel = +// observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( +// std::make_shared< OneWayDopplerObservationSettings > +// ( linkEnds, std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( +// perturbingBodies ), +// std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Mars" ), +// std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), +// nullptr, +// std::make_shared< LightTimeConvergenceCriteria >( ), +// static_cast< bool >( normalizeObservable ) ), bodies ); +// } +// // Create parameter objects. +// std::shared_ptr< EstimatableParameterSet< double > > fullEstimatableParameterSet; +// Eigen::VectorXd parameterPerturbationMultipliers = Eigen::Vector4d::Constant( 1.0 ); +// if( estimationCase < 2 ) +// { +// fullEstimatableParameterSet = createEstimatableParameters( bodies, 1.1E7 ); +// } +// else +// { +// fullEstimatableParameterSet = createEstimatableParameters( bodies, 1.1E7, true ); +// parameterPerturbationMultipliers( 2 ) = 1.0E-4; +// } +// +// testObservationPartials< 1 >( +// oneWayDopplerModel, bodies, fullEstimatableParameterSet, linkEnds, one_way_doppler, 1.0E-4, false, true, +// 1.0, parameterPerturbationMultipliers ); +// } +// } +// +// std::cout<<"C ****************************************** "< > oneWayDopplerModel = +// std::dynamic_pointer_cast< OneWayDopplerObservationModel< > >( +// observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( +// std::make_shared< OneWayDopplerObservationSettings > +// ( linkEnds, std::shared_ptr< LightTimeCorrectionSettings >( ), +// std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), +// std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Mars" ), +// nullptr, +// std::make_shared< LightTimeConvergenceCriteria >( ), +// static_cast< bool >( normalizeObservable ) ), bodies ) ); +// +// +// // Extract proper time calculators +// std::shared_ptr< DopplerProperTimeRateInterface > receiverProperTimeRateCalculator = +// oneWayDopplerModel->getReceiverProperTimeRateCalculator( ); +// std::shared_ptr< DopplerProperTimeRateInterface > transmitterProperTimeRateCalculator = +// oneWayDopplerModel->getTransmitterProperTimeRateCalculator( ); +// +// // Create parameter objects. +// std::shared_ptr< EstimatableParameterSet< double > > fullEstimatableParameterSet = +// createEstimatableParameters( bodies, 1.1E7 ); +// +// // Create partials for Doppler with proper time rates +// std::pair< SingleLinkObservationPartialList, std::shared_ptr< PositionPartialScaling > > dopplerPartials = +// ObservationPartialCreator<1, double, double>::createObservationPartials( +// oneWayDopplerModel, bodies, fullEstimatableParameterSet ); +// +// // Retrieve scaling objects and partials with proper time +// std::shared_ptr< OneWayDopplerScaling > partialScalingObject = +// std::dynamic_pointer_cast< OneWayDopplerScaling >( dopplerPartials.second ); +// +// std::shared_ptr< OneWayDopplerProperTimeComponentScaling > transmitterProperTimePartials = +// partialScalingObject->getTransmitterProperTimePartials( ); +// std::shared_ptr< OneWayDopplerProperTimeComponentScaling > receiverProperTimePartials = +// partialScalingObject->getReceiverProperTimePartials( ); +// +// std::shared_ptr< DirectObservationPartial< 1 > > earthStatePartial = +// std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( +// ( dopplerPartials.first ).begin( )->second ); +// std::shared_ptr< DirectObservationPartial< 1 > > marsStatePartial = +// std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( +// ( ++( ( dopplerPartials.first ).begin( ) ) )->second ); +// +// // Compute nominal observation with proper time +// double observationTime = 1.1E7; +// std::vector< double > linkEndTimes; +// std::vector< Eigen::Vector6d > linkEndStates; +// LinkEndType referenceLinkEnd = transmitter; +// Eigen::VectorXd nominalObservable = oneWayDopplerModel->computeIdealObservationsWithLinkEndData( +// observationTime, referenceLinkEnd, linkEndTimes, linkEndStates ); +// +// std::cout<<"Nominal: "<update( +// linkEndStates, linkEndTimes, referenceLinkEnd, nominalObservable ); +// +// std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > earthStatePartialOutput = +// earthStatePartial->calculatePartial( linkEndStates, linkEndTimes, referenceLinkEnd, {}, nominalObservable ); +// +// std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > marsStatePartialOutput = +// marsStatePartial->calculatePartial( linkEndStates, linkEndTimes, referenceLinkEnd, {}, nominalObservable ); +// +// // Compute numerical proper time rate partials and compare to analytical results +// { +// +// std::function< Eigen::VectorXd( const double ) > transmitterProperTimeRateFunction = +// std::bind( &getProperTimeRateInVectorForm, +// transmitterProperTimeRateCalculator, +// linkEndTimes, linkEndStates, referenceLinkEnd ); +// Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalTransmitterProperTimePartialsWrtMarsPosition = +// calculatePartialWrtConstantBodyState( +// "Earth", bodies, Eigen::Vector3d::Constant( 1000.0E3 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); +// Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalTransmitterProperTimePartialsWrtEarthPosition = +// calculatePartialWrtConstantBodyState( +// "Mars", bodies, Eigen::Vector3d::Constant( 1000.0E3 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); +// Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalTransmitterProperTimePartialsWrtMarsVelocity = +// calculatePartialWrtConstantBodyVelocity( +// "Earth", bodies, Eigen::Vector3d::Constant( 1.0E0 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); +// Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalTransmitterProperTimePartialsWrtEarthVelocity = +// calculatePartialWrtConstantBodyVelocity( +// "Mars", bodies, Eigen::Vector3d::Constant( 1.0E0 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); +// +// std::cout<<"Trans. scaling factor: "<getPositionScalingFactor( transmitter )<getPositionScalingFactor( receiver )<getPositionScalingFactor( transmitter ) ), +// ( numericalTransmitterProperTimePartialsWrtMarsPosition ), 1.0E-6 ); +// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( +// ( transmitterProperTimePartials->getPositionScalingFactor( receiver ) ), +// ( numericalTransmitterProperTimePartialsWrtEarthPosition ), 1.0E-6 ); +// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( +// ( transmitterProperTimePartials->getVelocityScalingFactor( transmitter ) ), +// ( numericalTransmitterProperTimePartialsWrtMarsVelocity ), 1.0E-6 ); +// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( +// ( transmitterProperTimePartials->getVelocityScalingFactor( receiver ) ), +// ( numericalTransmitterProperTimePartialsWrtEarthVelocity ), 1.0E-6 ); +// +// std::function< Eigen::VectorXd( const double ) > receiverProperTimeRateFunction = +// std::bind( &getProperTimeRateInVectorForm, +// receiverProperTimeRateCalculator, +// linkEndTimes, linkEndStates, referenceLinkEnd ); +// Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtMarsPosition = +// calculatePartialWrtConstantBodyState( +// "Earth", bodies, Eigen::Vector3d::Constant( 10000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); +// Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtEarthPosition = +// calculatePartialWrtConstantBodyState( +// "Mars", bodies, Eigen::Vector3d::Constant( 10000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); +// Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtMarsVelocity = +// calculatePartialWrtConstantBodyVelocity( +// "Earth", bodies, Eigen::Vector3d::Constant( 1000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); +// Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtEarthVelocity = +// calculatePartialWrtConstantBodyVelocity( +// "Mars", bodies, Eigen::Vector3d::Constant( 1000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); +// +// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( +// ( receiverProperTimePartials->getPositionScalingFactor( receiver ) ), +// ( numericalReceiverProperTimePartialsWrtEarthPosition ), 1.0E-6 ); +// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( +// ( receiverProperTimePartials->getPositionScalingFactor( transmitter ) ), +// ( numericalReceiverProperTimePartialsWrtMarsPosition ), 1.0E-6 ); +// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( +// ( receiverProperTimePartials->getVelocityScalingFactor( transmitter ) ), +// ( numericalReceiverProperTimePartialsWrtMarsVelocity ), 1.0E-6 ); +// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( +// ( receiverProperTimePartials->getVelocityScalingFactor( receiver ) ), +// ( numericalReceiverProperTimePartialsWrtEarthVelocity ), 1.0E-6 ); +// } +// +// std::cout<<"D ****************************************** "< > oneWayDopplerModelWithoutProperTime = +// std::dynamic_pointer_cast< OneWayDopplerObservationModel< > >( +// observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( +// std::make_shared< observation_models::OneWayDopplerObservationSettings >( +// linkEnds, nullptr, nullptr, nullptr, nullptr, +// std::make_shared< LightTimeConvergenceCriteria >( ), +// static_cast< bool >( normalizeObservable ) ), bodies ) ); +// +// // Create partials for Doppler without proper time rates +// std::pair< SingleLinkObservationPartialList, std::shared_ptr< PositionPartialScaling > > dopplerPartialsWithoutProperTime = +// ObservationPartialCreator<1, double, double>::createObservationPartials( +// oneWayDopplerModelWithoutProperTime, bodies, fullEstimatableParameterSet ); +// +// // Retrieve partial object without proper time +// std::shared_ptr< OneWayDopplerScaling > partialScalingObjectWithoutProperTime = +// std::dynamic_pointer_cast< OneWayDopplerScaling >( dopplerPartialsWithoutProperTime.second ); +// std::shared_ptr< DirectObservationPartial< 1 > > earthStatePartialWithoutProperTime = +// std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( +// ( dopplerPartialsWithoutProperTime.first ).begin( )->second ); +// std::shared_ptr< DirectObservationPartial< 1 > > marsStatePartialWithoutProperTime = +// std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( +// ( ++( ( dopplerPartialsWithoutProperTime.first ).begin( ) ) )->second ); +// +// // Compute nominal observation without proper time +// std::vector< double > linkEndTimesWithoutProperTime; +// std::vector< Eigen::Vector6d > linkEndStatesWithoutProperTime; +// Eigen::VectorXd nominalObservableWithoutProperTime = oneWayDopplerModelWithoutProperTime->computeIdealObservationsWithLinkEndData( +// observationTime, referenceLinkEnd, linkEndTimesWithoutProperTime, linkEndStatesWithoutProperTime ); +// +// std::cout<<"Nominal without proper time: "<update( +// linkEndStatesWithoutProperTime, linkEndTimesWithoutProperTime, +// referenceLinkEnd, nominalObservableWithoutProperTime ); +// std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > earthStatePartialOutputWithoutProperTime = +// earthStatePartialWithoutProperTime->calculatePartial( +// linkEndStates, linkEndTimes, referenceLinkEnd, {}, nominalObservable ); +// std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > marsStatePartialOutputWithoutProperTime = +// marsStatePartialWithoutProperTime->calculatePartial( +// linkEndStates, linkEndTimes, referenceLinkEnd, {}, nominalObservable ); +// +// Eigen::MatrixXd partialWrtEarthState = earthStatePartialOutput.at( 0 ).first; +// Eigen::MatrixXd partialWrtEarthStateWithoutProperTime = earthStatePartialOutputWithoutProperTime.at( 0 ).first; +// +// Eigen::MatrixXd partialWrtMarsState = marsStatePartialOutput.at( 0 ).first; +// Eigen::MatrixXd partialWrtMarsStateWithoutProperTime = marsStatePartialOutputWithoutProperTime.at( 0 ).first; +// +// Eigen::MatrixXd properTimePartialWrtMarsPosition = transmitterProperTimePartials->getPositionScalingFactor( transmitter ) * partialScalingTerm; +// Eigen::MatrixXd properTimePartialWrtEarthPosition = receiverProperTimePartials->getPositionScalingFactor( receiver ) * partialScalingTerm; +// +// Eigen::MatrixXd properTimePartialWrtMarsVelocity = transmitterProperTimePartials->getVelocityScalingFactor( transmitter ) * partialScalingTerm; +// Eigen::MatrixXd properTimePartialWrtEarthVelocity = receiverProperTimePartials->getVelocityScalingFactor( receiver ) * partialScalingTerm; +// +// std::cout<<"Partial w.r.t. Earth state "< Date: Tue, 17 Dec 2024 09:38:52 +0100 Subject: [PATCH 09/24] Progress in proper time partials --- .../observation_models/lightTimeSolution.h | 14 +- .../oneWayDopplerObservationModel.h | 27 +- .../oneWayDopplerPartial.cpp | 38 +- .../relativity/relativisticTimeConversion.cpp | 6 + .../unitTestOneWayDopplerPartials.cpp | 578 +++++++++--------- .../observationPartialTestFunctions.cpp | 3 + 6 files changed, 348 insertions(+), 318 deletions(-) diff --git a/include/tudat/astro/observation_models/lightTimeSolution.h b/include/tudat/astro/observation_models/lightTimeSolution.h index 39eae8737e..fd8452cf74 100644 --- a/include/tudat/astro/observation_models/lightTimeSolution.h +++ b/include/tudat/astro/observation_models/lightTimeSolution.h @@ -650,13 +650,13 @@ class LightTimeCalculator Eigen::Matrix< ObservationScalarType, 1, 3 > partialWrtLinkEndPosition = ( relativePosition.normalized( ) ).transpose( ) * ( isPartialWrtReceiver ? mathematical_constants::getFloatingInteger< ObservationScalarType >( 1 ) : mathematical_constants::getFloatingInteger< ObservationScalarType >( -1 ) ); - for( unsigned int i = 0; i < correctionFunctions_.size( ); i++ ) - { - partialWrtLinkEndPosition += correctionFunctions_.at( i )->calculateLightTimeCorrectionPartialDerivativeWrtLinkEndPosition( - transmitterState.template cast< double >( ), - receiverState.template cast< double >( ), - transmitterTime, receiverTime, isPartialWrtReceiver ? receiver : transmitter ).template cast< ObservationScalarType >( ); - } +// for( unsigned int i = 0; i < correctionFunctions_.size( ); i++ ) +// { +// partialWrtLinkEndPosition += correctionFunctions_.at( i )->calculateLightTimeCorrectionPartialDerivativeWrtLinkEndPosition( +// transmitterState.template cast< double >( ), +// receiverState.template cast< double >( ), +// transmitterTime, receiverTime, isPartialWrtReceiver ? receiver : transmitter ).template cast< ObservationScalarType >( ); +// } return partialWrtLinkEndPosition; } diff --git a/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h b/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h index 59858e8b1b..0492f1f546 100644 --- a/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h +++ b/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h @@ -397,22 +397,12 @@ class DirectFirstOrderDopplerProperTimeRateInterface: currentCentralBodyGravitationalParameters_[ i ] = gravitationalParameterFunctions_.at( i )( ); } } -// -// //! Function to retrieve central body gravitational parameter -// /*! -// * Function to retrieve central body gravitational parameter -// * \return Central body gravitational parameter -// */ -// double getGravitationalParameter( ) -// { -// return gravitationalParameterFunction_( ); -// } -// -// //! Function to return the name of body generating the gravity field. -// /*! -// * Function to return the name of body generating the gravity field -// * \return Name of body generating the gravity field -// */ + + std::vector< std::function< double( ) > > getGravitationalParameters( ) + { + return gravitationalParameterFunctions_; + } + bool matchWithBody( const std::string bodyName ) { for( unsigned int i = 0; i < perturbingBodyNames_.size( ); i++ ) @@ -425,6 +415,11 @@ class DirectFirstOrderDopplerProperTimeRateInterface: return false; } + std::vector< Eigen::Vector6d > getCurrentPerturbedStates( ) + { + return currentPerturbedStates_; + } + private: std::vector< std::function< double( ) > > gravitationalParameterFunctions_; diff --git a/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp b/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp index 38b3b5b411..b1054619f1 100644 --- a/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp +++ b/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp @@ -51,25 +51,25 @@ void OneWayDopplerDirectFirstOrderProperTimeComponentScaling::update( const std: const Eigen::VectorXd currentObservation ) { // Get relative state -// Eigen::Vector6d relativeState = properTimeRateModel_->getComputationPointState( -// times, linkEndStates ); -// currentDistance_ = relativeState.segment( 0, 3 ).norm( ); -// currentGravitationalParameter_ = properTimeRateModel_->getGravitationalParameter( ); -// -// currentLinkEndTime_ == ( linkEndWithPartial_ == observation_models::transmitter ) ? ( times.at( 0 ) ) : ( times.at( 1 ) ); -// -// // Compute partials w.r.t. position and velocity -// if( computeStatePartials_ ) -// { -// partialWrPosition_ = -physical_constants::INVERSE_SQUARE_SPEED_OF_LIGHT * -// ( 1.0 + relativity::equivalencePrincipleLpiViolationParameter ) * -// currentGravitationalParameter_ / ( currentDistance_ * currentDistance_ ) * -// ( relativeState.segment( 0, 3 ).normalized( ) ).transpose( ); -// partialWrtVelocity_ = physical_constants::INVERSE_SQUARE_SPEED_OF_LIGHT * -// ( relativeState.segment( 3, 3 ) ).transpose( ); -// } - partialWrPosition_.setZero( ); - partialWrtVelocity_.setZero( ); + Eigen::Vector6d relativeState = properTimeRateModel_->getComputationPointState( + times, linkEndStates ); + currentGravitationalParameter_ = properTimeRateModel_->getGravitationalParameters( ).at( 0 )( ); + + currentLinkEndTime_ == ( linkEndWithPartial_ == observation_models::transmitter ) ? ( times.at( 0 ) ) : ( times.at( 1 ) ); + + Eigen::Vector6d perturberRelativeState = properTimeRateModel_->getCurrentPerturbedStates( ).at( 0 ); + currentDistance_ = ( relativeState - perturberRelativeState ).segment( 0, 3 ).norm( ); + // Compute partials w.r.t. position and velocity + if( computeStatePartials_ ) + { + + partialWrPosition_ = physical_constants::INVERSE_SQUARE_SPEED_OF_LIGHT * + ( 1.0 + relativity::equivalencePrincipleLpiViolationParameter ) * + currentGravitationalParameter_ / ( currentDistance_ * currentDistance_ ) * + ( ( relativeState - perturberRelativeState ).segment( 0, 3 ).normalized( ) ).transpose( ); + partialWrtVelocity_ = -physical_constants::INVERSE_SQUARE_SPEED_OF_LIGHT * + ( relativeState.segment( 3, 3 ) ).transpose( ); + } } //! Function to retrieve the scaling factor for the derivative w.r.t. the position of a given link end diff --git a/src/astro/relativity/relativisticTimeConversion.cpp b/src/astro/relativity/relativisticTimeConversion.cpp index 0788e81432..81ad81e65e 100644 --- a/src/astro/relativity/relativisticTimeConversion.cpp +++ b/src/astro/relativity/relativisticTimeConversion.cpp @@ -10,6 +10,7 @@ #include #include "tudat/astro/basic_astro/physicalConstants.h" #include "tudat/astro/relativity/relativisticTimeConversion.h" +#include namespace tudat { @@ -41,6 +42,11 @@ double calculateFirstCentralBodyProperTimeRateDifference( centralBodyGravitationalParameters.at( i ) / ( perturbedInertialStates.at( i ).segment( 0, 3 ) - computationPointState.segment( 0, 3 ) ).norm( ); } + std::cout<<"COMPUTING "< properTimeRateCalculator, - const std::vector< double >& linkEndTimes, - const std::vector< Eigen::Matrix< double, 6, 1 > >& linkEndStates, - const LinkEndType linkEndAssociatedWithTime ) + std::shared_ptr< OneWayDopplerObservationModel< > > oneWayDopplerModel, + const LinkEndType referenceLinkEnd, + const double observationTime ) { + std::vector< double > linkEndTimes; + std::vector< Eigen::Matrix< double, 6, 1 > > linkEndStates; + Eigen::VectorXd nominalObservable = oneWayDopplerModel->computeIdealObservationsWithLinkEndData( + observationTime, referenceLinkEnd, linkEndTimes, linkEndStates ); return ( Eigen::Vector1d( ) << properTimeRateCalculator->getOberverProperTimeDeviation( linkEndTimes, linkEndStates ) ).finished( ); } @@ -89,121 +93,121 @@ BOOST_AUTO_TEST_CASE( testOneWayDopplerPartials ) groundStations[ 0 ] = std::make_pair( "Earth", "Graz" ); groundStations[ 1 ] = std::make_pair( "Mars", "MSL" ); - - // Test ancilliary functions - { - double nominalEvaluationTime = 1.1E7; - - // Create environment - SystemOfBodies bodies = setupEnvironment( groundStations, 1.0E7, 1.2E7, 1.1E7, false ); - - // Set link ends for observation model - LinkDefinition linkEnds; - linkEnds[ transmitter ] = groundStations[ 1 ]; - linkEnds[ receiver ] = groundStations[ 0 ]; - - // Create transmitter/receriver state functions - std::function< Eigen::Vector6d( const double ) > transmitterStateFunction = - getLinkEndCompleteEphemerisFunction< double, double >( linkEnds[ transmitter ], bodies ); - std::function< Eigen::Vector6d( const double ) > receiverStateFunction = - getLinkEndCompleteEphemerisFunction< double, double >( linkEnds[ receiver ], bodies ); - - // Define (independent!) transmission/reception times - double transmissionTime = nominalEvaluationTime; - double receptionTime = nominalEvaluationTime + 1.0E3; - - // Compute associated states - Eigen::Vector6d nominalTransmitterState = transmitterStateFunction( transmissionTime ); - Eigen::Vector6d nominalReceiverState = receiverStateFunction( receptionTime ); - Eigen::Vector3d nominalVectorToReceiver = ( nominalReceiverState - nominalTransmitterState ).segment( 0, 3 ); - - double timePerturbation = 100.0; - - // Partials for fixed receiver - { - // Compute numerical derivative of transmitter state for acceleration) - Eigen::Vector6d numericalStateDerivative = numerical_derivatives::computeCentralDifferenceFromFunction( - transmitterStateFunction, transmissionTime, timePerturbation, numerical_derivatives::order8 ); - - // Compute unit vector derivative numerically - std::function< Eigen::Vector3d( const double ) > unitVectorFunction = - std::bind( &computeUnitVectorToReceiverFromTransmitterState, - nominalReceiverState.segment( 0, 3 ), transmitterStateFunction, std::placeholders::_1 ); - Eigen::Vector3d numericalUnitVectorDerivative = numerical_derivatives::computeCentralDifferenceFromFunction( - unitVectorFunction, transmissionTime, timePerturbation, numerical_derivatives::order8 ); - - // Compute projected velocoty vector derivative numerically - std::function< double( const double) > projectedVelocityFunction = - std::bind( &calculateLineOfSightVelocityAsCFractionFromTransmitterStateFunction< double, double >, - nominalReceiverState.segment( 0, 3 ), transmitterStateFunction, std::placeholders::_1 ); - double numericalProjectedVelocityDerivative = - numerical_derivatives::computeCentralDifferenceFromFunction( - projectedVelocityFunction, transmissionTime, timePerturbation, numerical_derivatives::order8 ); - - // Compute analytical partial derivatives - Eigen::Vector3d analyticalUnitVectorDerivative = - -computePartialOfUnitVectorWrtLinkEndTime( - nominalVectorToReceiver, nominalVectorToReceiver.normalized( ), - nominalVectorToReceiver.norm( ), nominalTransmitterState.segment( 3, 3 ) ); - double analyticalProjectedVelocityDerivative = computePartialOfProjectedLinkEndVelocityWrtAssociatedTime( - nominalVectorToReceiver, - nominalTransmitterState.segment( 3, 3 ), - nominalTransmitterState.segment( 3, 3 ), - numericalStateDerivative.segment( 3, 3 ), false ); - - - for( unsigned int i = 0; i < 3; i++ ) - { - BOOST_CHECK_SMALL( std::fabs( analyticalUnitVectorDerivative( i ) - numericalUnitVectorDerivative( i ) ), 1.0E-16 ); - - } - BOOST_CHECK_SMALL( std::fabs( analyticalProjectedVelocityDerivative / physical_constants::SPEED_OF_LIGHT - - numericalProjectedVelocityDerivative ), 1.0E-21 ); - } - - - // Partials for fixed transmitter - { - // Compute numerical derivative of receiver state for acceleration) - Eigen::Vector6d numericalStateDerivative = numerical_derivatives::computeCentralDifferenceFromFunction( - receiverStateFunction, receptionTime, timePerturbation, numerical_derivatives::order8 ); - - // Compute unit vector derivative numerically - std::function< Eigen::Vector3d( const double ) > unitVectorFunction = - std::bind( &computeUnitVectorToReceiverFromReceiverState, - receiverStateFunction, nominalTransmitterState.segment( 0, 3 ), std::placeholders::_1 ); - Eigen::Vector3d numericalUnitVectorDerivative = numerical_derivatives::computeCentralDifferenceFromFunction( - unitVectorFunction, receptionTime, timePerturbation, numerical_derivatives::order8 ); - - // Compute projected velocoty vector derivative numerically - std::function< double( const double) > projectedVelocityFunction = - std::bind( &calculateLineOfSightVelocityAsCFractionFromReceiverStateFunction< double, double >, - receiverStateFunction, nominalTransmitterState.segment( 0, 3 ), std::placeholders::_1 ); - double numericalProjectedVelocityDerivative = - numerical_derivatives::computeCentralDifferenceFromFunction( - projectedVelocityFunction, receptionTime, timePerturbation, numerical_derivatives::order8 ); - - // Compute analytical partial derivatives - Eigen::Vector3d analyticalUnitVectorDerivative = - computePartialOfUnitVectorWrtLinkEndTime( - nominalVectorToReceiver, nominalVectorToReceiver.normalized( ), - nominalVectorToReceiver.norm( ), nominalReceiverState.segment( 3, 3 ) ); - double analyticalProjectedVelocityDerivative = computePartialOfProjectedLinkEndVelocityWrtAssociatedTime( - nominalVectorToReceiver, - nominalReceiverState.segment( 3, 3 ), - nominalReceiverState.segment( 3, 3 ),\ - numericalStateDerivative.segment( 3, 3 ), true ); - - for( unsigned int i = 0; i < 3; i++ ) - { - BOOST_CHECK_SMALL( std::fabs( analyticalUnitVectorDerivative( i ) - numericalUnitVectorDerivative( i ) ), 1.0E-17 ); - - } - BOOST_CHECK_SMALL( std::fabs( analyticalProjectedVelocityDerivative / physical_constants::SPEED_OF_LIGHT - - numericalProjectedVelocityDerivative ), 1.5E-22 ); - } - - } +// +// // Test ancilliary functions +// { +// double nominalEvaluationTime = 1.1E7; +// +// // Create environment +// SystemOfBodies bodies = setupEnvironment( groundStations, 1.0E7, 1.2E7, 1.1E7, false ); +// +// // Set link ends for observation model +// LinkDefinition linkEnds; +// linkEnds[ transmitter ] = groundStations[ 1 ]; +// linkEnds[ receiver ] = groundStations[ 0 ]; +// +// // Create transmitter/receriver state functions +// std::function< Eigen::Vector6d( const double ) > transmitterStateFunction = +// getLinkEndCompleteEphemerisFunction< double, double >( linkEnds[ transmitter ], bodies ); +// std::function< Eigen::Vector6d( const double ) > receiverStateFunction = +// getLinkEndCompleteEphemerisFunction< double, double >( linkEnds[ receiver ], bodies ); +// +// // Define (independent!) transmission/reception times +// double transmissionTime = nominalEvaluationTime; +// double receptionTime = nominalEvaluationTime + 1.0E3; +// +// // Compute associated states +// Eigen::Vector6d nominalTransmitterState = transmitterStateFunction( transmissionTime ); +// Eigen::Vector6d nominalReceiverState = receiverStateFunction( receptionTime ); +// Eigen::Vector3d nominalVectorToReceiver = ( nominalReceiverState - nominalTransmitterState ).segment( 0, 3 ); +// +// double timePerturbation = 100.0; +// +// // Partials for fixed receiver +// { +// // Compute numerical derivative of transmitter state for acceleration) +// Eigen::Vector6d numericalStateDerivative = numerical_derivatives::computeCentralDifferenceFromFunction( +// transmitterStateFunction, transmissionTime, timePerturbation, numerical_derivatives::order8 ); +// +// // Compute unit vector derivative numerically +// std::function< Eigen::Vector3d( const double ) > unitVectorFunction = +// std::bind( &computeUnitVectorToReceiverFromTransmitterState, +// nominalReceiverState.segment( 0, 3 ), transmitterStateFunction, std::placeholders::_1 ); +// Eigen::Vector3d numericalUnitVectorDerivative = numerical_derivatives::computeCentralDifferenceFromFunction( +// unitVectorFunction, transmissionTime, timePerturbation, numerical_derivatives::order8 ); +// +// // Compute projected velocoty vector derivative numerically +// std::function< double( const double) > projectedVelocityFunction = +// std::bind( &calculateLineOfSightVelocityAsCFractionFromTransmitterStateFunction< double, double >, +// nominalReceiverState.segment( 0, 3 ), transmitterStateFunction, std::placeholders::_1 ); +// double numericalProjectedVelocityDerivative = +// numerical_derivatives::computeCentralDifferenceFromFunction( +// projectedVelocityFunction, transmissionTime, timePerturbation, numerical_derivatives::order8 ); +// +// // Compute analytical partial derivatives +// Eigen::Vector3d analyticalUnitVectorDerivative = +// -computePartialOfUnitVectorWrtLinkEndTime( +// nominalVectorToReceiver, nominalVectorToReceiver.normalized( ), +// nominalVectorToReceiver.norm( ), nominalTransmitterState.segment( 3, 3 ) ); +// double analyticalProjectedVelocityDerivative = computePartialOfProjectedLinkEndVelocityWrtAssociatedTime( +// nominalVectorToReceiver, +// nominalTransmitterState.segment( 3, 3 ), +// nominalTransmitterState.segment( 3, 3 ), +// numericalStateDerivative.segment( 3, 3 ), false ); +// +// +// for( unsigned int i = 0; i < 3; i++ ) +// { +// BOOST_CHECK_SMALL( std::fabs( analyticalUnitVectorDerivative( i ) - numericalUnitVectorDerivative( i ) ), 1.0E-16 ); +// +// } +// BOOST_CHECK_SMALL( std::fabs( analyticalProjectedVelocityDerivative / physical_constants::SPEED_OF_LIGHT - +// numericalProjectedVelocityDerivative ), 1.0E-21 ); +// } +// +// +// // Partials for fixed transmitter +// { +// // Compute numerical derivative of receiver state for acceleration) +// Eigen::Vector6d numericalStateDerivative = numerical_derivatives::computeCentralDifferenceFromFunction( +// receiverStateFunction, receptionTime, timePerturbation, numerical_derivatives::order8 ); +// +// // Compute unit vector derivative numerically +// std::function< Eigen::Vector3d( const double ) > unitVectorFunction = +// std::bind( &computeUnitVectorToReceiverFromReceiverState, +// receiverStateFunction, nominalTransmitterState.segment( 0, 3 ), std::placeholders::_1 ); +// Eigen::Vector3d numericalUnitVectorDerivative = numerical_derivatives::computeCentralDifferenceFromFunction( +// unitVectorFunction, receptionTime, timePerturbation, numerical_derivatives::order8 ); +// +// // Compute projected velocoty vector derivative numerically +// std::function< double( const double) > projectedVelocityFunction = +// std::bind( &calculateLineOfSightVelocityAsCFractionFromReceiverStateFunction< double, double >, +// receiverStateFunction, nominalTransmitterState.segment( 0, 3 ), std::placeholders::_1 ); +// double numericalProjectedVelocityDerivative = +// numerical_derivatives::computeCentralDifferenceFromFunction( +// projectedVelocityFunction, receptionTime, timePerturbation, numerical_derivatives::order8 ); +// +// // Compute analytical partial derivatives +// Eigen::Vector3d analyticalUnitVectorDerivative = +// computePartialOfUnitVectorWrtLinkEndTime( +// nominalVectorToReceiver, nominalVectorToReceiver.normalized( ), +// nominalVectorToReceiver.norm( ), nominalReceiverState.segment( 3, 3 ) ); +// double analyticalProjectedVelocityDerivative = computePartialOfProjectedLinkEndVelocityWrtAssociatedTime( +// nominalVectorToReceiver, +// nominalReceiverState.segment( 3, 3 ), +// nominalReceiverState.segment( 3, 3 ),\ +// numericalStateDerivative.segment( 3, 3 ), true ); +// +// for( unsigned int i = 0; i < 3; i++ ) +// { +// BOOST_CHECK_SMALL( std::fabs( analyticalUnitVectorDerivative( i ) - numericalUnitVectorDerivative( i ) ), 1.0E-17 ); +// +// } +// BOOST_CHECK_SMALL( std::fabs( analyticalProjectedVelocityDerivative / physical_constants::SPEED_OF_LIGHT - +// numericalProjectedVelocityDerivative ), 1.5E-22 ); +// } +// +// } std::cout<<"A ****************************************** "< > oneWayDopplerModel; - std::vector< std::string > perturbingBodies; - perturbingBodies.push_back( "Earth" ); - if( estimationCase == 0 ) - { - oneWayDopplerModel = - observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( - std::make_shared< observation_models::OneWayDopplerObservationSettings >( - linkEnds, - std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( perturbingBodies ), - nullptr, nullptr, nullptr, - std::make_shared< LightTimeConvergenceCriteria >( ), - static_cast< bool >( normalizeObservable ) ), bodies ); - } - else - { - oneWayDopplerModel = - observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( - std::make_shared< OneWayDopplerObservationSettings > - ( linkEnds, std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( - perturbingBodies ), - std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Mars" ), - std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), - nullptr, std::make_shared< LightTimeConvergenceCriteria >( ), static_cast< bool >( normalizeObservable ) ), bodies ); - } - - // Create parameter objects. - std::shared_ptr< EstimatableParameterSet< double > > fullEstimatableParameterSet; - Eigen::VectorXd parameterPerturbationMultipliers = Eigen::Vector4d::Constant( 1.0 ); - if( estimationCase < 2 ) - { - fullEstimatableParameterSet = createEstimatableParameters( bodies, 1.1E7 ); - } - else - { - fullEstimatableParameterSet = createEstimatableParameters( bodies, 1.1E7, true ); - parameterPerturbationMultipliers( 2 ) = 1.0E-4; - } - std::cout << "Case " <( - oneWayDopplerModel, bodies, fullEstimatableParameterSet, linkEnds, one_way_doppler, 1.0E-5, - true, true, 10.0, parameterPerturbationMultipliers ); - - } - - +// for( unsigned int estimationCase = 0; estimationCase < 3; estimationCase ++ ) +// { +// std::cout<<"ESTIMATION CASE *********************************** "< > oneWayDopplerModel; +// std::vector< std::string > perturbingBodies; +// perturbingBodies.push_back( "Earth" ); +// if( estimationCase == 0 ) +// { +// oneWayDopplerModel = +// observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( +// std::make_shared< observation_models::OneWayDopplerObservationSettings >( +// linkEnds, +// std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( perturbingBodies ), +// nullptr, nullptr, nullptr, +// std::make_shared< LightTimeConvergenceCriteria >( ), +// static_cast< bool >( normalizeObservable ) ), bodies ); +// } +// else +// { +// oneWayDopplerModel = +// observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( +// std::make_shared< OneWayDopplerObservationSettings > +// ( linkEnds, std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( +// perturbingBodies ), +// std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Mars" ), +// std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), +// nullptr, std::make_shared< LightTimeConvergenceCriteria >( ), static_cast< bool >( normalizeObservable ) ), bodies ); +// } +// +// // Create parameter objects. +// std::shared_ptr< EstimatableParameterSet< double > > fullEstimatableParameterSet; +// Eigen::VectorXd parameterPerturbationMultipliers = Eigen::Vector4d::Constant( 1.0 ); +// if( estimationCase < 2 ) +// { +// fullEstimatableParameterSet = createEstimatableParameters( bodies, 1.1E7 ); +// } +// else +// { +// fullEstimatableParameterSet = createEstimatableParameters( bodies, 1.1E7, true ); +// parameterPerturbationMultipliers( 2 ) = 1.0E-4; +// } +// std::cout << "Case " <( +// oneWayDopplerModel, bodies, fullEstimatableParameterSet, linkEnds, one_way_doppler, 1.0E-5, +// true, true, 10.0, parameterPerturbationMultipliers ); +// +// } +// +// // std::cout<<"B ****************************************** "< > oneWayDopplerModel = -// std::dynamic_pointer_cast< OneWayDopplerObservationModel< > >( -// observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( -// std::make_shared< OneWayDopplerObservationSettings > -// ( linkEnds, std::shared_ptr< LightTimeCorrectionSettings >( ), + + std::cout<<"C ****************************************** "< > oneWayDopplerModel = + std::dynamic_pointer_cast< OneWayDopplerObservationModel< > >( + observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( + std::make_shared< OneWayDopplerObservationSettings > + ( linkEnds, std::shared_ptr< LightTimeCorrectionSettings >( ), // std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), -// std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Mars" ), -// nullptr, -// std::make_shared< LightTimeConvergenceCriteria >( ), -// static_cast< bool >( normalizeObservable ) ), bodies ) ); -// -// -// // Extract proper time calculators -// std::shared_ptr< DopplerProperTimeRateInterface > receiverProperTimeRateCalculator = -// oneWayDopplerModel->getReceiverProperTimeRateCalculator( ); -// std::shared_ptr< DopplerProperTimeRateInterface > transmitterProperTimeRateCalculator = -// oneWayDopplerModel->getTransmitterProperTimeRateCalculator( ); -// -// // Create parameter objects. -// std::shared_ptr< EstimatableParameterSet< double > > fullEstimatableParameterSet = -// createEstimatableParameters( bodies, 1.1E7 ); -// -// // Create partials for Doppler with proper time rates -// std::pair< SingleLinkObservationPartialList, std::shared_ptr< PositionPartialScaling > > dopplerPartials = -// ObservationPartialCreator<1, double, double>::createObservationPartials( -// oneWayDopplerModel, bodies, fullEstimatableParameterSet ); -// -// // Retrieve scaling objects and partials with proper time -// std::shared_ptr< OneWayDopplerScaling > partialScalingObject = -// std::dynamic_pointer_cast< OneWayDopplerScaling >( dopplerPartials.second ); -// -// std::shared_ptr< OneWayDopplerProperTimeComponentScaling > transmitterProperTimePartials = -// partialScalingObject->getTransmitterProperTimePartials( ); -// std::shared_ptr< OneWayDopplerProperTimeComponentScaling > receiverProperTimePartials = -// partialScalingObject->getReceiverProperTimePartials( ); -// + std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), + nullptr, + nullptr, + std::make_shared< LightTimeConvergenceCriteria >( ), + static_cast< bool >( normalizeObservable ) ), bodies ) ); + + + // Extract proper time calculators + std::shared_ptr< DopplerProperTimeRateInterface > receiverProperTimeRateCalculator = + oneWayDopplerModel->getReceiverProperTimeRateCalculator( ); + std::shared_ptr< DopplerProperTimeRateInterface > transmitterProperTimeRateCalculator = + oneWayDopplerModel->getTransmitterProperTimeRateCalculator( ); + + std::cout<<"Trans/rec (null) "< > fullEstimatableParameterSet = + createEstimatableParameters( bodies, 1.1E7 ); + + // Create partials for Doppler with proper time rates + std::pair< SingleLinkObservationPartialList, std::shared_ptr< PositionPartialScaling > > dopplerPartials = + ObservationPartialCreator<1, double, double>::createObservationPartials( + oneWayDopplerModel, bodies, fullEstimatableParameterSet ); + + // Retrieve scaling objects and partials with proper time + std::shared_ptr< OneWayDopplerScaling > partialScalingObject = + std::dynamic_pointer_cast< OneWayDopplerScaling >( dopplerPartials.second ); + + std::shared_ptr< OneWayDopplerProperTimeComponentScaling > transmitterProperTimePartials = + partialScalingObject->getTransmitterProperTimePartials( ); + std::shared_ptr< OneWayDopplerProperTimeComponentScaling > receiverProperTimePartials = + partialScalingObject->getReceiverProperTimePartials( ); + + std::cout<<"List size "< > earthStatePartial = // std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( // ( dopplerPartials.first ).begin( )->second ); -// std::shared_ptr< DirectObservationPartial< 1 > > marsStatePartial = -// std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( -// ( ++( ( dopplerPartials.first ).begin( ) ) )->second ); -// -// // Compute nominal observation with proper time -// double observationTime = 1.1E7; -// std::vector< double > linkEndTimes; -// std::vector< Eigen::Vector6d > linkEndStates; -// LinkEndType referenceLinkEnd = transmitter; -// Eigen::VectorXd nominalObservable = oneWayDopplerModel->computeIdealObservationsWithLinkEndData( -// observationTime, referenceLinkEnd, linkEndTimes, linkEndStates ); -// -// std::cout<<"Nominal: "<update( -// linkEndStates, linkEndTimes, referenceLinkEnd, nominalObservable ); -// + std::shared_ptr< DirectObservationPartial< 1 > > marsStatePartial = + std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( + ( ( ( dopplerPartials.first ).begin( ) ) )->second ); + + // Compute nominal observation with proper time + double observationTime = 1.1E7; + std::vector< double > linkEndTimes; + std::vector< Eigen::Vector6d > linkEndStates; + LinkEndType referenceLinkEnd = transmitter; + std::cout<<"Computing nominal observable: "<computeIdealObservationsWithLinkEndData( + observationTime, referenceLinkEnd, linkEndTimes, linkEndStates ); + + std::cout<<"Nominal: "<update( + linkEndStates, linkEndTimes, referenceLinkEnd, nominalObservable ); + // std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > earthStatePartialOutput = // earthStatePartial->calculatePartial( linkEndStates, linkEndTimes, referenceLinkEnd, {}, nominalObservable ); + + std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > marsStatePartialOutput = + marsStatePartial->calculatePartial( linkEndStates, linkEndTimes, referenceLinkEnd, {}, nominalObservable ); + + // Compute numerical proper time rate partials and compare to analytical results + { + + std::function< Eigen::VectorXd( const double ) > transmitterProperTimeRateFunction = + std::bind( &getProperTimeRateInVectorForm, + transmitterProperTimeRateCalculator, oneWayDopplerModel, referenceLinkEnd, std::placeholders::_1 ); + std::cout<<"Mars w.r.t. Mars"< numericalTransmitterProperTimePartialsWrtMarsPosition = + calculatePartialWrtConstantBodyState( + "Mars", bodies, Eigen::Vector3d::Constant( 1000.0E3 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); + + std::cout< numericalTransmitterProperTimePartialsWrtEarthPosition = + calculatePartialWrtConstantBodyState( + "Earth", bodies, Eigen::Vector3d::Constant( 1000.0E3 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); + + std::cout<, double > > marsStatePartialOutput = -// marsStatePartial->calculatePartial( linkEndStates, linkEndTimes, referenceLinkEnd, {}, nominalObservable ); -// -// // Compute numerical proper time rate partials and compare to analytical results -// { -// -// std::function< Eigen::VectorXd( const double ) > transmitterProperTimeRateFunction = -// std::bind( &getProperTimeRateInVectorForm, -// transmitterProperTimeRateCalculator, -// linkEndTimes, linkEndStates, referenceLinkEnd ); -// Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalTransmitterProperTimePartialsWrtMarsPosition = -// calculatePartialWrtConstantBodyState( -// "Earth", bodies, Eigen::Vector3d::Constant( 1000.0E3 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); -// Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalTransmitterProperTimePartialsWrtEarthPosition = -// calculatePartialWrtConstantBodyState( -// "Mars", bodies, Eigen::Vector3d::Constant( 1000.0E3 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); // Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalTransmitterProperTimePartialsWrtMarsVelocity = // calculatePartialWrtConstantBodyVelocity( -// "Earth", bodies, Eigen::Vector3d::Constant( 1.0E0 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); -// Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalTransmitterProperTimePartialsWrtEarthVelocity = -// calculatePartialWrtConstantBodyVelocity( // "Mars", bodies, Eigen::Vector3d::Constant( 1.0E0 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); // -// std::cout<<"Trans. scaling factor: "<getPositionScalingFactor( transmitter )<getPositionScalingFactor( receiver )<getPositionScalingFactor( transmitter ) ), -// ( numericalTransmitterProperTimePartialsWrtMarsPosition ), 1.0E-6 ); -// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( -// ( transmitterProperTimePartials->getPositionScalingFactor( receiver ) ), -// ( numericalTransmitterProperTimePartialsWrtEarthPosition ), 1.0E-6 ); +// Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalTransmitterProperTimePartialsWrtEarthVelocity = +// calculatePartialWrtConstantBodyVelocity( +// "Earth", bodies, Eigen::Vector3d::Constant( 1.0E0 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); + + std::cout<<"Partials w.r.t. Mars: "<getPositionScalingFactor( transmitter )<< + std::endl<getPositionScalingFactor( receiver )<< + std::endl<getPositionScalingFactor( transmitter ) ), + ( numericalTransmitterProperTimePartialsWrtMarsPosition ), 1.0E-6 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ( transmitterProperTimePartials->getPositionScalingFactor( receiver ) ), + ( numericalTransmitterProperTimePartialsWrtEarthPosition ), 1.0E-6 ); // TUDAT_CHECK_MATRIX_CLOSE_FRACTION( // ( transmitterProperTimePartials->getVelocityScalingFactor( transmitter ) ), // ( numericalTransmitterProperTimePartialsWrtMarsVelocity ), 1.0E-6 ); // TUDAT_CHECK_MATRIX_CLOSE_FRACTION( // ( transmitterProperTimePartials->getVelocityScalingFactor( receiver ) ), // ( numericalTransmitterProperTimePartialsWrtEarthVelocity ), 1.0E-6 ); -// + // std::function< Eigen::VectorXd( const double ) > receiverProperTimeRateFunction = // std::bind( &getProperTimeRateInVectorForm, -// receiverProperTimeRateCalculator, -// linkEndTimes, linkEndStates, referenceLinkEnd ); +// receiverProperTimeRateCalculator,oneWayDopplerModel, referenceLinkEnd, std::placeholders::_1 ); // Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtMarsPosition = // calculatePartialWrtConstantBodyState( -// "Earth", bodies, Eigen::Vector3d::Constant( 10000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); +// "Mars", bodies, Eigen::Vector3d::Constant( 10000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); // Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtEarthPosition = // calculatePartialWrtConstantBodyState( -// "Mars", bodies, Eigen::Vector3d::Constant( 10000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); +// "Earth", bodies, Eigen::Vector3d::Constant( 10000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); // Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtMarsVelocity = // calculatePartialWrtConstantBodyVelocity( -// "Earth", bodies, Eigen::Vector3d::Constant( 1000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); +// "Mars", bodies, Eigen::Vector3d::Constant( 1000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); // Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtEarthVelocity = // calculatePartialWrtConstantBodyVelocity( -// "Mars", bodies, Eigen::Vector3d::Constant( 1000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); +// "Earth", bodies, Eigen::Vector3d::Constant( 1000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); +// +// std::cout<<"Trans. scaling factor: "<getPositionScalingFactor( transmitter )<< +// std::endl<getPositionScalingFactor( receiver )<< +// std::endl<getPositionScalingFactor( receiver ) ), @@ -472,7 +498,7 @@ BOOST_AUTO_TEST_CASE( testOneWayDopplerPartials ) // TUDAT_CHECK_MATRIX_CLOSE_FRACTION( // ( receiverProperTimePartials->getVelocityScalingFactor( receiver ) ), // ( numericalReceiverProperTimePartialsWrtEarthVelocity ), 1.0E-6 ); -// } + } // // std::cout<<"D ****************************************** "< calculatePartialWrtConstantBodyState( Eigen::Matrix< double, Eigen::Dynamic, 3 >::Zero( observableSize, 3 ); for( int i = 0; i < 3; i++ ) { + std::cout<<"Up "<updateConstantState( perturbedBodyState ); bodies.at( bodyName )->recomputeStateOnNextCall( ); Eigen::VectorXd upPerturbedObservation = observationFunction( observationTime ); + std::cout<<"Down "< calculatePartialWrtConstantBodyState( numericalPartialWrtBodyPosition.block( 0, i, observableSize, 1 ) = ( upPerturbedObservation - downPerturbedObservation ) / ( 2.0 * bodyPositionVariation( i ) ); + std::cout<updateConstantState( bodyUnperturbedState ); bodies.at( bodyName )->recomputeStateOnNextCall( ); From 544b53abd9316281d7cb2a35cf022a1e18e4fbad Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Tue, 17 Dec 2024 10:40:03 +0100 Subject: [PATCH 10/24] Moving non-link end proper time partials --- .../oneWayDopplerPartial.cpp | 1 + .../relativity/relativisticTimeConversion.cpp | 4 - .../unitTestOneWayDopplerPartials.cpp | 360 +++++++++--------- .../observationPartialTestFunctions.cpp | 3 - 4 files changed, 178 insertions(+), 190 deletions(-) diff --git a/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp b/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp index b1054619f1..d84336fc45 100644 --- a/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp +++ b/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp @@ -313,6 +313,7 @@ int OneWayDopplerScaling::getProperTimeParameterDependencySize( std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > OneWayDopplerScaling::getLinkIndependentPartials( const estimatable_parameters::EstimatebleParameterIdentifier parameterType ) { + std::cout<, double > > totalPartial; if( parameterType.first == estimatable_parameters::equivalence_principle_lpi_violation_parameter ) diff --git a/src/astro/relativity/relativisticTimeConversion.cpp b/src/astro/relativity/relativisticTimeConversion.cpp index 81ad81e65e..f97d10778b 100644 --- a/src/astro/relativity/relativisticTimeConversion.cpp +++ b/src/astro/relativity/relativisticTimeConversion.cpp @@ -42,10 +42,6 @@ double calculateFirstCentralBodyProperTimeRateDifference( centralBodyGravitationalParameters.at( i ) / ( perturbedInertialStates.at( i ).segment( 0, 3 ) - computationPointState.segment( 0, 3 ) ).norm( ); } - std::cout<<"COMPUTING "< transmitterStateFunction = -// getLinkEndCompleteEphemerisFunction< double, double >( linkEnds[ transmitter ], bodies ); -// std::function< Eigen::Vector6d( const double ) > receiverStateFunction = -// getLinkEndCompleteEphemerisFunction< double, double >( linkEnds[ receiver ], bodies ); -// -// // Define (independent!) transmission/reception times -// double transmissionTime = nominalEvaluationTime; -// double receptionTime = nominalEvaluationTime + 1.0E3; -// -// // Compute associated states -// Eigen::Vector6d nominalTransmitterState = transmitterStateFunction( transmissionTime ); -// Eigen::Vector6d nominalReceiverState = receiverStateFunction( receptionTime ); -// Eigen::Vector3d nominalVectorToReceiver = ( nominalReceiverState - nominalTransmitterState ).segment( 0, 3 ); -// -// double timePerturbation = 100.0; -// -// // Partials for fixed receiver -// { -// // Compute numerical derivative of transmitter state for acceleration) -// Eigen::Vector6d numericalStateDerivative = numerical_derivatives::computeCentralDifferenceFromFunction( -// transmitterStateFunction, transmissionTime, timePerturbation, numerical_derivatives::order8 ); -// -// // Compute unit vector derivative numerically -// std::function< Eigen::Vector3d( const double ) > unitVectorFunction = -// std::bind( &computeUnitVectorToReceiverFromTransmitterState, -// nominalReceiverState.segment( 0, 3 ), transmitterStateFunction, std::placeholders::_1 ); -// Eigen::Vector3d numericalUnitVectorDerivative = numerical_derivatives::computeCentralDifferenceFromFunction( -// unitVectorFunction, transmissionTime, timePerturbation, numerical_derivatives::order8 ); -// -// // Compute projected velocoty vector derivative numerically -// std::function< double( const double) > projectedVelocityFunction = -// std::bind( &calculateLineOfSightVelocityAsCFractionFromTransmitterStateFunction< double, double >, -// nominalReceiverState.segment( 0, 3 ), transmitterStateFunction, std::placeholders::_1 ); -// double numericalProjectedVelocityDerivative = -// numerical_derivatives::computeCentralDifferenceFromFunction( -// projectedVelocityFunction, transmissionTime, timePerturbation, numerical_derivatives::order8 ); -// -// // Compute analytical partial derivatives -// Eigen::Vector3d analyticalUnitVectorDerivative = -// -computePartialOfUnitVectorWrtLinkEndTime( -// nominalVectorToReceiver, nominalVectorToReceiver.normalized( ), -// nominalVectorToReceiver.norm( ), nominalTransmitterState.segment( 3, 3 ) ); -// double analyticalProjectedVelocityDerivative = computePartialOfProjectedLinkEndVelocityWrtAssociatedTime( -// nominalVectorToReceiver, -// nominalTransmitterState.segment( 3, 3 ), -// nominalTransmitterState.segment( 3, 3 ), -// numericalStateDerivative.segment( 3, 3 ), false ); -// -// -// for( unsigned int i = 0; i < 3; i++ ) -// { -// BOOST_CHECK_SMALL( std::fabs( analyticalUnitVectorDerivative( i ) - numericalUnitVectorDerivative( i ) ), 1.0E-16 ); -// -// } -// BOOST_CHECK_SMALL( std::fabs( analyticalProjectedVelocityDerivative / physical_constants::SPEED_OF_LIGHT - -// numericalProjectedVelocityDerivative ), 1.0E-21 ); -// } -// -// -// // Partials for fixed transmitter -// { -// // Compute numerical derivative of receiver state for acceleration) -// Eigen::Vector6d numericalStateDerivative = numerical_derivatives::computeCentralDifferenceFromFunction( -// receiverStateFunction, receptionTime, timePerturbation, numerical_derivatives::order8 ); -// -// // Compute unit vector derivative numerically -// std::function< Eigen::Vector3d( const double ) > unitVectorFunction = -// std::bind( &computeUnitVectorToReceiverFromReceiverState, -// receiverStateFunction, nominalTransmitterState.segment( 0, 3 ), std::placeholders::_1 ); -// Eigen::Vector3d numericalUnitVectorDerivative = numerical_derivatives::computeCentralDifferenceFromFunction( -// unitVectorFunction, receptionTime, timePerturbation, numerical_derivatives::order8 ); -// -// // Compute projected velocoty vector derivative numerically -// std::function< double( const double) > projectedVelocityFunction = -// std::bind( &calculateLineOfSightVelocityAsCFractionFromReceiverStateFunction< double, double >, -// receiverStateFunction, nominalTransmitterState.segment( 0, 3 ), std::placeholders::_1 ); -// double numericalProjectedVelocityDerivative = -// numerical_derivatives::computeCentralDifferenceFromFunction( -// projectedVelocityFunction, receptionTime, timePerturbation, numerical_derivatives::order8 ); -// -// // Compute analytical partial derivatives -// Eigen::Vector3d analyticalUnitVectorDerivative = -// computePartialOfUnitVectorWrtLinkEndTime( -// nominalVectorToReceiver, nominalVectorToReceiver.normalized( ), -// nominalVectorToReceiver.norm( ), nominalReceiverState.segment( 3, 3 ) ); -// double analyticalProjectedVelocityDerivative = computePartialOfProjectedLinkEndVelocityWrtAssociatedTime( -// nominalVectorToReceiver, -// nominalReceiverState.segment( 3, 3 ), -// nominalReceiverState.segment( 3, 3 ),\ -// numericalStateDerivative.segment( 3, 3 ), true ); -// -// for( unsigned int i = 0; i < 3; i++ ) -// { -// BOOST_CHECK_SMALL( std::fabs( analyticalUnitVectorDerivative( i ) - numericalUnitVectorDerivative( i ) ), 1.0E-17 ); -// -// } -// BOOST_CHECK_SMALL( std::fabs( analyticalProjectedVelocityDerivative / physical_constants::SPEED_OF_LIGHT - -// numericalProjectedVelocityDerivative ), 1.5E-22 ); -// } -// -// } + + // Test ancilliary functions + { + double nominalEvaluationTime = 1.1E7; + + // Create environment + SystemOfBodies bodies = setupEnvironment( groundStations, 1.0E7, 1.2E7, 1.1E7, false ); + + // Set link ends for observation model + LinkDefinition linkEnds; + linkEnds[ transmitter ] = groundStations[ 1 ]; + linkEnds[ receiver ] = groundStations[ 0 ]; + + // Create transmitter/receriver state functions + std::function< Eigen::Vector6d( const double ) > transmitterStateFunction = + getLinkEndCompleteEphemerisFunction< double, double >( linkEnds[ transmitter ], bodies ); + std::function< Eigen::Vector6d( const double ) > receiverStateFunction = + getLinkEndCompleteEphemerisFunction< double, double >( linkEnds[ receiver ], bodies ); + + // Define (independent!) transmission/reception times + double transmissionTime = nominalEvaluationTime; + double receptionTime = nominalEvaluationTime + 1.0E3; + + // Compute associated states + Eigen::Vector6d nominalTransmitterState = transmitterStateFunction( transmissionTime ); + Eigen::Vector6d nominalReceiverState = receiverStateFunction( receptionTime ); + Eigen::Vector3d nominalVectorToReceiver = ( nominalReceiverState - nominalTransmitterState ).segment( 0, 3 ); + + double timePerturbation = 100.0; + + // Partials for fixed receiver + { + // Compute numerical derivative of transmitter state for acceleration) + Eigen::Vector6d numericalStateDerivative = numerical_derivatives::computeCentralDifferenceFromFunction( + transmitterStateFunction, transmissionTime, timePerturbation, numerical_derivatives::order8 ); + + // Compute unit vector derivative numerically + std::function< Eigen::Vector3d( const double ) > unitVectorFunction = + std::bind( &computeUnitVectorToReceiverFromTransmitterState, + nominalReceiverState.segment( 0, 3 ), transmitterStateFunction, std::placeholders::_1 ); + Eigen::Vector3d numericalUnitVectorDerivative = numerical_derivatives::computeCentralDifferenceFromFunction( + unitVectorFunction, transmissionTime, timePerturbation, numerical_derivatives::order8 ); + + // Compute projected velocoty vector derivative numerically + std::function< double( const double) > projectedVelocityFunction = + std::bind( &calculateLineOfSightVelocityAsCFractionFromTransmitterStateFunction< double, double >, + nominalReceiverState.segment( 0, 3 ), transmitterStateFunction, std::placeholders::_1 ); + double numericalProjectedVelocityDerivative = + numerical_derivatives::computeCentralDifferenceFromFunction( + projectedVelocityFunction, transmissionTime, timePerturbation, numerical_derivatives::order8 ); + + // Compute analytical partial derivatives + Eigen::Vector3d analyticalUnitVectorDerivative = + -computePartialOfUnitVectorWrtLinkEndTime( + nominalVectorToReceiver, nominalVectorToReceiver.normalized( ), + nominalVectorToReceiver.norm( ), nominalTransmitterState.segment( 3, 3 ) ); + double analyticalProjectedVelocityDerivative = computePartialOfProjectedLinkEndVelocityWrtAssociatedTime( + nominalVectorToReceiver, + nominalTransmitterState.segment( 3, 3 ), + nominalTransmitterState.segment( 3, 3 ), + numericalStateDerivative.segment( 3, 3 ), false ); + + + for( unsigned int i = 0; i < 3; i++ ) + { + BOOST_CHECK_SMALL( std::fabs( analyticalUnitVectorDerivative( i ) - numericalUnitVectorDerivative( i ) ), 1.0E-16 ); + + } + BOOST_CHECK_SMALL( std::fabs( analyticalProjectedVelocityDerivative / physical_constants::SPEED_OF_LIGHT - + numericalProjectedVelocityDerivative ), 1.0E-21 ); + } + + + // Partials for fixed transmitter + { + // Compute numerical derivative of receiver state for acceleration) + Eigen::Vector6d numericalStateDerivative = numerical_derivatives::computeCentralDifferenceFromFunction( + receiverStateFunction, receptionTime, timePerturbation, numerical_derivatives::order8 ); + + // Compute unit vector derivative numerically + std::function< Eigen::Vector3d( const double ) > unitVectorFunction = + std::bind( &computeUnitVectorToReceiverFromReceiverState, + receiverStateFunction, nominalTransmitterState.segment( 0, 3 ), std::placeholders::_1 ); + Eigen::Vector3d numericalUnitVectorDerivative = numerical_derivatives::computeCentralDifferenceFromFunction( + unitVectorFunction, receptionTime, timePerturbation, numerical_derivatives::order8 ); + + // Compute projected velocoty vector derivative numerically + std::function< double( const double) > projectedVelocityFunction = + std::bind( &calculateLineOfSightVelocityAsCFractionFromReceiverStateFunction< double, double >, + receiverStateFunction, nominalTransmitterState.segment( 0, 3 ), std::placeholders::_1 ); + double numericalProjectedVelocityDerivative = + numerical_derivatives::computeCentralDifferenceFromFunction( + projectedVelocityFunction, receptionTime, timePerturbation, numerical_derivatives::order8 ); + + // Compute analytical partial derivatives + Eigen::Vector3d analyticalUnitVectorDerivative = + computePartialOfUnitVectorWrtLinkEndTime( + nominalVectorToReceiver, nominalVectorToReceiver.normalized( ), + nominalVectorToReceiver.norm( ), nominalReceiverState.segment( 3, 3 ) ); + double analyticalProjectedVelocityDerivative = computePartialOfProjectedLinkEndVelocityWrtAssociatedTime( + nominalVectorToReceiver, + nominalReceiverState.segment( 3, 3 ), + nominalReceiverState.segment( 3, 3 ),\ + numericalStateDerivative.segment( 3, 3 ), true ); + + for( unsigned int i = 0; i < 3; i++ ) + { + BOOST_CHECK_SMALL( std::fabs( analyticalUnitVectorDerivative( i ) - numericalUnitVectorDerivative( i ) ), 1.0E-17 ); + + } + BOOST_CHECK_SMALL( std::fabs( analyticalProjectedVelocityDerivative / physical_constants::SPEED_OF_LIGHT - + numericalProjectedVelocityDerivative ), 1.5E-22 ); + } + + } std::cout<<"A ****************************************** "<::createObservationModel( std::make_shared< OneWayDopplerObservationSettings > ( linkEnds, std::shared_ptr< LightTimeCorrectionSettings >( ), -// std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), - nullptr, + std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Mars" ), nullptr, std::make_shared< LightTimeConvergenceCriteria >( ), static_cast< bool >( normalizeObservable ) ), bodies ) ); @@ -388,12 +387,12 @@ BOOST_AUTO_TEST_CASE( testOneWayDopplerPartials ) partialScalingObject->getReceiverProperTimePartials( ); std::cout<<"List size "< > earthStatePartial = -// std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( -// ( dopplerPartials.first ).begin( )->second ); + std::shared_ptr< DirectObservationPartial< 1 > > earthStatePartial = + std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( + ( dopplerPartials.first ).begin( )->second ); std::shared_ptr< DirectObservationPartial< 1 > > marsStatePartial = std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( - ( ( ( dopplerPartials.first ).begin( ) ) )->second ); + ( ++( ( dopplerPartials.first ).begin( ) ) )->second ); // Compute nominal observation with proper time double observationTime = 1.1E7; @@ -411,8 +410,8 @@ BOOST_AUTO_TEST_CASE( testOneWayDopplerPartials ) partialScalingObject->update( linkEndStates, linkEndTimes, referenceLinkEnd, nominalObservable ); -// std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > earthStatePartialOutput = -// earthStatePartial->calculatePartial( linkEndStates, linkEndTimes, referenceLinkEnd, {}, nominalObservable ); + std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > earthStatePartialOutput = + earthStatePartial->calculatePartial( linkEndStates, linkEndTimes, referenceLinkEnd, {}, nominalObservable ); std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > marsStatePartialOutput = marsStatePartial->calculatePartial( linkEndStates, linkEndTimes, referenceLinkEnd, {}, nominalObservable ); @@ -423,27 +422,22 @@ BOOST_AUTO_TEST_CASE( testOneWayDopplerPartials ) std::function< Eigen::VectorXd( const double ) > transmitterProperTimeRateFunction = std::bind( &getProperTimeRateInVectorForm, transmitterProperTimeRateCalculator, oneWayDopplerModel, referenceLinkEnd, std::placeholders::_1 ); - std::cout<<"Mars w.r.t. Mars"< numericalTransmitterProperTimePartialsWrtMarsPosition = calculatePartialWrtConstantBodyState( "Mars", bodies, Eigen::Vector3d::Constant( 1000.0E3 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); - std::cout< numericalTransmitterProperTimePartialsWrtEarthPosition = + Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalTransmitterProperTimePartialsWrtEarthPosition = calculatePartialWrtConstantBodyState( "Earth", bodies, Eigen::Vector3d::Constant( 1000.0E3 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); - std::cout< numericalTransmitterProperTimePartialsWrtMarsVelocity = -// calculatePartialWrtConstantBodyVelocity( -// "Mars", bodies, Eigen::Vector3d::Constant( 1.0E0 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); -// -// Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalTransmitterProperTimePartialsWrtEarthVelocity = -// calculatePartialWrtConstantBodyVelocity( -// "Earth", bodies, Eigen::Vector3d::Constant( 1.0E0 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); + Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalTransmitterProperTimePartialsWrtMarsVelocity = + calculatePartialWrtConstantBodyVelocity( + "Mars", bodies, Eigen::Vector3d::Constant( 1.0E0 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); + + Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalTransmitterProperTimePartialsWrtEarthVelocity = + calculatePartialWrtConstantBodyVelocity( + "Earth", bodies, Eigen::Vector3d::Constant( 1.0E0 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); std::cout<<"Partials w.r.t. Mars: "<getPositionScalingFactor( transmitter )<< std::endl<getPositionScalingFactor( receiver ) ), ( numericalTransmitterProperTimePartialsWrtEarthPosition ), 1.0E-6 ); -// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( -// ( transmitterProperTimePartials->getVelocityScalingFactor( transmitter ) ), -// ( numericalTransmitterProperTimePartialsWrtMarsVelocity ), 1.0E-6 ); -// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( -// ( transmitterProperTimePartials->getVelocityScalingFactor( receiver ) ), -// ( numericalTransmitterProperTimePartialsWrtEarthVelocity ), 1.0E-6 ); - -// std::function< Eigen::VectorXd( const double ) > receiverProperTimeRateFunction = -// std::bind( &getProperTimeRateInVectorForm, -// receiverProperTimeRateCalculator,oneWayDopplerModel, referenceLinkEnd, std::placeholders::_1 ); -// Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtMarsPosition = -// calculatePartialWrtConstantBodyState( -// "Mars", bodies, Eigen::Vector3d::Constant( 10000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); -// Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtEarthPosition = -// calculatePartialWrtConstantBodyState( -// "Earth", bodies, Eigen::Vector3d::Constant( 10000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); -// Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtMarsVelocity = -// calculatePartialWrtConstantBodyVelocity( -// "Mars", bodies, Eigen::Vector3d::Constant( 1000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); -// Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtEarthVelocity = -// calculatePartialWrtConstantBodyVelocity( -// "Earth", bodies, Eigen::Vector3d::Constant( 1000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); -// -// std::cout<<"Trans. scaling factor: "<getPositionScalingFactor( transmitter )<< -// std::endl<getPositionScalingFactor( receiver )<< -// std::endl<getPositionScalingFactor( receiver ) ), -// ( numericalReceiverProperTimePartialsWrtEarthPosition ), 1.0E-6 ); -// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( -// ( receiverProperTimePartials->getPositionScalingFactor( transmitter ) ), -// ( numericalReceiverProperTimePartialsWrtMarsPosition ), 1.0E-6 ); -// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( -// ( receiverProperTimePartials->getVelocityScalingFactor( transmitter ) ), -// ( numericalReceiverProperTimePartialsWrtMarsVelocity ), 1.0E-6 ); -// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( -// ( receiverProperTimePartials->getVelocityScalingFactor( receiver ) ), -// ( numericalReceiverProperTimePartialsWrtEarthVelocity ), 1.0E-6 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ( transmitterProperTimePartials->getVelocityScalingFactor( transmitter ) ), + ( numericalTransmitterProperTimePartialsWrtMarsVelocity ), 1.0E-6 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ( transmitterProperTimePartials->getVelocityScalingFactor( receiver ) ), + ( numericalTransmitterProperTimePartialsWrtEarthVelocity ), 1.0E-6 ); + + std::function< Eigen::VectorXd( const double ) > receiverProperTimeRateFunction = + std::bind( &getProperTimeRateInVectorForm, + receiverProperTimeRateCalculator,oneWayDopplerModel, referenceLinkEnd, std::placeholders::_1 ); + Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtMarsPosition = + calculatePartialWrtConstantBodyState( + "Mars", bodies, Eigen::Vector3d::Constant( 10000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); + Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtEarthPosition = + calculatePartialWrtConstantBodyState( + "Earth", bodies, Eigen::Vector3d::Constant( 10000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); + Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtMarsVelocity = + calculatePartialWrtConstantBodyVelocity( + "Mars", bodies, Eigen::Vector3d::Constant( 1000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); + Eigen::Matrix< double, Eigen::Dynamic, 3 > numericalReceiverProperTimePartialsWrtEarthVelocity = + calculatePartialWrtConstantBodyVelocity( + "Earth", bodies, Eigen::Vector3d::Constant( 1000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); + + std::cout<<"Trans. scaling factor: "<getPositionScalingFactor( transmitter )<< + std::endl<getPositionScalingFactor( receiver )<< + std::endl<getPositionScalingFactor( receiver ) ), + ( numericalReceiverProperTimePartialsWrtEarthPosition ), 1.0E-6 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ( receiverProperTimePartials->getPositionScalingFactor( transmitter ) ), + ( numericalReceiverProperTimePartialsWrtMarsPosition ), 1.0E-6 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ( receiverProperTimePartials->getVelocityScalingFactor( transmitter ) ), + ( numericalReceiverProperTimePartialsWrtMarsVelocity ), 1.0E-6 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ( receiverProperTimePartials->getVelocityScalingFactor( receiver ) ), + ( numericalReceiverProperTimePartialsWrtEarthVelocity ), 1.0E-6 ); } -// + // std::cout<<"D ****************************************** "< calculatePartialWrtConstantBodyState( Eigen::Matrix< double, Eigen::Dynamic, 3 >::Zero( observableSize, 3 ); for( int i = 0; i < 3; i++ ) { - std::cout<<"Up "<updateConstantState( perturbedBodyState ); bodies.at( bodyName )->recomputeStateOnNextCall( ); Eigen::VectorXd upPerturbedObservation = observationFunction( observationTime ); - std::cout<<"Down "< calculatePartialWrtConstantBodyState( numericalPartialWrtBodyPosition.block( 0, i, observableSize, 1 ) = ( upPerturbedObservation - downPerturbedObservation ) / ( 2.0 * bodyPositionVariation( i ) ); - std::cout<updateConstantState( bodyUnperturbedState ); bodies.at( bodyName )->recomputeStateOnNextCall( ); From 0bd3d5f53c2de95b82181fd4c5bf8ab3ac9ff135 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Tue, 17 Dec 2024 14:13:18 +0100 Subject: [PATCH 11/24] Working further on doppler tests --- .../oneWayDopplerObservationModel.h | 18 + .../oneWayDopplerPartial.h | 19 +- .../createPositionPartialScaling.h | 2 +- .../oneWayDopplerPartial.cpp | 87 ++++- .../support/observationPartialTestFunctions.h | 2 +- .../unitTestOneWayDopplerPartials.cpp | 369 +++++++++--------- 6 files changed, 284 insertions(+), 213 deletions(-) diff --git a/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h b/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h index 0492f1f546..e9ff2d2a75 100644 --- a/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h +++ b/include/tudat/astro/observation_models/oneWayDopplerObservationModel.h @@ -415,6 +415,24 @@ class DirectFirstOrderDopplerProperTimeRateInterface: return false; } + int getBodyIndex( const std::string bodyName ) + { + for( unsigned int i = 0; i < perturbingBodyNames_.size( ); i++ ) + { + if( bodyName == perturbingBodyNames_.at( i ) ) + { + return i; + } + } + return -1; + } + + std::vector< std::string > getPerturbingBodyNames( ) + { + return perturbingBodyNames_; + } + + std::vector< Eigen::Vector6d > getCurrentPerturbedStates( ) { return currentPerturbedStates_; diff --git a/include/tudat/astro/orbit_determination/observation_partials/oneWayDopplerPartial.h b/include/tudat/astro/orbit_determination/observation_partials/oneWayDopplerPartial.h index 08c0bf01bd..4f76ba39a2 100644 --- a/include/tudat/astro/orbit_determination/observation_partials/oneWayDopplerPartial.h +++ b/include/tudat/astro/orbit_determination/observation_partials/oneWayDopplerPartial.h @@ -105,10 +105,8 @@ class OneWayDopplerDirectFirstOrderProperTimeComponentScaling: public OneWayDopp OneWayDopplerDirectFirstOrderProperTimeComponentScaling( const std::shared_ptr< observation_models::DirectFirstOrderDopplerProperTimeRateInterface > properTimeRateModel, const observation_models::LinkEndType linkEndWithPartial, - const bool computeStatePartials ): - OneWayDopplerProperTimeComponentScaling( linkEndWithPartial ), - properTimeRateModel_( properTimeRateModel ), - computeStatePartials_( computeStatePartials ){ } + const observation_models::LinkEnds linkEnds, + const bool computeStatePartials ); //! Update the scaling object to the current times and states /*! @@ -147,7 +145,7 @@ class OneWayDopplerDirectFirstOrderProperTimeComponentScaling: public OneWayDopp */ double getEquivalencePrincipleViolationParameterPartial( ) { - return -currentGravitationalParameter_ / ( currentDistance_ * physical_constants::SPEED_OF_LIGHT ); + return -currentScalarPotential_ / physical_constants::SPEED_OF_LIGHT; } //! Function to get the direct partial derivative, and associated time, of proper time @@ -177,6 +175,8 @@ class OneWayDopplerDirectFirstOrderProperTimeComponentScaling: public OneWayDopp //! Partial of proper time rate w.r.t. position, as computed by last call to update function. Eigen::Matrix< double, 1, 3 > partialWrPosition_; + std::vector< Eigen::Matrix< double, 1, 3 > > partialWrtPerturbedPositions_; + //! Partial of proper time rate w.r.t. velocity, as computed by last call to update function. Eigen::Matrix< double, 1, 3 > partialWrtVelocity_; @@ -189,12 +189,21 @@ class OneWayDopplerDirectFirstOrderProperTimeComponentScaling: public OneWayDopp //! Current value of gravitational parameter of central body. double currentGravitationalParameter_; + double currentScalarPotential_; + //! Boolean to denote whether state partials are to be computed /*! * Boolean to denote whether state partials are to be computed. It is false if the link end for whicj this object computes * the proper time partials is fixed to the perturbing body. */ bool computeStatePartials_; + + observation_models::LinkEndId oppositeLinkEnd_; + + int oppositeBodyIndex_; + + int skipBodyIndex_; + }; //! Derived class for scaling three-dimensional position partial to one-way doppler observable partial diff --git a/include/tudat/simulation/estimation_setup/createPositionPartialScaling.h b/include/tudat/simulation/estimation_setup/createPositionPartialScaling.h index d54b4ae85a..341b53c44e 100644 --- a/include/tudat/simulation/estimation_setup/createPositionPartialScaling.h +++ b/include/tudat/simulation/estimation_setup/createPositionPartialScaling.h @@ -54,7 +54,7 @@ inline std::shared_ptr< OneWayDopplerProperTimeComponentScaling > createDopplerP dopplerProperTimeInterface )->matchWithBody( oneWayDopplerLinkEnds.at( linkEndAtWhichPartialIsComputed ).bodyName_ ) ); properTimeRateDopplerPartial = std::make_shared< OneWayDopplerDirectFirstOrderProperTimeComponentScaling >( std::dynamic_pointer_cast< observation_models::DirectFirstOrderDopplerProperTimeRateInterface >( - dopplerProperTimeInterface ), linkEndAtWhichPartialIsComputed, computeStatePartials ); + dopplerProperTimeInterface ), linkEndAtWhichPartialIsComputed, oneWayDopplerLinkEnds, computeStatePartials ); } else { diff --git a/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp b/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp index d84336fc45..e790ff7a26 100644 --- a/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp +++ b/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp @@ -44,31 +44,77 @@ double computePartialOfProjectedLinkEndVelocityWrtAssociatedTime( static_cast< double >( projectedLinkEndIsVariableLinkEnd ) * normalizedVector.dot( projectedLinkEndAcceleration ); } +OneWayDopplerDirectFirstOrderProperTimeComponentScaling::OneWayDopplerDirectFirstOrderProperTimeComponentScaling( + const std::shared_ptr< observation_models::DirectFirstOrderDopplerProperTimeRateInterface > properTimeRateModel, + const observation_models::LinkEndType linkEndWithPartial, + const observation_models::LinkEnds linkEnds, + const bool computeStatePartials ): + OneWayDopplerProperTimeComponentScaling( linkEndWithPartial ), + properTimeRateModel_( properTimeRateModel ), + computeStatePartials_( computeStatePartials ) +{ + partialWrtPerturbedPositions_.resize( properTimeRateModel->getGravitationalParameters( ).size( ) ); + if( linkEndWithPartial_ == observation_models::transmitter ) + { + oppositeLinkEnd_ = linkEnds.at( observation_models::receiver ); + } + else if( linkEndWithPartial_ == observation_models::receiver ) + { + oppositeLinkEnd_ = linkEnds.at( observation_models::transmitter ); + } + + skipBodyIndex_ = properTimeRateModel->getBodyIndex( linkEnds.at( linkEndWithPartial_ ).bodyName_ ); + oppositeBodyIndex_ = properTimeRateModel->getBodyIndex( oppositeLinkEnd_.bodyName_ ); +} + //! Update the scaling object to the current times and states void OneWayDopplerDirectFirstOrderProperTimeComponentScaling::update( const std::vector< Eigen::Vector6d >& linkEndStates, const std::vector< double >& times, const observation_models::LinkEndType fixedLinkEnd, const Eigen::VectorXd currentObservation ) { - // Get relative state - Eigen::Vector6d relativeState = properTimeRateModel_->getComputationPointState( + // Get state of computation point + Eigen::Vector6d computationPointState = properTimeRateModel_->getComputationPointState( times, linkEndStates ); - currentGravitationalParameter_ = properTimeRateModel_->getGravitationalParameters( ).at( 0 )( ); + // Set evaluation time of proper time derivative currentLinkEndTime_ == ( linkEndWithPartial_ == observation_models::transmitter ) ? ( times.at( 0 ) ) : ( times.at( 1 ) ); - Eigen::Vector6d perturberRelativeState = properTimeRateModel_->getCurrentPerturbedStates( ).at( 0 ); - currentDistance_ = ( relativeState - perturberRelativeState ).segment( 0, 3 ).norm( ); + currentScalarPotential_ = 0.0; // Compute partials w.r.t. position and velocity if( computeStatePartials_ ) { - - partialWrPosition_ = physical_constants::INVERSE_SQUARE_SPEED_OF_LIGHT * - ( 1.0 + relativity::equivalencePrincipleLpiViolationParameter ) * - currentGravitationalParameter_ / ( currentDistance_ * currentDistance_ ) * - ( ( relativeState - perturberRelativeState ).segment( 0, 3 ).normalized( ) ).transpose( ); + // Set partial w.r.t. velocity (only for computation point) partialWrtVelocity_ = -physical_constants::INVERSE_SQUARE_SPEED_OF_LIGHT * - ( relativeState.segment( 3, 3 ) ).transpose( ); + ( computationPointState.segment( 3, 3 )).transpose( ); + } + else + { + partialWrtVelocity_.setZero( ); + } + partialWrPosition_.setZero( ); + + currentScalarPotential_ = 0.0; + for( unsigned int i = 0; i < partialWrtPerturbedPositions_.size( ); i++ ) + { + currentGravitationalParameter_ = properTimeRateModel_->getGravitationalParameters( ).at( i )( ); + + + Eigen::Vector6d perturberRelativeState = properTimeRateModel_->getCurrentPerturbedStates( ).at( i ); + currentDistance_ = ( computationPointState - perturberRelativeState ).segment( 0, 3 ).norm( ); + + currentScalarPotential_ += currentGravitationalParameter_ / currentDistance_; + if( computeStatePartials_ ) + { + partialWrtPerturbedPositions_[ i ] = -physical_constants::INVERSE_SQUARE_SPEED_OF_LIGHT * + ( 1.0 + relativity::equivalencePrincipleLpiViolationParameter ) * + currentGravitationalParameter_ / ( currentDistance_ * currentDistance_ ) * + ( ( computationPointState - perturberRelativeState ).segment( 0, 3 ).normalized( ) ).transpose( ); + if( !( static_cast< int >( i ) == skipBodyIndex_ ) ) + { + partialWrPosition_ -= partialWrtPerturbedPositions_[ i ]; + } + } } } @@ -78,8 +124,18 @@ Eigen::Matrix< double, 1, 3 > OneWayDopplerDirectFirstOrderProperTimeComponentSc { if( computeStatePartials_ ) { - return partialWrPosition_ * ( ( linkEndType == properTimeRateModel_->getComputationPointLinkEndType( ) ) ? - ( 1.0 ) : ( 0.0 ) ); + if ( linkEndType == properTimeRateModel_->getComputationPointLinkEndType( )) + { + return partialWrPosition_; + } + else if ( oppositeBodyIndex_ >= 0 ) + { + return partialWrtPerturbedPositions_.at( oppositeBodyIndex_ ); + } + else + { + return Eigen::Matrix::Zero( ); + } } else { @@ -313,7 +369,6 @@ int OneWayDopplerScaling::getProperTimeParameterDependencySize( std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > OneWayDopplerScaling::getLinkIndependentPartials( const estimatable_parameters::EstimatebleParameterIdentifier parameterType ) { - std::cout<, double > > totalPartial; if( parameterType.first == estimatable_parameters::equivalence_principle_lpi_violation_parameter ) @@ -346,6 +401,10 @@ std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > O { throw std::runtime_error( "Error, proper time parameter partials have inconsistent size" ); } + } + if( parameterType.first == estimatable_parameters::initial_body_state ) + { + } return totalPartial; } diff --git a/tests/include/tudat/support/observationPartialTestFunctions.h b/tests/include/tudat/support/observationPartialTestFunctions.h index 2e3f5a78cd..6262d3d550 100644 --- a/tests/include/tudat/support/observationPartialTestFunctions.h +++ b/tests/include/tudat/support/observationPartialTestFunctions.h @@ -426,7 +426,7 @@ void testObservationPartials( currentParameterPartial += analyticalObservationPartials[i + numberOfEstimatedBodies].at( j ).first; } - std::cout<<"Current double partial "< > oneWayDopplerModel; -// std::vector< std::string > perturbingBodies; -// perturbingBodies.push_back( "Earth" ); -// if( estimationCase == 0 ) -// { -// oneWayDopplerModel = -// observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( -// std::make_shared< observation_models::OneWayDopplerObservationSettings >( -// linkEnds, -// std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( perturbingBodies ), -// nullptr, nullptr, nullptr, -// std::make_shared< LightTimeConvergenceCriteria >( ), -// static_cast< bool >( normalizeObservable ) ), bodies ); -// } -// else -// { -// oneWayDopplerModel = -// observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( -// std::make_shared< OneWayDopplerObservationSettings > -// ( linkEnds, std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( -// perturbingBodies ), -// std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Mars" ), -// std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), -// nullptr, std::make_shared< LightTimeConvergenceCriteria >( ), static_cast< bool >( normalizeObservable ) ), bodies ); -// } -// -// // Create parameter objects. -// std::shared_ptr< EstimatableParameterSet< double > > fullEstimatableParameterSet; -// Eigen::VectorXd parameterPerturbationMultipliers = Eigen::Vector4d::Constant( 1.0 ); -// if( estimationCase < 2 ) -// { -// fullEstimatableParameterSet = createEstimatableParameters( bodies, 1.1E7 ); -// } -// else -// { -// fullEstimatableParameterSet = createEstimatableParameters( bodies, 1.1E7, true ); -// parameterPerturbationMultipliers( 2 ) = 1.0E-4; -// } -// std::cout << "Case " <( -// oneWayDopplerModel, bodies, fullEstimatableParameterSet, linkEnds, one_way_doppler, 1.0E-5, -// true, true, 10.0, parameterPerturbationMultipliers ); -// -// } + for( unsigned int estimationCase = 0; estimationCase < 3; estimationCase ++ ) + { + std::cout<<"ESTIMATION CASE *********************************** "< > oneWayDopplerModel; + std::vector< std::string > perturbingBodies; + perturbingBodies.push_back( "Earth" ); + if( estimationCase == 0 ) + { + oneWayDopplerModel = + observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( + std::make_shared< observation_models::OneWayDopplerObservationSettings >( + linkEnds, + std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( perturbingBodies ), + nullptr, nullptr, nullptr, + std::make_shared< LightTimeConvergenceCriteria >( ), + static_cast< bool >( normalizeObservable ) ), bodies ); + } + else + { + oneWayDopplerModel = + observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( + std::make_shared< OneWayDopplerObservationSettings > + ( linkEnds, std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( + perturbingBodies ), + std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Mars" ), + std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), + nullptr, std::make_shared< LightTimeConvergenceCriteria >( ), static_cast< bool >( normalizeObservable ) ), bodies ); + } + + // Create parameter objects. + std::shared_ptr< EstimatableParameterSet< double > > fullEstimatableParameterSet; + Eigen::VectorXd parameterPerturbationMultipliers = Eigen::Vector4d::Constant( 1.0 ); + if( estimationCase < 2 ) + { + fullEstimatableParameterSet = createEstimatableParameters( bodies, 1.1E7 ); + } + else + { + fullEstimatableParameterSet = createEstimatableParameters( bodies, 1.1E7, true ); + parameterPerturbationMultipliers( 2 ) = 1.0E-4; + } + + testObservationPartials< 1 >( + oneWayDopplerModel, bodies, fullEstimatableParameterSet, linkEnds, one_way_doppler, 1.0E-5, + true, true, 10.0, parameterPerturbationMultipliers ); + + } std::cout<<"B ****************************************** "< > oneWayDopplerModel; -// std::vector< std::string > perturbingBodies; -// perturbingBodies.push_back( "Earth" ); -// if( estimationCase == 0 ) -// { -// oneWayDopplerModel = -// observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( -// std::make_shared< observation_models::OneWayDopplerObservationSettings >( -// linkEnds, -// std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( perturbingBodies ), -// nullptr, nullptr, nullptr, -// std::make_shared< LightTimeConvergenceCriteria >( ), -// static_cast< bool >( normalizeObservable ) ), bodies ); -// } -// else -// { -// oneWayDopplerModel = -// observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( -// std::make_shared< OneWayDopplerObservationSettings > -// ( linkEnds, std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( -// perturbingBodies ), -// std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Mars" ), -// std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), -// nullptr, -// std::make_shared< LightTimeConvergenceCriteria >( ), -// static_cast< bool >( normalizeObservable ) ), bodies ); -// } -// // Create parameter objects. -// std::shared_ptr< EstimatableParameterSet< double > > fullEstimatableParameterSet; -// Eigen::VectorXd parameterPerturbationMultipliers = Eigen::Vector4d::Constant( 1.0 ); -// if( estimationCase < 2 ) -// { -// fullEstimatableParameterSet = createEstimatableParameters( bodies, 1.1E7 ); -// } -// else -// { -// fullEstimatableParameterSet = createEstimatableParameters( bodies, 1.1E7, true ); -// parameterPerturbationMultipliers( 2 ) = 1.0E-4; -// } -// -// testObservationPartials< 1 >( -// oneWayDopplerModel, bodies, fullEstimatableParameterSet, linkEnds, one_way_doppler, 1.0E-4, false, true, -// 1.0, parameterPerturbationMultipliers ); -// } -// } + { + // Create environment + SystemOfBodies bodies = setupEnvironment( groundStations, 1.0E7, 1.2E7, 1.1E7, false ); + + // Set link ends for observation model + LinkDefinition linkEnds; + linkEnds[ transmitter ] = groundStations[ 1 ]; + linkEnds[ receiver ] = groundStations[ 0 ]; + + for( unsigned int estimationCase = 0; estimationCase < 3; estimationCase ++ ) + { + std::cout<<"ESTIMATION CASE *********************************** "<<" "< > oneWayDopplerModel; + std::vector< std::string > perturbingBodies; + perturbingBodies.push_back( "Earth" ); + if( estimationCase == 0 ) + { + oneWayDopplerModel = + observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( + std::make_shared< observation_models::OneWayDopplerObservationSettings >( + linkEnds, + std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( perturbingBodies ), + nullptr, nullptr, nullptr, + std::make_shared< LightTimeConvergenceCriteria >( ), + static_cast< bool >( normalizeObservable ) ), bodies ); + } + else + { + oneWayDopplerModel = + observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( + std::make_shared< OneWayDopplerObservationSettings > + ( linkEnds, std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( + perturbingBodies ), + std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Mars" ), + std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), + nullptr, + std::make_shared< LightTimeConvergenceCriteria >( ), + static_cast< bool >( normalizeObservable ) ), bodies ); + } + // Create parameter objects. + std::shared_ptr< EstimatableParameterSet< double > > fullEstimatableParameterSet; + Eigen::VectorXd parameterPerturbationMultipliers = Eigen::Vector4d::Constant( 1.0 ); + if( estimationCase < 2 ) + { + fullEstimatableParameterSet = createEstimatableParameters( bodies, 1.1E7 ); + } + else + { + fullEstimatableParameterSet = createEstimatableParameters( bodies, 1.1E7, true ); + parameterPerturbationMultipliers( 2 ) = 1.0E-4; + } + + testObservationPartials< 1 >( + oneWayDopplerModel, bodies, fullEstimatableParameterSet, linkEnds, one_way_doppler, 1.0E-4, false, true, + 1.0, parameterPerturbationMultipliers ); + } + } std::cout<<"C ****************************************** "< > oneWayDopplerModelWithoutProperTime = -// std::dynamic_pointer_cast< OneWayDopplerObservationModel< > >( -// observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( -// std::make_shared< observation_models::OneWayDopplerObservationSettings >( -// linkEnds, nullptr, nullptr, nullptr, nullptr, -// std::make_shared< LightTimeConvergenceCriteria >( ), -// static_cast< bool >( normalizeObservable ) ), bodies ) ); -// -// // Create partials for Doppler without proper time rates -// std::pair< SingleLinkObservationPartialList, std::shared_ptr< PositionPartialScaling > > dopplerPartialsWithoutProperTime = -// ObservationPartialCreator<1, double, double>::createObservationPartials( -// oneWayDopplerModelWithoutProperTime, bodies, fullEstimatableParameterSet ); -// -// // Retrieve partial object without proper time -// std::shared_ptr< OneWayDopplerScaling > partialScalingObjectWithoutProperTime = -// std::dynamic_pointer_cast< OneWayDopplerScaling >( dopplerPartialsWithoutProperTime.second ); -// std::shared_ptr< DirectObservationPartial< 1 > > earthStatePartialWithoutProperTime = -// std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( -// ( dopplerPartialsWithoutProperTime.first ).begin( )->second ); -// std::shared_ptr< DirectObservationPartial< 1 > > marsStatePartialWithoutProperTime = -// std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( -// ( ++( ( dopplerPartialsWithoutProperTime.first ).begin( ) ) )->second ); -// -// // Compute nominal observation without proper time -// std::vector< double > linkEndTimesWithoutProperTime; -// std::vector< Eigen::Vector6d > linkEndStatesWithoutProperTime; -// Eigen::VectorXd nominalObservableWithoutProperTime = oneWayDopplerModelWithoutProperTime->computeIdealObservationsWithLinkEndData( -// observationTime, referenceLinkEnd, linkEndTimesWithoutProperTime, linkEndStatesWithoutProperTime ); -// -// std::cout<<"Nominal without proper time: "<update( -// linkEndStatesWithoutProperTime, linkEndTimesWithoutProperTime, -// referenceLinkEnd, nominalObservableWithoutProperTime ); -// std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > earthStatePartialOutputWithoutProperTime = -// earthStatePartialWithoutProperTime->calculatePartial( -// linkEndStates, linkEndTimes, referenceLinkEnd, {}, nominalObservable ); -// std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > marsStatePartialOutputWithoutProperTime = -// marsStatePartialWithoutProperTime->calculatePartial( -// linkEndStates, linkEndTimes, referenceLinkEnd, {}, nominalObservable ); -// -// Eigen::MatrixXd partialWrtEarthState = earthStatePartialOutput.at( 0 ).first; -// Eigen::MatrixXd partialWrtEarthStateWithoutProperTime = earthStatePartialOutputWithoutProperTime.at( 0 ).first; -// -// Eigen::MatrixXd partialWrtMarsState = marsStatePartialOutput.at( 0 ).first; -// Eigen::MatrixXd partialWrtMarsStateWithoutProperTime = marsStatePartialOutputWithoutProperTime.at( 0 ).first; -// -// Eigen::MatrixXd properTimePartialWrtMarsPosition = transmitterProperTimePartials->getPositionScalingFactor( transmitter ) * partialScalingTerm; -// Eigen::MatrixXd properTimePartialWrtEarthPosition = receiverProperTimePartials->getPositionScalingFactor( receiver ) * partialScalingTerm; -// -// Eigen::MatrixXd properTimePartialWrtMarsVelocity = transmitterProperTimePartials->getVelocityScalingFactor( transmitter ) * partialScalingTerm; -// Eigen::MatrixXd properTimePartialWrtEarthVelocity = receiverProperTimePartials->getVelocityScalingFactor( receiver ) * partialScalingTerm; -// -// std::cout<<"Partial w.r.t. Earth state "< > oneWayDopplerModelWithoutProperTime = + std::dynamic_pointer_cast< OneWayDopplerObservationModel< > >( + observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( + std::make_shared< observation_models::OneWayDopplerObservationSettings >( + linkEnds, nullptr, nullptr, nullptr, nullptr, + std::make_shared< LightTimeConvergenceCriteria >( ), + static_cast< bool >( normalizeObservable ) ), bodies ) ); + + // Create partials for Doppler without proper time rates + std::pair< SingleLinkObservationPartialList, std::shared_ptr< PositionPartialScaling > > dopplerPartialsWithoutProperTime = + ObservationPartialCreator<1, double, double>::createObservationPartials( + oneWayDopplerModelWithoutProperTime, bodies, fullEstimatableParameterSet ); + + // Retrieve partial object without proper time + std::shared_ptr< OneWayDopplerScaling > partialScalingObjectWithoutProperTime = + std::dynamic_pointer_cast< OneWayDopplerScaling >( dopplerPartialsWithoutProperTime.second ); + std::shared_ptr< DirectObservationPartial< 1 > > earthStatePartialWithoutProperTime = + std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( + ( dopplerPartialsWithoutProperTime.first ).begin( )->second ); + std::shared_ptr< DirectObservationPartial< 1 > > marsStatePartialWithoutProperTime = + std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( + ( ++( ( dopplerPartialsWithoutProperTime.first ).begin( ) ) )->second ); + + // Compute nominal observation without proper time + std::vector< double > linkEndTimesWithoutProperTime; + std::vector< Eigen::Vector6d > linkEndStatesWithoutProperTime; + Eigen::VectorXd nominalObservableWithoutProperTime = oneWayDopplerModelWithoutProperTime->computeIdealObservationsWithLinkEndData( + observationTime, referenceLinkEnd, linkEndTimesWithoutProperTime, linkEndStatesWithoutProperTime ); + + std::cout<<"Nominal without proper time: "<update( + linkEndStatesWithoutProperTime, linkEndTimesWithoutProperTime, + referenceLinkEnd, nominalObservableWithoutProperTime ); + std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > earthStatePartialOutputWithoutProperTime = + earthStatePartialWithoutProperTime->calculatePartial( + linkEndStates, linkEndTimes, referenceLinkEnd, {}, nominalObservable ); + std::vector< std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > > marsStatePartialOutputWithoutProperTime = + marsStatePartialWithoutProperTime->calculatePartial( + linkEndStates, linkEndTimes, referenceLinkEnd, {}, nominalObservable ); + + Eigen::MatrixXd partialWrtEarthState = earthStatePartialOutput.at( 0 ).first; + Eigen::MatrixXd partialWrtEarthStateWithoutProperTime = earthStatePartialOutputWithoutProperTime.at( 0 ).first; + + Eigen::MatrixXd partialWrtMarsState = marsStatePartialOutput.at( 0 ).first; + Eigen::MatrixXd partialWrtMarsStateWithoutProperTime = marsStatePartialOutputWithoutProperTime.at( 0 ).first; + + Eigen::MatrixXd properTimePartialWrtMarsPosition = transmitterProperTimePartials->getPositionScalingFactor( transmitter ) * partialScalingTerm; + Eigen::MatrixXd properTimePartialWrtEarthPosition = receiverProperTimePartials->getPositionScalingFactor( receiver ) * partialScalingTerm; + + Eigen::MatrixXd properTimePartialWrtMarsVelocity = transmitterProperTimePartials->getVelocityScalingFactor( transmitter ) * partialScalingTerm; + Eigen::MatrixXd properTimePartialWrtEarthVelocity = receiverProperTimePartials->getVelocityScalingFactor( receiver ) * partialScalingTerm; + + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ( ( partialWrtMarsState - partialWrtMarsStateWithoutProperTime ).block( 0, 0, 1, 3 ) ), + ( properTimePartialWrtMarsPosition + properTimePartialWrtEarthPosition ), 1.0E-5 ); + + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ( -( partialWrtEarthState - partialWrtEarthStateWithoutProperTime ).block( 0, 0, 1, 3 ) ), + ( properTimePartialWrtMarsPosition + properTimePartialWrtEarthPosition ), 1.0E-5 ); + + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ( ( partialWrtMarsState - partialWrtMarsStateWithoutProperTime ).block( 0, 3, 1, 3 ) ), + properTimePartialWrtMarsVelocity, 1.0E-5 ); + + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + ( -( partialWrtEarthState - partialWrtEarthStateWithoutProperTime ).block( 0, 3, 1, 3 ) ), + properTimePartialWrtEarthVelocity, 1.0E-5 ); } } From 97d09183c39a1c135a61baacf44234584ab70996 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Tue, 17 Dec 2024 16:17:25 +0100 Subject: [PATCH 12/24] Update to Doppler models --- .../astro/observation_models/lightTimeSolution.h | 14 +++++++------- .../createPositionPartialScaling.h | 5 +---- .../observation_partials/oneWayDopplerPartial.cpp | 3 ++- .../support/observationPartialTestFunctions.h | 10 ++++++++-- .../unitTestOneWayDopplerPartials.cpp | 4 ++-- 5 files changed, 20 insertions(+), 16 deletions(-) diff --git a/include/tudat/astro/observation_models/lightTimeSolution.h b/include/tudat/astro/observation_models/lightTimeSolution.h index fd8452cf74..39eae8737e 100644 --- a/include/tudat/astro/observation_models/lightTimeSolution.h +++ b/include/tudat/astro/observation_models/lightTimeSolution.h @@ -650,13 +650,13 @@ class LightTimeCalculator Eigen::Matrix< ObservationScalarType, 1, 3 > partialWrtLinkEndPosition = ( relativePosition.normalized( ) ).transpose( ) * ( isPartialWrtReceiver ? mathematical_constants::getFloatingInteger< ObservationScalarType >( 1 ) : mathematical_constants::getFloatingInteger< ObservationScalarType >( -1 ) ); -// for( unsigned int i = 0; i < correctionFunctions_.size( ); i++ ) -// { -// partialWrtLinkEndPosition += correctionFunctions_.at( i )->calculateLightTimeCorrectionPartialDerivativeWrtLinkEndPosition( -// transmitterState.template cast< double >( ), -// receiverState.template cast< double >( ), -// transmitterTime, receiverTime, isPartialWrtReceiver ? receiver : transmitter ).template cast< ObservationScalarType >( ); -// } + for( unsigned int i = 0; i < correctionFunctions_.size( ); i++ ) + { + partialWrtLinkEndPosition += correctionFunctions_.at( i )->calculateLightTimeCorrectionPartialDerivativeWrtLinkEndPosition( + transmitterState.template cast< double >( ), + receiverState.template cast< double >( ), + transmitterTime, receiverTime, isPartialWrtReceiver ? receiver : transmitter ).template cast< ObservationScalarType >( ); + } return partialWrtLinkEndPosition; } diff --git a/include/tudat/simulation/estimation_setup/createPositionPartialScaling.h b/include/tudat/simulation/estimation_setup/createPositionPartialScaling.h index 341b53c44e..ba809cbc63 100644 --- a/include/tudat/simulation/estimation_setup/createPositionPartialScaling.h +++ b/include/tudat/simulation/estimation_setup/createPositionPartialScaling.h @@ -49,12 +49,9 @@ inline std::shared_ptr< OneWayDopplerProperTimeComponentScaling > createDopplerP else if( std::dynamic_pointer_cast< observation_models::DirectFirstOrderDopplerProperTimeRateInterface >( dopplerProperTimeInterface ) != nullptr ) { - bool computeStatePartials = - !( std::dynamic_pointer_cast< observation_models::DirectFirstOrderDopplerProperTimeRateInterface >( - dopplerProperTimeInterface )->matchWithBody( oneWayDopplerLinkEnds.at( linkEndAtWhichPartialIsComputed ).bodyName_ ) ); properTimeRateDopplerPartial = std::make_shared< OneWayDopplerDirectFirstOrderProperTimeComponentScaling >( std::dynamic_pointer_cast< observation_models::DirectFirstOrderDopplerProperTimeRateInterface >( - dopplerProperTimeInterface ), linkEndAtWhichPartialIsComputed, oneWayDopplerLinkEnds, computeStatePartials ); + dopplerProperTimeInterface ), linkEndAtWhichPartialIsComputed, oneWayDopplerLinkEnds, true ); } else { diff --git a/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp b/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp index e790ff7a26..6e909f9bd9 100644 --- a/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp +++ b/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp @@ -51,7 +51,7 @@ OneWayDopplerDirectFirstOrderProperTimeComponentScaling::OneWayDopplerDirectFirs const bool computeStatePartials ): OneWayDopplerProperTimeComponentScaling( linkEndWithPartial ), properTimeRateModel_( properTimeRateModel ), - computeStatePartials_( computeStatePartials ) + computeStatePartials_( true ) { partialWrtPerturbedPositions_.resize( properTimeRateModel->getGravitationalParameters( ).size( ) ); if( linkEndWithPartial_ == observation_models::transmitter ) @@ -122,6 +122,7 @@ void OneWayDopplerDirectFirstOrderProperTimeComponentScaling::update( const std: Eigen::Matrix< double, 1, 3 > OneWayDopplerDirectFirstOrderProperTimeComponentScaling::getPositionScalingFactor( const observation_models::LinkEndType linkEndType ) { +// std::cout<<"In proper time partials "<getComputationPointLinkEndType( )<<" "<getComputationPointLinkEndType( )) diff --git a/tests/include/tudat/support/observationPartialTestFunctions.h b/tests/include/tudat/support/observationPartialTestFunctions.h index 6262d3d550..dd5bd8cd71 100644 --- a/tests/include/tudat/support/observationPartialTestFunctions.h +++ b/tests/include/tudat/support/observationPartialTestFunctions.h @@ -144,7 +144,8 @@ void testObservationPartials( const double positionPerturbationMultiplier = 1.0, const Eigen::VectorXd parameterPerturbationMultipliers = Eigen::VectorXd::Constant( 4, 1.0 ), const std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > ancilliarySettings = nullptr, - double observationTime = 1.1E7 ) + double observationTime = 1.1E7, + const double gammaToleranceWeakening = 1.0 ) { printEstimatableParameterEntries( fullEstimatableParameterSet ); @@ -429,8 +430,13 @@ void testObservationPartials( std::cout<<"Current double partial "<( oneWayDopplerModel, bodies, fullEstimatableParameterSet, linkEnds, one_way_doppler, 1.0E-5, - true, true, 10.0, parameterPerturbationMultipliers ); + true, true, 10.0, parameterPerturbationMultipliers, nullptr, 1.1E7, estimationCase == 2 ? 1.0 : 2000.0 ); } @@ -328,7 +328,7 @@ BOOST_AUTO_TEST_CASE( testOneWayDopplerPartials ) testObservationPartials< 1 >( oneWayDopplerModel, bodies, fullEstimatableParameterSet, linkEnds, one_way_doppler, 1.0E-4, false, true, - 1.0, parameterPerturbationMultipliers ); + 1.0, parameterPerturbationMultipliers, nullptr, 1.1E7, estimationCase == 2 ? 1.0 : 2000.0 ); } } From 4bee5a24b9878b2337b7413147292f806d85fe83 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Tue, 17 Dec 2024 16:21:24 +0100 Subject: [PATCH 13/24] Doppler partials working; light time derivative partial not yet implemented --- .../observation_partials/unitTestTwoWayDopplerPartials.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/src/astro/orbit_determination/observation_partials/unitTestTwoWayDopplerPartials.cpp b/tests/src/astro/orbit_determination/observation_partials/unitTestTwoWayDopplerPartials.cpp index ce40e7c05e..9ac0d3dc66 100644 --- a/tests/src/astro/orbit_determination/observation_partials/unitTestTwoWayDopplerPartials.cpp +++ b/tests/src/astro/orbit_determination/observation_partials/unitTestTwoWayDopplerPartials.cpp @@ -149,7 +149,7 @@ BOOST_AUTO_TEST_CASE( testTwoWayDopplerPartials ) testObservationPartials< 1 >( twoWayDopplerModel, bodies, fullEstimatableParameterSet, linkEnds, two_way_doppler, 1.0E-5, - true, true, 10.0, parameterPerturbationMultipliers ); + true, true, 10.0, parameterPerturbationMultipliers, nullptr, 1.1E7, 100.0 ); } } @@ -209,7 +209,7 @@ BOOST_AUTO_TEST_CASE( testTwoWayDopplerPartials ) testObservationPartials< 1 >( twoWayDopplerModel, bodies, fullEstimatableParameterSet, linkEnds, two_way_doppler, 1.0E-4, false, true, - 1.0, parameterPerturbationMultipliers ); + 1.0, parameterPerturbationMultipliers, nullptr, 1.1E7, 100.0 ); } } } From cd4b3f3726eae75e41c3a0b3ec312657549416ae Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Wed, 18 Dec 2024 11:02:22 +0100 Subject: [PATCH 14/24] Corrected light time correction gradient effects in Doppler models --- .../observation_models/lightTimeSolution.h | 6 +- .../firstOrderRelativisticCorrection.cpp | 15 +- .../relativisticLightTimeCorrection.cpp | 5 +- .../unitTestDopplerModels.cpp | 169 +++++++++++------- .../relativity/unitTestShapiroTimeDelay.cpp | 75 +++++++- 5 files changed, 190 insertions(+), 80 deletions(-) diff --git a/include/tudat/astro/observation_models/lightTimeSolution.h b/include/tudat/astro/observation_models/lightTimeSolution.h index 39eae8737e..e3e1a8da0f 100644 --- a/include/tudat/astro/observation_models/lightTimeSolution.h +++ b/include/tudat/astro/observation_models/lightTimeSolution.h @@ -650,12 +650,14 @@ class LightTimeCalculator Eigen::Matrix< ObservationScalarType, 1, 3 > partialWrtLinkEndPosition = ( relativePosition.normalized( ) ).transpose( ) * ( isPartialWrtReceiver ? mathematical_constants::getFloatingInteger< ObservationScalarType >( 1 ) : mathematical_constants::getFloatingInteger< ObservationScalarType >( -1 ) ); + for( unsigned int i = 0; i < correctionFunctions_.size( ); i++ ) { - partialWrtLinkEndPosition += correctionFunctions_.at( i )->calculateLightTimeCorrectionPartialDerivativeWrtLinkEndPosition( + partialWrtLinkEndPosition += + correctionFunctions_.at( i )->calculateLightTimeCorrectionPartialDerivativeWrtLinkEndPosition( transmitterState.template cast< double >( ), receiverState.template cast< double >( ), - transmitterTime, receiverTime, isPartialWrtReceiver ? receiver : transmitter ).template cast< ObservationScalarType >( ); + transmitterTime, receiverTime, isPartialWrtReceiver ? receiver : transmitter ).template cast< ObservationScalarType >( ) * physical_constants::SPEED_OF_LIGHT; } return partialWrtLinkEndPosition; } diff --git a/src/astro/observation_models/corrections/firstOrderRelativisticCorrection.cpp b/src/astro/observation_models/corrections/firstOrderRelativisticCorrection.cpp index ffc72b0fcb..4a7f87c7fa 100644 --- a/src/astro/observation_models/corrections/firstOrderRelativisticCorrection.cpp +++ b/src/astro/observation_models/corrections/firstOrderRelativisticCorrection.cpp @@ -46,12 +46,12 @@ double FirstOrderLightTimeCorrectionCalculator::calculateLightTimeCorrectionWith { evaluationTime = transmissionTime + lightTimeEvaluationContribution_.at( i ) * ( receptionTime - transmissionTime ); // Calculate correction due to current body and add to total. - currentLighTimeCorrectionComponents_[ i ] = relativity::calculateFirstOrderLightTimeCorrectionFromCentralBody( - perturbingBodyGravitationalParameterFunctions_[ i ]( ), + currentLighTimeCorrectionComponents_.at( i ) = relativity::calculateFirstOrderLightTimeCorrectionFromCentralBody( + perturbingBodyGravitationalParameterFunctions_.at( i )( ), transmitterState.segment( 0, 3 ), receiverState.segment( 0, 3 ), - perturbingBodyStateFunctions_[ i ]( evaluationTime ).segment( 0, 3 ), + perturbingBodyStateFunctions_.at( i )( evaluationTime ).segment( 0, 3 ), ppnParameterGamma ); - currentTotalLightTimeCorrection_ += currentLighTimeCorrectionComponents_[ i ]; + currentTotalLightTimeCorrection_ += currentLighTimeCorrectionComponents_.at( i ); } return currentTotalLightTimeCorrection_; @@ -79,14 +79,13 @@ calculateLightTimeCorrectionPartialDerivativeWrtLinkEndPosition( for( unsigned int i = 0; i < perturbingBodyStateFunctions_.size( ); i++ ) { evaluationTime = transmissionTime + lightTimeEvaluationContribution_.at( i ) * ( receptionTime - transmissionTime ); - - perturbingBodyState = perturbingBodyStateFunctions_[ i ]( evaluationTime ); + perturbingBodyState = perturbingBodyStateFunctions_.at( i )( evaluationTime ); // Calculate correction due to current body and add to total. currentTotalLightTimeCorrectionPartial_ += relativity::calculateFirstOrderCentralBodyLightTimeCorrectionGradient( - perturbingBodyGravitationalParameterFunctions_[ i ]( ), + perturbingBodyGravitationalParameterFunctions_.at( i )( ), transmitterState.segment( 0, 3 ), receiverState.segment( 0, 3 ), - perturbingBodyStateFunctions_[ i ]( evaluationTime ).segment( 0, 3 ), + perturbingBodyStateFunctions_.at( i )( evaluationTime ).segment( 0, 3 ), ( linkEndAtWhichPartialIsEvaluated == receiver ), ppnParameterGamma ); } diff --git a/src/astro/relativity/relativisticLightTimeCorrection.cpp b/src/astro/relativity/relativisticLightTimeCorrection.cpp index a4e8550ece..5fbf5c4d58 100644 --- a/src/astro/relativity/relativisticLightTimeCorrection.cpp +++ b/src/astro/relativity/relativisticLightTimeCorrection.cpp @@ -53,11 +53,12 @@ Eigen::Matrix< double, 1, 3 > calculateFirstOrderCentralBodyLightTimeCorrectionG ( relativePositionVector.normalized( ) ).transpose( ); if( evaluateGradientAtReceiver ) { - gradient -= relativePositionVector.norm( ) * ( receiverPosition.normalized( ) ).transpose( ); + gradient -= relativePositionVector.norm( ) * ( ( receiverPosition - centralBodyPosition ).normalized( ) ).transpose( ); } else { - gradient += relativePositionVector.norm( ) * ( transmitterPosition.normalized( ) ).transpose( );\ + gradient += relativePositionVector.norm( ) * ( ( transmitterPosition - centralBodyPosition ).normalized( ) ).transpose( ); + gradient *= -1.0; } return 2.0 * ( 1.0 + ppnParameterGamma ) * bodyGravitationalParameter * physical_constants::INVERSE_CUBIC_SPEED_OF_LIGHT * gradient / diff --git a/tests/src/astro/observation_models/unitTestDopplerModels.cpp b/tests/src/astro/observation_models/unitTestDopplerModels.cpp index 85f6f3fc22..92427753c9 100644 --- a/tests/src/astro/observation_models/unitTestDopplerModels.cpp +++ b/tests/src/astro/observation_models/unitTestDopplerModels.cpp @@ -68,6 +68,7 @@ BOOST_AUTO_TEST_CASE( testOneWayDoppplerModel ) BodyListSettings defaultBodySettings = getDefaultBodySettings( bodiesToCreate, "SSB", "ECLIPJ2000" ); + defaultBodySettings.get( "Sun" )->gravityFieldSettings = centralGravitySettings( spice_interface::getBodyGravitationalParameter( "Sun") * 10000.0 ); // Create bodies SystemOfBodies bodies = createSystemOfBodies( defaultBodySettings ); @@ -101,86 +102,126 @@ BOOST_AUTO_TEST_CASE( testOneWayDoppplerModel ) linkEnds[ receiver ] = std::make_pair< std::string, std::string >( "Mars" , "" ); // Create observation settings - std::shared_ptr< ObservationModelSettings > observableSettings = std::make_shared< ObservationModelSettings > - ( one_way_doppler, linkEnds ); - - // Create observation model. - std::shared_ptr< ObservationModel< 1, double, double> > observationModel = - ObservationModelCreator< 1, double, double>::createObservationModel( - observableSettings, bodies ); - - std::shared_ptr< OneWayDopplerObservationModel< double, double> > dopplerObservationModel = - std::dynamic_pointer_cast< OneWayDopplerObservationModel< double, double> >( observationModel ); - - // Test observable for both fixed link ends - for( unsigned testCase = 0; testCase < 4; testCase++ ) + for( int useCorrections = 0; useCorrections < 2; useCorrections++ ) { - - double observationTime = ( finalEphemerisTime + initialEphemerisTime ) / 2.0; - std::vector< double > linkEndTimes; - std::vector< Eigen::Vector6d > linkEndStates; - - // Define link end - LinkEndType referenceLinkEnd; - if( testCase == 0 || testCase == 2 ) - { - referenceLinkEnd = transmitter; - } - else + double toleranceScaling = 1.0; + std::vector< std::shared_ptr< LightTimeCorrectionSettings > > correctionSettings; + if( useCorrections == 1 ) { - referenceLinkEnd = receiver; + correctionSettings.push_back( std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( std::vector< std::string >( { "Sun" } ) ) ); + toleranceScaling *= 100.0; } + std::shared_ptr observableSettings = std::make_shared + ( one_way_doppler, linkEnds, correctionSettings ); - double scalingTerm = physical_constants::SPEED_OF_LIGHT; - bool useNormalization = false; - if( testCase > 1 ) - { - scalingTerm = 1.0; - useNormalization = true; - dopplerObservationModel->setNormalizeWithSpeedOfLight( useNormalization ); - } - - // Compute observable - double dopplerObservable = observationModel->computeObservationsWithLinkEndData( - observationTime, referenceLinkEnd, linkEndTimes, linkEndStates )( 0 ); + // Create observation model. + std::shared_ptr > observationModel = + ObservationModelCreator<1, double, double>::createObservationModel( + observableSettings, bodies ); - // Creare independent light time calculator object - std::shared_ptr< LightTimeCalculator< double, double > > lightTimeCalculator = - createLightTimeCalculator( linkEnds, transmitter, receiver, bodies ); - Eigen::Vector6d transmitterState, receiverState; - // Compute light time - double lightTime = lightTimeCalculator->calculateLightTimeWithLinkEndsStates( - receiverState, transmitterState, observationTime, ( !( testCase == 0 || testCase == 2 ) ) ); + std::shared_ptr > dopplerObservationModel = + std::dynamic_pointer_cast >( observationModel ); - // Compare light time calculator link end conditions with observation model + // Test observable for both fixed link ends + for ( unsigned testCase = 2; testCase < 4; testCase++ ) { - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( receiverState, linkEndStates.at( 1 ), 1.0E-15 ); - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( transmitterState, linkEndStates.at( 0 ), 1.0E-15 ); - - if( testCase == 0 || testCase == 2 ) + std::cout< linkEndTimes; + std::vector linkEndStates; + + // Define link end + LinkEndType referenceLinkEnd; + if ( testCase == 0 || testCase == 2 ) { - BOOST_CHECK_SMALL( std::fabs( observationTime - linkEndTimes.at( 0 ) ), 1.0E-12 ); - BOOST_CHECK_SMALL( std::fabs( observationTime + lightTime - linkEndTimes.at( 1 ) ), 1.0E-10 ); + referenceLinkEnd = transmitter; } else { - BOOST_CHECK_SMALL( std::fabs( observationTime - linkEndTimes.at( 1 ) ), 1.0E-12 ); - BOOST_CHECK_SMALL( std::fabs( observationTime - lightTime - linkEndTimes.at( 0 ) ), 1.0E-10 ); + referenceLinkEnd = receiver; } - } - // Compute numerical partial derivative of light time. - double timePerturbation = 100.0; - double upPerturbedLightTime = lightTimeCalculator->calculateLightTime( linkEndTimes.at( 1 ) + timePerturbation, true ); - double downPerturbedLightTime = lightTimeCalculator->calculateLightTime( linkEndTimes.at( 1 ) - timePerturbation, true ); + double scalingTerm = physical_constants::SPEED_OF_LIGHT; + bool useNormalization = false; + if ( testCase > 1 ) + { + scalingTerm = 1.0; + useNormalization = true; + dopplerObservationModel->setNormalizeWithSpeedOfLight( useNormalization ); + } - double lightTimeSensitivity = -( upPerturbedLightTime - downPerturbedLightTime ) / ( 2.0 * timePerturbation ); + // Compute observable + double dopplerObservable = observationModel->computeObservationsWithLinkEndData( + observationTime, referenceLinkEnd, linkEndTimes, linkEndStates )( 0 ); - // Test numerical derivative against Doppler observable - BOOST_CHECK_SMALL( std::fabs( scalingTerm * lightTimeSensitivity - dopplerObservable ), 2.0E-14 * scalingTerm ); - } + // Creare independent light time calculator object + std::shared_ptr > lightTimeCalculator = + createLightTimeCalculator( linkEnds, transmitter, receiver, bodies, undefined_observation_model, correctionSettings ); + Eigen::Vector6d transmitterState, receiverState; + // Compute light time + double lightTime = lightTimeCalculator->calculateLightTimeWithLinkEndsStates( + receiverState, transmitterState, observationTime, ( !( testCase == 0 || testCase == 2 ))); - dopplerObservationModel->setNormalizeWithSpeedOfLight( false ); + // Compare light time calculator link end conditions with observation model + { + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( receiverState, linkEndStates.at( 1 ), 1.0E-15 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( transmitterState, linkEndStates.at( 0 ), 1.0E-15 ); + + if ( testCase == 0 || testCase == 2 ) + { + BOOST_CHECK_SMALL( std::fabs( observationTime - linkEndTimes.at( 0 )), 1.0E-12 ); + BOOST_CHECK_SMALL( std::fabs( observationTime + lightTime - linkEndTimes.at( 1 )), 1.0E-10 ); + } + else + { + BOOST_CHECK_SMALL( std::fabs( observationTime - linkEndTimes.at( 1 )), 1.0E-12 ); + BOOST_CHECK_SMALL( std::fabs( observationTime - lightTime - linkEndTimes.at( 0 )), 1.0E-10 ); + } + } + + // Compute numerical partial derivative of light time. + double timePerturbation = 100.0; + double upPerturbedLightTime = + lightTimeCalculator->calculateLightTime( linkEndTimes.at( 1 ) + timePerturbation, true ); + double downPerturbedLightTime = + lightTimeCalculator->calculateLightTime( linkEndTimes.at( 1 ) - timePerturbation, true ); + + double lightTimeSensitivity = -( upPerturbedLightTime - downPerturbedLightTime ) / ( 2.0 * timePerturbation ); + + // Test numerical derivative against Doppler observable + BOOST_CHECK_CLOSE_FRACTION( scalingTerm * lightTimeSensitivity, dopplerObservable, 1.0E-8 * toleranceScaling ); + + if( useCorrections && testCase == 3 ) + { + std::shared_ptr< LightTimeCorrection > correction = lightTimeCalculator->getLightTimeCorrection( ).at( 0 ); + double nominalLightTimeCorrection = correction->calculateLightTimeCorrection( + transmitterState, receiverState, linkEndTimes.at( 0 ), linkEndTimes.at( 1 ) ); + + Eigen::Vector6d transmitterStateUp, receiverStateUp; + // Compute light time + double lightTimeUp = lightTimeCalculator->calculateLightTimeWithLinkEndsStates( + receiverStateUp, transmitterStateUp, observationTime + timePerturbation, true ); + double lightTimeCorrectionUp = correction->calculateLightTimeCorrection( + transmitterStateUp, receiverStateUp, observationTime + timePerturbation - lightTimeUp, observationTime + timePerturbation ); + + Eigen::Vector6d transmitterStateDown, receiverStateDown; + // Compute light time + double lightTimeDown = lightTimeCalculator->calculateLightTimeWithLinkEndsStates( + receiverStateDown, transmitterStateDown, observationTime - timePerturbation, true ); + double lightTimeCorrectionDown = correction->calculateLightTimeCorrection( + transmitterStateDown, receiverStateDown, observationTime - timePerturbation - lightTimeUp, observationTime - timePerturbation ); + + Eigen::Matrix< double, 3, 1 > lightTimeCorrectionWrtReceiver = correction->calculateLightTimeCorrectionPartialDerivativeWrtLinkEndPosition( + transmitterState, receiverState, linkEndTimes.at( 0 ), linkEndTimes.at( 1 ), receiver ); + Eigen::Matrix< double, 3, 1 > lightTimeCorrectionWrtTransmitter = correction->calculateLightTimeCorrectionPartialDerivativeWrtLinkEndPosition( + transmitterState, receiverState, linkEndTimes.at( 0 ), linkEndTimes.at( 1 ), transmitter ); + + BOOST_CHECK_CLOSE_FRACTION( lightTimeCorrectionWrtReceiver.dot( receiverState.segment( 3, 3 ) ) + lightTimeCorrectionWrtTransmitter.dot( transmitterState.segment( 3, 3 ) ), + ( lightTimeCorrectionUp - lightTimeCorrectionDown ) / ( 2.0 * timePerturbation ), 1.0E-3 ); + + } + } + } // Test observation biases { diff --git a/tests/src/astro/relativity/unitTestShapiroTimeDelay.cpp b/tests/src/astro/relativity/unitTestShapiroTimeDelay.cpp index afcef1212c..beb195ee63 100644 --- a/tests/src/astro/relativity/unitTestShapiroTimeDelay.cpp +++ b/tests/src/astro/relativity/unitTestShapiroTimeDelay.cpp @@ -14,10 +14,7 @@ #include - - - - +#include "tudat/basics/testMacros.h" #include "tudat/astro/relativity/relativisticLightTimeCorrection.h" #include "tudat/astro/ephemerides/constantEphemeris.h" #include "tudat/astro/observation_models/corrections/firstOrderRelativisticCorrection.h" @@ -71,6 +68,76 @@ BOOST_AUTO_TEST_CASE( testShapiroDelay ) BOOST_CHECK_CLOSE_FRACTION( 0.5 * directCalculation * physical_constants::SPEED_OF_LIGHT, expectedResult, 6.0E-2 ); } +BOOST_AUTO_TEST_CASE( testShapiroDelayGradient ) +{ + Eigen::Vector6d groundStationState; + groundStationState << -513.0E3, 120E3, 6378.0E3, 0.0, 0.0, 0.0; + Eigen::Vector6d satelliteState; + satelliteState << 324.0E3, 1434.0E3, 26600.0E3, 0.0, 0.0, 0.0; + Eigen::Vector6d centralBodyPosition; + centralBodyPosition << -80.0E3, 120.0E3, 1.0E3, 0.0, 0.0, 0.0; + + double earthGravitationalParameter = 398600.44189E15; + + double directCalculation = calculateFirstOrderLightTimeCorrectionFromCentralBody( + earthGravitationalParameter, groundStationState.segment( 0, 3 ), + satelliteState.segment( 0, 3 ), centralBodyPosition.segment( 0, 3 ) ); + + double positionPerturbation = 1.0E2; + + Eigen::Matrix< double, 1, 3 > numericalLightTimePartialWrtReceiver = Eigen::Matrix< double, 1, 3 >::Zero( ); + Eigen::Matrix< double, 1, 3 > numericalLightTimePartialWrtTransmitter = Eigen::Matrix< double, 1, 3 >::Zero( ); + + Eigen::Vector6d perturbedSatelliteState; + perturbedSatelliteState.setZero( ); + + Eigen::Vector6d perturbedStationState; + perturbedStationState.setZero( ); + for( int i = 0; i < 3; i++ ) + { + perturbedSatelliteState = satelliteState; + perturbedSatelliteState( i ) += positionPerturbation; + double upperturbedLightTime = calculateFirstOrderLightTimeCorrectionFromCentralBody( + earthGravitationalParameter, groundStationState.segment( 0, 3 ), + perturbedSatelliteState.segment( 0, 3 ), centralBodyPosition.segment( 0, 3 ) ); + + perturbedSatelliteState = satelliteState; + perturbedSatelliteState( i ) -= positionPerturbation; + double downperturbedLightTime = calculateFirstOrderLightTimeCorrectionFromCentralBody( + earthGravitationalParameter, groundStationState.segment( 0, 3 ), + perturbedSatelliteState.segment( 0, 3 ), centralBodyPosition.segment( 0, 3 ) ); + + numericalLightTimePartialWrtReceiver( i ) = ( upperturbedLightTime - downperturbedLightTime ) / ( 2.0 * positionPerturbation ); + + perturbedStationState = groundStationState; + perturbedStationState( i ) += positionPerturbation; + upperturbedLightTime = calculateFirstOrderLightTimeCorrectionFromCentralBody( + earthGravitationalParameter, perturbedStationState.segment( 0, 3 ), + satelliteState.segment( 0, 3 ), centralBodyPosition.segment( 0, 3 ) ); + + perturbedStationState = groundStationState; + perturbedStationState( i ) -= positionPerturbation; + downperturbedLightTime = calculateFirstOrderLightTimeCorrectionFromCentralBody( + earthGravitationalParameter, perturbedStationState.segment( 0, 3 ), + satelliteState.segment( 0, 3 ), centralBodyPosition.segment( 0, 3 ) ); + + numericalLightTimePartialWrtTransmitter( i ) = ( upperturbedLightTime - downperturbedLightTime ) / ( 2.0 * positionPerturbation ); + } + Eigen::Matrix< double, 1, 3 > analyticalLightTimePartialWrtReceiver = calculateFirstOrderCentralBodyLightTimeCorrectionGradient( + earthGravitationalParameter, groundStationState.segment( 0, 3 ), + satelliteState.segment( 0, 3 ), centralBodyPosition.segment( 0, 3 ), true ); + + Eigen::Matrix< double, 1, 3 > analyticalLightTimePartialWrtTransmitter = calculateFirstOrderCentralBodyLightTimeCorrectionGradient( + earthGravitationalParameter, groundStationState.segment( 0, 3 ), + satelliteState.segment( 0, 3 ), centralBodyPosition.segment( 0, 3 ), false ); + + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( analyticalLightTimePartialWrtReceiver, numericalLightTimePartialWrtReceiver, 1.0E-7 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( analyticalLightTimePartialWrtTransmitter, numericalLightTimePartialWrtTransmitter, 1.0E-7 ); + +} + + + BOOST_AUTO_TEST_SUITE_END( ) } From 3cb6769b3e72848e67a3f8d5503c8dba73b85a1b Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Wed, 18 Dec 2024 11:34:02 +0100 Subject: [PATCH 15/24] Warning suppressions --- .../observation_partials/directObservationPartial.h | 4 ++++ src/astro/electromagnetism/radiationSourceModel.cpp | 2 +- .../unitTestObservedFrequencyPartials.cpp | 2 +- .../propagators/unitTestMultiArcVariationalEquations.cpp | 2 +- .../propagators/unitTestRotationalDynamicsPropagator.cpp | 6 +++--- .../src/astro/propagators/unitTestVariationalEquations.cpp | 2 +- 6 files changed, 11 insertions(+), 7 deletions(-) diff --git a/include/tudat/astro/orbit_determination/observation_partials/directObservationPartial.h b/include/tudat/astro/orbit_determination/observation_partials/directObservationPartial.h index f56d20b423..dc8996ec62 100644 --- a/include/tudat/astro/orbit_determination/observation_partials/directObservationPartial.h +++ b/include/tudat/astro/orbit_determination/observation_partials/directObservationPartial.h @@ -226,6 +226,10 @@ class DirectObservationPartial: public ObservationPartial< ObservationSize > const std::vector< Eigen::Vector6d >&, const std::vector< double >& ) > > lighTimeCorrectionPartialsFunctions_; + std::vector< std::function< SingleLightTimePartialReturnType( + const std::vector< Eigen::Vector6d >&, const std::vector< double >& ) > > + lighTimeCorrectionGradientPartialsFunctions_; + //! List of light-time correction partial objects. std::vector< std::shared_ptr< observation_partials::LightTimeCorrectionPartial > > lighTimeCorrectionPartials_; diff --git a/src/astro/electromagnetism/radiationSourceModel.cpp b/src/astro/electromagnetism/radiationSourceModel.cpp index 11903c1268..c35a43e13c 100644 --- a/src/astro/electromagnetism/radiationSourceModel.cpp +++ b/src/astro/electromagnetism/radiationSourceModel.cpp @@ -299,7 +299,7 @@ void SourcePanelRadiosityModelUpdater::updateMembers(const double currentTime) Eigen::Vector3d originalSourceToSourceDirectionInSourceFrame = sourceRotationFromGlobalToLocalFrame - * -(originalSourceCenterPositionInGlobalFrame - sourceCenterPositionInGlobalFrame).normalized(); + * ( -(originalSourceCenterPositionInGlobalFrame - sourceCenterPositionInGlobalFrame).normalized() ); originalSourceToSourceCenterDirections_[originalSourceName] = originalSourceToSourceDirectionInSourceFrame; originalSourceToSourceOccultationModels_[originalSourceName]->updateMembers(currentTime); diff --git a/tests/src/astro/orbit_determination/observation_partials/unitTestObservedFrequencyPartials.cpp b/tests/src/astro/orbit_determination/observation_partials/unitTestObservedFrequencyPartials.cpp index 59b8d3de2b..84a7e6cfc2 100644 --- a/tests/src/astro/orbit_determination/observation_partials/unitTestObservedFrequencyPartials.cpp +++ b/tests/src/astro/orbit_determination/observation_partials/unitTestObservedFrequencyPartials.cpp @@ -86,8 +86,8 @@ class LinearStateWrapper referenceState_ = state; } - double referenceTime_; Eigen::Vector6d referenceState_; + double referenceTime_; }; void testPartials( diff --git a/tests/src/astro/propagators/unitTestMultiArcVariationalEquations.cpp b/tests/src/astro/propagators/unitTestMultiArcVariationalEquations.cpp index 1d2183dbf9..97ddae79dd 100644 --- a/tests/src/astro/propagators/unitTestMultiArcVariationalEquations.cpp +++ b/tests/src/astro/propagators/unitTestMultiArcVariationalEquations.cpp @@ -169,7 +169,7 @@ executeMultiArcEarthMoonSimulation( } // Create propagator settings - TranslationalPropagatorType propagatorType; + TranslationalPropagatorType propagatorType = undefined_translational_propagator; if( propagationType == 0 ) { propagatorType = cowell; diff --git a/tests/src/astro/propagators/unitTestRotationalDynamicsPropagator.cpp b/tests/src/astro/propagators/unitTestRotationalDynamicsPropagator.cpp index 911e940fd1..3114111acf 100755 --- a/tests/src/astro/propagators/unitTestRotationalDynamicsPropagator.cpp +++ b/tests/src/astro/propagators/unitTestRotationalDynamicsPropagator.cpp @@ -1142,9 +1142,9 @@ BOOST_AUTO_TEST_CASE( testSimpleRotationalDynamicsPropagationWithVaryinInertiaTe auto stateHistory = dynamicsSimulator.getEquationsOfMotionNumericalSolution( ); auto dependentVariableHistory = dynamicsSimulator.getDependentVariableHistory( ); - Eigen::Matrix3d nominalInertiaTensor; - Eigen::Vector3d nominalCosineCorrection; - Eigen::Vector2d nominalSineCorrection; + Eigen::Matrix3d nominalInertiaTensor = Eigen::Matrix3d::Zero( ); + Eigen::Vector3d nominalCosineCorrection = Eigen::Matrix3d::Zero( ); + Eigen::Vector2d nominalSineCorrection = Eigen::Matrix3d::Zero( ); std::shared_ptr< gravitation::SphericalHarmonicsGravityField > phobosGravityField = std::dynamic_pointer_cast< gravitation::SphericalHarmonicsGravityField >( diff --git a/tests/src/astro/propagators/unitTestVariationalEquations.cpp b/tests/src/astro/propagators/unitTestVariationalEquations.cpp index 389bbe668d..0bbe28e033 100644 --- a/tests/src/astro/propagators/unitTestVariationalEquations.cpp +++ b/tests/src/astro/propagators/unitTestVariationalEquations.cpp @@ -138,7 +138,7 @@ executeEarthMoonSimulation( // Create propagator settings std::shared_ptr< TranslationalStatePropagatorSettings< StateScalarType, TimeType > > propagatorSettings; - TranslationalPropagatorType propagatorType; + TranslationalPropagatorType propagatorType = undefined_translational_propagator ; if( propagationType == 0 ) { propagatorType = cowell; From 48a001f867e5b3f00304ff50476add27d6463687 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Wed, 18 Dec 2024 13:30:59 +0100 Subject: [PATCH 16/24] Progress on light time gradient partials --- .../directObservationPartial.h | 40 ++++++++++++++-- .../firstOrderRelativisticPartial.h | 9 ++++ .../lightTimeCorrectionPartial.h | 8 ++++ .../observation_partials/observationPartial.h | 13 ++++++ .../oneWayDopplerPartial.h | 32 +++++++++++++ .../lightTimeCorrectionPartial.cpp | 46 +++++++++++++++++++ .../oneWayDopplerPartial.cpp | 46 +++++++++---------- .../twoWayDopplerPartial.cpp | 17 ++++++- .../unitTestOneWayDopplerPartials.cpp | 4 +- .../unitTestRotationalDynamicsPropagator.cpp | 4 +- 10 files changed, 188 insertions(+), 31 deletions(-) diff --git a/include/tudat/astro/orbit_determination/observation_partials/directObservationPartial.h b/include/tudat/astro/orbit_determination/observation_partials/directObservationPartial.h index dc8996ec62..21a868af14 100644 --- a/include/tudat/astro/orbit_determination/observation_partials/directObservationPartial.h +++ b/include/tudat/astro/orbit_determination/observation_partials/directObservationPartial.h @@ -28,6 +28,7 @@ class DirectObservationPartial: public ObservationPartial< ObservationSize > typedef std::vector< std::pair< Eigen::Matrix< double, ObservationSize, Eigen::Dynamic >, double > > ObservationPartialReturnType; typedef std::pair< Eigen::Matrix< double, ObservationSize, Eigen::Dynamic >, double > SingleObservationPartialReturnType; typedef std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > SingleLightTimePartialReturnType; + typedef std::pair< Eigen::Matrix< double, 3, Eigen::Dynamic >, double > SingleLightTimeGradientPartialReturnType; //! Constructor /*! @@ -51,6 +52,8 @@ class DirectObservationPartial: public ObservationPartial< ObservationSize > std::pair< std::function< SingleLightTimePartialReturnType( const std::vector< Eigen::Vector6d >&, const std::vector< double >& ) >, bool > lightTimeCorrectionPartial; + std::pair< std::function< SingleLightTimeGradientPartialReturnType( + const std::vector< Eigen::Vector6d >&, const std::vector< double >&, const observation_models::LinkEndType ) >, bool > lightTimeCorrectionGradientPartial; // Create light time correction partial functions for( unsigned int i = 0; i < lighTimeCorrectionPartials.size( ); i++ ) @@ -61,7 +64,17 @@ class DirectObservationPartial: public ObservationPartial< ObservationSize > { lighTimeCorrectionPartialsFunctions_.push_back( lightTimeCorrectionPartial.first ); } - } + + if( positionPartialScaler->useLightTimeGradientPartials( ) ) + { + lightTimeCorrectionGradientPartial = getLightTimeGradientParameterPartialFunction( + parameterIdentifier, lighTimeCorrectionPartials.at( i ) ); + if( lightTimeCorrectionGradientPartial.second != 0 ) + { + lighTimeCorrectionGradientPartialsFunctions_.push_back( lightTimeCorrectionGradientPartial.first ); + } + } + } } //! Destructor. @@ -161,6 +174,25 @@ class DirectObservationPartial: public ObservationPartial< ObservationSize > currentLinkTimeCorrectionPartial_.second ) ); } +// std::cout<<"Gradient partials "<getLightTimeGradientPartialScalingFactor( observation_models::transmitter ) * physical_constants::SPEED_OF_LIGHT * + currentLinkTimeCorrectionGradientPartial_.first, + currentLinkTimeCorrectionGradientPartial_.second ) ); +// std::cout<<"Trans correction "<getLightTimeGradientPartialScalingFactor( observation_models::transmitter ) * physical_constants::SPEED_OF_LIGHT * currentLinkTimeCorrectionGradientPartial_.first<getLightTimeGradientPartialScalingFactor( observation_models::receiver ) * physical_constants::SPEED_OF_LIGHT * + currentLinkTimeCorrectionGradientPartial_.first, + currentLinkTimeCorrectionGradientPartial_.second ) ); +// std::cout<<"Rec. correction "<getLightTimeGradientPartialScalingFactor( observation_models::receiver ) * physical_constants::SPEED_OF_LIGHT* currentLinkTimeCorrectionGradientPartial_.first<, double > > additionalPartials = @@ -226,8 +258,8 @@ class DirectObservationPartial: public ObservationPartial< ObservationSize > const std::vector< Eigen::Vector6d >&, const std::vector< double >& ) > > lighTimeCorrectionPartialsFunctions_; - std::vector< std::function< SingleLightTimePartialReturnType( - const std::vector< Eigen::Vector6d >&, const std::vector< double >& ) > > + std::vector< std::function< SingleLightTimeGradientPartialReturnType( + const std::vector< Eigen::Vector6d >&, const std::vector< double >&, const observation_models::LinkEndType ) > > lighTimeCorrectionGradientPartialsFunctions_; //! List of light-time correction partial objects. @@ -241,6 +273,8 @@ class DirectObservationPartial: public ObservationPartial< ObservationSize > std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > currentLinkTimeCorrectionPartial_; + std::pair< Eigen::Matrix< double, 3, Eigen::Dynamic >, double > currentLinkTimeCorrectionGradientPartial_; + std::map< observation_models::LinkEndType, int > stateEntryIndices_; bool fixLinkEndTime_ = false; diff --git a/include/tudat/astro/orbit_determination/observation_partials/firstOrderRelativisticPartial.h b/include/tudat/astro/orbit_determination/observation_partials/firstOrderRelativisticPartial.h index ece8469f84..1c34bab0bf 100644 --- a/include/tudat/astro/orbit_determination/observation_partials/firstOrderRelativisticPartial.h +++ b/include/tudat/astro/orbit_determination/observation_partials/firstOrderRelativisticPartial.h @@ -107,6 +107,15 @@ class FirstOrderRelativisticLightTimeCorrectionPartial: public LightTimeCorrecti ( times[ 0 ] + times[ 1 ] ) / 2.0 ); } + SingleOneWayRangeGradientPartialReturnType gradientWrtPpnParameterGamma( + const std::vector< Eigen::Vector6d >& states, const std::vector< double >& times, const observation_models::LinkEndType gradientLinkEnd ) + { + return std::make_pair( correctionCalculator_->calculateLightTimeCorrectionPartialDerivativeWrtLinkEndPosition( + states.at( 0 ), states.at( 1 ), times.at( 0 ), times.at( 1 ), gradientLinkEnd ) / + ( correctionCalculator_->getPpnParameterGammaFunction_( )( ) + 1.0 ), + gradientLinkEnd == observation_models::receiver ? times.at( 1 ) : times.at( 0 ) ); + } + //! Function to get the names of bodies causing light-time correction. /*! * Function to get the names of bodies causing light-time correction. diff --git a/include/tudat/astro/orbit_determination/observation_partials/lightTimeCorrectionPartial.h b/include/tudat/astro/orbit_determination/observation_partials/lightTimeCorrectionPartial.h index 3b5d2787b1..1f084c10b1 100644 --- a/include/tudat/astro/orbit_determination/observation_partials/lightTimeCorrectionPartial.h +++ b/include/tudat/astro/orbit_determination/observation_partials/lightTimeCorrectionPartial.h @@ -41,6 +41,8 @@ class LightTimeCorrectionPartial //! Typedef for return argument for light time partial (partial with vector and associated time) typedef std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > SingleOneWayRangePartialReturnType; + typedef std::pair< Eigen::Matrix< double, 3, Eigen::Dynamic >, double > SingleOneWayRangeGradientPartialReturnType; + //! Constructor /*! * Constructor @@ -83,6 +85,12 @@ getLightTimeParameterPartialFunction( const estimatable_parameters::EstimatebleParameterIdentifier parameterId, const std::shared_ptr< LightTimeCorrectionPartial > lightTimeCorrectionPartial ); +std::pair< std::function< LightTimeCorrectionPartial::SingleOneWayRangeGradientPartialReturnType( + const std::vector< Eigen::Vector6d >&, const std::vector< double >&, const observation_models::LinkEndType ) >, bool > +getLightTimeGradientParameterPartialFunction( + const estimatable_parameters::EstimatebleParameterIdentifier parameterId, + const std::shared_ptr< LightTimeCorrectionPartial > lightTimeCorrectionPartial ); + } } diff --git a/include/tudat/astro/orbit_determination/observation_partials/observationPartial.h b/include/tudat/astro/orbit_determination/observation_partials/observationPartial.h index 72db2e7aaa..d2c789cb45 100644 --- a/include/tudat/astro/orbit_determination/observation_partials/observationPartial.h +++ b/include/tudat/astro/orbit_determination/observation_partials/observationPartial.h @@ -125,11 +125,24 @@ class DirectPositionPartialScaling: public PositionPartialScaling } } + virtual Eigen::Matrix< double, ObservationSize, 3 > getLightTimeGradientPartialScalingFactor( const observation_models::LinkEndType linkEndType ) + { + throw std::runtime_error( "Error when getting light time gradient partials of observable type " + + observation_models::getObservableName( observableType_) + + ", derived class model is not implemented." ); + } + + virtual bool useLinkIndependentPartials( ) { return false; } + virtual bool useLightTimeGradientPartials( ) + { + return false; + } + protected: observation_models::ObservableType observableType_; diff --git a/include/tudat/astro/orbit_determination/observation_partials/oneWayDopplerPartial.h b/include/tudat/astro/orbit_determination/observation_partials/oneWayDopplerPartial.h index 4f76ba39a2..a9289106df 100644 --- a/include/tudat/astro/orbit_determination/observation_partials/oneWayDopplerPartial.h +++ b/include/tudat/astro/orbit_determination/observation_partials/oneWayDopplerPartial.h @@ -300,6 +300,23 @@ class OneWayDopplerScaling: public DirectPositionPartialScaling< 1 > return ( Eigen::Vector1d( ) << lightTimeEffectPositionScalingFactor_ ).finished( ); } + Eigen::Matrix< double, 1, 3 > getLightTimeGradientPartialScalingFactor( const observation_models::LinkEndType linkEndType ) + { + if( linkEndType == observation_models::transmitter ) + { + return transmitterPartialScalingTerm_ * transmitterVelocity_.transpose( ) * divisionTerm_; + } + else if( linkEndType == observation_models::receiver ) + { + return receiverPartialScalingTerm_ * receiverVelocity_.transpose( ) * divisionTerm_; + } + else + { + throw std::runtime_error( "Error when getting one-way Doppler light time correction gradient partial, link end type " + + observation_models::getLinkEndTypeString( linkEndType ) + " not supported. " ); + } + } + bool isVelocityScalingNonZero( ) { return true; @@ -351,6 +368,11 @@ class OneWayDopplerScaling: public DirectPositionPartialScaling< 1 > return ( transmitterProperTimePartials_ != nullptr ) || ( receiverProperTimePartials_ != nullptr ); } + virtual bool useLightTimeGradientPartials( ) + { + return true; + } + private: @@ -386,6 +408,16 @@ class OneWayDopplerScaling: public DirectPositionPartialScaling< 1 > //! Object used to compute the contribution of transmitter proper time rate to the scaling std::shared_ptr< OneWayDopplerProperTimeComponentScaling > receiverProperTimePartials_; + + double transmitterPartialScalingTerm_; + + double receiverPartialScalingTerm_; + + Eigen::Vector3d receiverVelocity_; + + Eigen::Vector3d transmitterVelocity_; + + }; //! Function to computed the derivative of the unit vector from transmitter to receiver w.r.t. the observation time diff --git a/src/astro/orbit_determination/observation_partials/lightTimeCorrectionPartial.cpp b/src/astro/orbit_determination/observation_partials/lightTimeCorrectionPartial.cpp index c3244fa1a9..cb4f34aa23 100644 --- a/src/astro/orbit_determination/observation_partials/lightTimeCorrectionPartial.cpp +++ b/src/astro/orbit_determination/observation_partials/lightTimeCorrectionPartial.cpp @@ -84,6 +84,52 @@ getLightTimeParameterPartialFunction( return partialFunction; } +std::pair< std::function< LightTimeCorrectionPartial::SingleOneWayRangeGradientPartialReturnType( + const std::vector< Eigen::Vector6d >&, const std::vector< double >&, const observation_models::LinkEndType ) >, bool > +getLightTimeGradientParameterPartialFunction( + const estimatable_parameters::EstimatebleParameterIdentifier parameterId, + const std::shared_ptr< LightTimeCorrectionPartial > lightTimeCorrectionPartial ) +{ + // Declare return type, set second part to 0 (no dependency found). + std::pair< std::function< LightTimeCorrectionPartial::SingleOneWayRangeGradientPartialReturnType( + const std::vector< Eigen::Vector6d >&, const std::vector< double >&, const observation_models::LinkEndType ) >, bool > partialFunction; + partialFunction.second = 0; + + // Check type of light-time correction + switch( lightTimeCorrectionPartial->getCorrectionType( ) ) + { + // Correction type of 1st-order relativistic + case observation_models::first_order_relativistic: + { + // Check consistency of input. + std::shared_ptr< FirstOrderRelativisticLightTimeCorrectionPartial > currentLightTimeCorrectorPartial = + std::dynamic_pointer_cast< FirstOrderRelativisticLightTimeCorrectionPartial >( lightTimeCorrectionPartial ); + if( currentLightTimeCorrectorPartial == nullptr ) + { + std::string errorMessage = "Error when getting light time correction partial function, type " + + std::to_string( lightTimeCorrectionPartial->getCorrectionType( ) ) + + "is inconsistent."; + throw std::runtime_error( errorMessage ); + } + + // Set partial of gravitational parameter + if( parameterId.first == estimatable_parameters::ppn_parameter_gamma ) + { + partialFunction = std::make_pair( + std::bind( &FirstOrderRelativisticLightTimeCorrectionPartial::gradientWrtPpnParameterGamma, + currentLightTimeCorrectorPartial, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3 ), 1 ); + } + break; + } + default: + std::string errorMessage = "Error, light time correction type " + std::to_string( + lightTimeCorrectionPartial->getCorrectionType( ) ) + "not found when creating partial "; + throw std::runtime_error( errorMessage ); + } + + return partialFunction; +} + } } diff --git a/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp b/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp index 6e909f9bd9..91f1930d67 100644 --- a/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp +++ b/src/astro/orbit_determination/observation_partials/oneWayDopplerPartial.cpp @@ -200,68 +200,68 @@ void OneWayDopplerScaling::update( const std::vector< Eigen::Vector6d >& linkEnd // Compute geometry. double distance = ( linkEndStates.at( 1 ) - linkEndStates.at( 0 ) ).segment( 0, 3 ).norm( ); Eigen::Vector3d lineOfSightVector = ( linkEndStates.at( 1 ) - linkEndStates.at( 0 ) ).segment( 0, 3 ).normalized( ); - Eigen::Vector3d receiverVelocity = linkEndStates.at( 1 ).segment( 3, 3 ); - Eigen::Vector3d transmitterVelocity = linkEndStates.at( 0 ).segment( 3, 3 ); + receiverVelocity_ = linkEndStates.at( 1 ).segment( 3, 3 ); + transmitterVelocity_ = linkEndStates.at( 0 ).segment( 3, 3 ); double lineOfSightVelocityReceiver = observation_models::calculateLineOfSightVelocityAsCFraction< double >( - lineOfSightVector, receiverVelocity ); + lineOfSightVector, receiverVelocity_ ); double lineOfSightVelocityTransmitter = observation_models::calculateLineOfSightVelocityAsCFraction< double >( - lineOfSightVector, transmitterVelocity ); + lineOfSightVector, transmitterVelocity_ ); // Compute scaling terms of transmitter/receiver - double transmitterPartialScalingTerm = 0.0; - double receiverPartialScalingTerm = -1.0; + transmitterPartialScalingTerm_ = 0.0; + receiverPartialScalingTerm_ = -1.0; double currentTaylorSeriesTerm = 1.0; for( int i = 1; i <= 3; i++ ) { - transmitterPartialScalingTerm += static_cast< double >( i ) * currentTaylorSeriesTerm; + transmitterPartialScalingTerm_ += static_cast< double >( i ) * currentTaylorSeriesTerm; currentTaylorSeriesTerm *= lineOfSightVelocityTransmitter; - receiverPartialScalingTerm -= currentTaylorSeriesTerm; + receiverPartialScalingTerm_ -= currentTaylorSeriesTerm; } - transmitterPartialScalingTerm *= ( 1.0 - lineOfSightVelocityReceiver ); + transmitterPartialScalingTerm_ *= ( 1.0 - lineOfSightVelocityReceiver ); // Compute position partial scaling term, positionScalingFactor_ = - ( receiverVelocity.transpose( ) * receiverPartialScalingTerm + - transmitterVelocity.transpose( ) * transmitterPartialScalingTerm ) / divisionTerm_ * + ( receiverVelocity_.transpose( ) * receiverPartialScalingTerm_ + + transmitterVelocity_.transpose( ) * transmitterPartialScalingTerm_ ) / divisionTerm_ * ( Eigen::Matrix3d::Identity( ) - lineOfSightVector * lineOfSightVector.transpose( ) ) / distance; if( fixedLinkEnd == observation_models::receiver ) { lightTimeEffectPositionScalingFactor_ = - -1.0 / ( ( physical_constants::SPEED_OF_LIGHT - lineOfSightVector.dot( transmitterVelocity ) ) * divisionTerm_ ) * - ( transmitterPartialScalingTerm * + -1.0 / ( ( physical_constants::SPEED_OF_LIGHT - lineOfSightVector.dot( transmitterVelocity_ ) ) * divisionTerm_ ) * + ( transmitterPartialScalingTerm_ * computePartialOfProjectedLinkEndVelocityWrtAssociatedTime( ( linkEndStates.at( 1 ) - linkEndStates.at( 0 ) ).segment( 0, 3 ), - transmitterVelocity, transmitterVelocity, transmitterAccelerationFunction_( times.at( 0 ) ), false, true ) + - receiverPartialScalingTerm * + transmitterVelocity_, transmitterVelocity_, transmitterAccelerationFunction_( times.at( 0 ) ), false, true ) + + receiverPartialScalingTerm_ * computePartialOfProjectedLinkEndVelocityWrtAssociatedTime( ( linkEndStates.at( 1 ) - linkEndStates.at( 0 ) ).segment( 0, 3 ), - receiverVelocity, transmitterVelocity, Eigen::Vector3d::Zero( ), false, false ) ); + receiverVelocity_, transmitterVelocity_, Eigen::Vector3d::Zero( ), false, false ) ); } else if( fixedLinkEnd == observation_models::transmitter ) { lightTimeEffectPositionScalingFactor_ = - 1.0 / ( ( physical_constants::SPEED_OF_LIGHT - lineOfSightVector.dot( receiverVelocity ) ) * divisionTerm_ ) * - ( receiverPartialScalingTerm * + 1.0 / ( ( physical_constants::SPEED_OF_LIGHT - lineOfSightVector.dot( receiverVelocity_ ) ) * divisionTerm_ ) * + ( receiverPartialScalingTerm_ * computePartialOfProjectedLinkEndVelocityWrtAssociatedTime( ( linkEndStates.at( 1 ) - linkEndStates.at( 0 ) ).segment( 0, 3 ), - receiverVelocity, receiverVelocity, receiverAccelerationFunction_( times.at( 1 ) ), true, true ) + - transmitterPartialScalingTerm * + receiverVelocity_, receiverVelocity_, receiverAccelerationFunction_( times.at( 1 ) ), true, true ) + + transmitterPartialScalingTerm_ * computePartialOfProjectedLinkEndVelocityWrtAssociatedTime( ( linkEndStates.at( 1 ) - linkEndStates.at( 0 ) ).segment( 0, 3 ), - transmitterVelocity, receiverVelocity, Eigen::Vector3d::Zero( ), true, true ) ); + transmitterVelocity_, receiverVelocity_, Eigen::Vector3d::Zero( ), true, true ) ); } positionScalingFactor_ += lineOfSightVector.transpose( ) * lightTimeEffectPositionScalingFactor_; // Compute velocity scaling terms - receiverVelocityScalingFactor_ = -lineOfSightVector.transpose( ) * receiverPartialScalingTerm / divisionTerm_; - transmitterVelocityScalingFactor_ = -lineOfSightVector.transpose( ) * transmitterPartialScalingTerm / divisionTerm_; + receiverVelocityScalingFactor_ = -lineOfSightVector.transpose( ) * receiverPartialScalingTerm_ / divisionTerm_; + transmitterVelocityScalingFactor_ = -lineOfSightVector.transpose( ) * transmitterPartialScalingTerm_ / divisionTerm_; // Update proper time scaling objects. currentLinkEndType_ = fixedLinkEnd; diff --git a/src/astro/orbit_determination/observation_partials/twoWayDopplerPartial.cpp b/src/astro/orbit_determination/observation_partials/twoWayDopplerPartial.cpp index 04145a352c..b3118c0c8d 100644 --- a/src/astro/orbit_determination/observation_partials/twoWayDopplerPartial.cpp +++ b/src/astro/orbit_determination/observation_partials/twoWayDopplerPartial.cpp @@ -106,6 +106,7 @@ TwoWayDopplerPartial::TwoWayDopplerPartialReturnType TwoWayDopplerPartial::calcu const std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > ancillarySettings, const Eigen::Vector1d& currentObservation ) { +// std::cout<<"Two-way parameter "<parameterIdentifier_.first<<" *****************************"<first<parameterIdentifier_.first != estimatable_parameters::ppn_parameter_gamma ) + { + + currentPartialSet[ i ].first *= currentPartialMultiplier; +// std::cout << "One-way Doppler partial (post)" << " " << currentPartialSet[ i ].first << std::endl; + } + else + { +// std::cout<<"Scaling "<( oneWayDopplerModel, bodies, fullEstimatableParameterSet, linkEnds, one_way_doppler, 1.0E-5, - true, true, 10.0, parameterPerturbationMultipliers, nullptr, 1.1E7, estimationCase == 2 ? 1.0 : 2000.0 ); + true, true, 10.0, parameterPerturbationMultipliers, nullptr, 1.1E7, estimationCase == 2 ? 1.0 : 20.0 ); } @@ -328,7 +328,7 @@ BOOST_AUTO_TEST_CASE( testOneWayDopplerPartials ) testObservationPartials< 1 >( oneWayDopplerModel, bodies, fullEstimatableParameterSet, linkEnds, one_way_doppler, 1.0E-4, false, true, - 1.0, parameterPerturbationMultipliers, nullptr, 1.1E7, estimationCase == 2 ? 1.0 : 2000.0 ); + 1.0, parameterPerturbationMultipliers, nullptr, 1.1E7 ); } } diff --git a/tests/src/astro/propagators/unitTestRotationalDynamicsPropagator.cpp b/tests/src/astro/propagators/unitTestRotationalDynamicsPropagator.cpp index 3114111acf..6aaf2b2448 100755 --- a/tests/src/astro/propagators/unitTestRotationalDynamicsPropagator.cpp +++ b/tests/src/astro/propagators/unitTestRotationalDynamicsPropagator.cpp @@ -1143,8 +1143,8 @@ BOOST_AUTO_TEST_CASE( testSimpleRotationalDynamicsPropagationWithVaryinInertiaTe auto dependentVariableHistory = dynamicsSimulator.getDependentVariableHistory( ); Eigen::Matrix3d nominalInertiaTensor = Eigen::Matrix3d::Zero( ); - Eigen::Vector3d nominalCosineCorrection = Eigen::Matrix3d::Zero( ); - Eigen::Vector2d nominalSineCorrection = Eigen::Matrix3d::Zero( ); + Eigen::Vector3d nominalCosineCorrection = Eigen::Vector3d::Zero( ); + Eigen::Vector2d nominalSineCorrection = Eigen::Vector2d::Zero( ); std::shared_ptr< gravitation::SphericalHarmonicsGravityField > phobosGravityField = std::dynamic_pointer_cast< gravitation::SphericalHarmonicsGravityField >( From dc285a3e5534099cbd18bee80707905125364342 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Wed, 18 Dec 2024 17:11:02 +0100 Subject: [PATCH 17/24] Fixed Doppler partials! --- .../directObservationPartial.h | 3 --- .../oneWayDopplerPartial.h | 4 ++-- .../twoWayDopplerPartial.cpp | 17 +--------------- .../unitTestOneWayDopplerPartials.cpp | 20 +------------------ 4 files changed, 4 insertions(+), 40 deletions(-) diff --git a/include/tudat/astro/orbit_determination/observation_partials/directObservationPartial.h b/include/tudat/astro/orbit_determination/observation_partials/directObservationPartial.h index 21a868af14..6dac35886f 100644 --- a/include/tudat/astro/orbit_determination/observation_partials/directObservationPartial.h +++ b/include/tudat/astro/orbit_determination/observation_partials/directObservationPartial.h @@ -174,7 +174,6 @@ class DirectObservationPartial: public ObservationPartial< ObservationSize > currentLinkTimeCorrectionPartial_.second ) ); } -// std::cout<<"Gradient partials "< std::make_pair( positionPartialScaler_->getLightTimeGradientPartialScalingFactor( observation_models::transmitter ) * physical_constants::SPEED_OF_LIGHT * currentLinkTimeCorrectionGradientPartial_.first, currentLinkTimeCorrectionGradientPartial_.second ) ); -// std::cout<<"Trans correction "<getLightTimeGradientPartialScalingFactor( observation_models::transmitter ) * physical_constants::SPEED_OF_LIGHT * currentLinkTimeCorrectionGradientPartial_.first<getLightTimeGradientPartialScalingFactor( observation_models::receiver ) * physical_constants::SPEED_OF_LIGHT * currentLinkTimeCorrectionGradientPartial_.first, currentLinkTimeCorrectionGradientPartial_.second ) ); -// std::cout<<"Rec. correction "<getLightTimeGradientPartialScalingFactor( observation_models::receiver ) * physical_constants::SPEED_OF_LIGHT* currentLinkTimeCorrectionGradientPartial_.first< { if( linkEndType == observation_models::transmitter ) { - return transmitterPartialScalingTerm_ * transmitterVelocity_.transpose( ) * divisionTerm_; + return -transmitterPartialScalingTerm_ * transmitterVelocity_.transpose( ) / divisionTerm_; } else if( linkEndType == observation_models::receiver ) { - return receiverPartialScalingTerm_ * receiverVelocity_.transpose( ) * divisionTerm_; + return receiverPartialScalingTerm_ * receiverVelocity_.transpose( ) / divisionTerm_; } else { diff --git a/src/astro/orbit_determination/observation_partials/twoWayDopplerPartial.cpp b/src/astro/orbit_determination/observation_partials/twoWayDopplerPartial.cpp index b3118c0c8d..04145a352c 100644 --- a/src/astro/orbit_determination/observation_partials/twoWayDopplerPartial.cpp +++ b/src/astro/orbit_determination/observation_partials/twoWayDopplerPartial.cpp @@ -106,7 +106,6 @@ TwoWayDopplerPartial::TwoWayDopplerPartialReturnType TwoWayDopplerPartial::calcu const std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > ancillarySettings, const Eigen::Vector1d& currentObservation ) { -// std::cout<<"Two-way parameter "<parameterIdentifier_.first<<" *****************************"<first<parameterIdentifier_.first != estimatable_parameters::ppn_parameter_gamma ) - { - - currentPartialSet[ i ].first *= currentPartialMultiplier; -// std::cout << "One-way Doppler partial (post)" << " " << currentPartialSet[ i ].first << std::endl; - } - else - { -// std::cout<<"Scaling "< transmitterProperTimeRateCalculator = oneWayDopplerModel->getTransmitterProperTimeRateCalculator( ); - std::cout<<"Trans/rec (null) "< > fullEstimatableParameterSet = createEstimatableParameters( bodies, 1.1E7 ); @@ -385,7 +383,6 @@ BOOST_AUTO_TEST_CASE( testOneWayDopplerPartials ) std::shared_ptr< OneWayDopplerProperTimeComponentScaling > receiverProperTimePartials = partialScalingObject->getReceiverProperTimePartials( ); - std::cout<<"List size "< > earthStatePartial = std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( ( dopplerPartials.first ).begin( )->second ); @@ -398,13 +395,10 @@ BOOST_AUTO_TEST_CASE( testOneWayDopplerPartials ) std::vector< double > linkEndTimes; std::vector< Eigen::Vector6d > linkEndStates; LinkEndType referenceLinkEnd = transmitter; - std::cout<<"Computing nominal observable: "<computeIdealObservationsWithLinkEndData( observationTime, referenceLinkEnd, linkEndTimes, linkEndStates ); - std::cout<<"Nominal: "<update( linkEndStates, linkEndTimes, referenceLinkEnd, nominalObservable ); @@ -438,11 +432,6 @@ BOOST_AUTO_TEST_CASE( testOneWayDopplerPartials ) calculatePartialWrtConstantBodyVelocity( "Earth", bodies, Eigen::Vector3d::Constant( 1.0E0 ), transmitterProperTimeRateFunction, 1.1E7, 1 ); - std::cout<<"Partials w.r.t. Mars: "<getPositionScalingFactor( transmitter )<< - std::endl<getPositionScalingFactor( receiver )<< - std::endl<getPositionScalingFactor( transmitter ) ), @@ -473,12 +462,6 @@ BOOST_AUTO_TEST_CASE( testOneWayDopplerPartials ) calculatePartialWrtConstantBodyVelocity( "Earth", bodies, Eigen::Vector3d::Constant( 1000.0 ), receiverProperTimeRateFunction, 1.1E7, 1 ); - std::cout<<"Trans. scaling factor: "<getPositionScalingFactor( transmitter )<< - std::endl<getPositionScalingFactor( receiver )<< - std::endl<getPositionScalingFactor( receiver ) ), ( numericalReceiverProperTimePartialsWrtEarthPosition ), 1.0E-6 ); @@ -525,7 +508,6 @@ BOOST_AUTO_TEST_CASE( testOneWayDopplerPartials ) Eigen::VectorXd nominalObservableWithoutProperTime = oneWayDopplerModelWithoutProperTime->computeIdealObservationsWithLinkEndData( observationTime, referenceLinkEnd, linkEndTimesWithoutProperTime, linkEndStatesWithoutProperTime ); - std::cout<<"Nominal without proper time: "<update( linkEndStatesWithoutProperTime, linkEndTimesWithoutProperTime, From e2bf6effc5a7591ee3d7ce23a5dc98ab96f62f7b Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Wed, 18 Dec 2024 17:21:51 +0100 Subject: [PATCH 18/24] Warning suppression; test re-enabling --- .../tudat/astro/electromagnetism/radiationSourceModel.h | 2 -- .../unitTestDsnNWayDopplerObservationModel.cpp | 8 ++++---- tests/src/interface/spice/unitTestSpiceInterface.cpp | 2 ++ 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/tudat/astro/electromagnetism/radiationSourceModel.h b/include/tudat/astro/electromagnetism/radiationSourceModel.h index 9e744397ef..d13b04668a 100644 --- a/include/tudat/astro/electromagnetism/radiationSourceModel.h +++ b/include/tudat/astro/electromagnetism/radiationSourceModel.h @@ -26,12 +26,10 @@ #include #include #include -#include #include #include #include -#include #include "tudat/astro/electromagnetism/luminosityModel.h" #include "tudat/astro/electromagnetism/sourcePanelRadiosityModel.h" diff --git a/tests/src/astro/observation_models/unitTestDsnNWayDopplerObservationModel.cpp b/tests/src/astro/observation_models/unitTestDsnNWayDopplerObservationModel.cpp index 87270d0f47..1a4d7aed89 100644 --- a/tests/src/astro/observation_models/unitTestDsnNWayDopplerObservationModel.cpp +++ b/tests/src/astro/observation_models/unitTestDsnNWayDopplerObservationModel.cpp @@ -268,8 +268,8 @@ BOOST_AUTO_TEST_CASE( testDsnNWayAveragedDopplerModel ) } if ( simulatedTimes.at( j ) < -2.56e7 ) { - // BOOST_TEST( residual >= firstBlockMinMaxResidual.first ); - // BOOST_TEST( residual <= firstBlockMinMaxResidual.second ); + BOOST_TEST( residual >= firstBlockMinMaxResidual.first ); + BOOST_TEST( residual <= firstBlockMinMaxResidual.second ); if( residual < firstMinimum ) { firstMinimum = residual; @@ -281,8 +281,8 @@ BOOST_AUTO_TEST_CASE( testDsnNWayAveragedDopplerModel ) } else { - // BOOST_TEST( residual >= secondBlockMinMaxResidual.first ); - // BOOST_TEST( residual <= secondBlockMinMaxResidual.second ); + BOOST_TEST( residual >= secondBlockMinMaxResidual.first ); + BOOST_TEST( residual <= secondBlockMinMaxResidual.second ); if( residual < secondMinimum ) { secondMinimum = residual; diff --git a/tests/src/interface/spice/unitTestSpiceInterface.cpp b/tests/src/interface/spice/unitTestSpiceInterface.cpp index beb6225bae..9ae5448c17 100644 --- a/tests/src/interface/spice/unitTestSpiceInterface.cpp +++ b/tests/src/interface/spice/unitTestSpiceInterface.cpp @@ -35,6 +35,7 @@ #include "tudat/astro/basic_astro/physicalConstants.h" #include "tudat/basics/testMacros.h" +#include "tudat/basics/utilityMacros.h" #include "tudat/io/basicInputOutput.h" #include "tudat/basics/basicTypedefs.h" #include "tudat/interface/spice/spiceEphemeris.h" @@ -83,6 +84,7 @@ BOOST_AUTO_TEST_CASE( testNoKernelCrash ) suppressErrorOutput( ); const Eigen::Vector6d wrapperState = getBodyCartesianStateAtEpoch( target, observer, referenceFrame, aberrationCorrections, ephemerisTime ); + TUDAT_UNUSED_PARAMETER( wrapperState ); } // Test 1: Test Julian day <-> Ephemeris time conversions at J2000. From 8000af8ec975d37eb9d65fddde74827466a6734d Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Wed, 18 Dec 2024 20:07:29 +0100 Subject: [PATCH 19/24] Added corrected 2-way frequency partials observable --- .../createObservationModel.cpp | 4 +- .../support/observationPartialTestFunctions.h | 4 +- .../unitTestObservedFrequencyPartials.cpp | 133 ++++++++++++- .../unitTestObservedFrequencyPartialsOld.cpp | 180 ------------------ 4 files changed, 129 insertions(+), 192 deletions(-) delete mode 100644 tests/src/astro/orbit_determination/observation_partials/unitTestObservedFrequencyPartialsOld.cpp diff --git a/src/simulation/estimation_setup/createObservationModel.cpp b/src/simulation/estimation_setup/createObservationModel.cpp index 77a146f300..709a4fffd7 100644 --- a/src/simulation/estimation_setup/createObservationModel.cpp +++ b/src/simulation/estimation_setup/createObservationModel.cpp @@ -59,7 +59,7 @@ std::function< double ( observation_models::FrequencyBands, observation_models:: if ( bodies.getBody( linkEnds.at( observation_models::retransmitter ).bodyName_ )->getVehicleSystems( ) == nullptr ) { throw std::runtime_error( - "Error when creating DSN N-way averaged Doppler observation model: vehicle systems are not " + "Error when retrieving turnaround ratio: vehicle systems are not " "defined for retransmitter link end body " + linkEnds.at( observation_models::retransmitter ).bodyName_ + "." ); } turnaroundRatioFunction = bodies.getBody( linkEnds.at( observation_models::retransmitter ).bodyName_ )->getVehicleSystems( @@ -72,7 +72,7 @@ std::function< double ( observation_models::FrequencyBands, observation_models:: linkEnds.at( observation_models::retransmitter ).stationName_ )->getVehicleSystems( ) == nullptr ) { throw std::runtime_error( - "Error when creating DSN N-way averaged Doppler observation model: vehicle systems are not " + "Error when retrieving turnaround ratio: vehicle systems are not " "defined for retransmitter link end ID " + linkEnds.at( observation_models::retransmitter ).stationName_ + "." ); } turnaroundRatioFunction = bodies.getBody( linkEnds.at( observation_models::retransmitter ).bodyName_ )->getGroundStation( diff --git a/tests/include/tudat/support/observationPartialTestFunctions.h b/tests/include/tudat/support/observationPartialTestFunctions.h index dd5bd8cd71..dbeaa91863 100644 --- a/tests/include/tudat/support/observationPartialTestFunctions.h +++ b/tests/include/tudat/support/observationPartialTestFunctions.h @@ -147,7 +147,6 @@ void testObservationPartials( double observationTime = 1.1E7, const double gammaToleranceWeakening = 1.0 ) { - printEstimatableParameterEntries( fullEstimatableParameterSet ); // Retrieve double and vector parameters and estimate body states @@ -206,7 +205,7 @@ void testObservationPartials( { runSimulation = false; } - + // Remove retransmission delay from the retransmitting reference link end: computation of multi-leg light currently doesn't support // retransmission delays at the reference link end std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > modifiedAncilliarySettings; @@ -233,6 +232,7 @@ void testObservationPartials( if ( runSimulation ) { + // Evaluate nominal observation values std::vector vectorOfStates; std::vector vectorOfTimes; diff --git a/tests/src/astro/orbit_determination/observation_partials/unitTestObservedFrequencyPartials.cpp b/tests/src/astro/orbit_determination/observation_partials/unitTestObservedFrequencyPartials.cpp index 84a7e6cfc2..6292207b26 100644 --- a/tests/src/astro/orbit_determination/observation_partials/unitTestObservedFrequencyPartials.cpp +++ b/tests/src/astro/orbit_determination/observation_partials/unitTestObservedFrequencyPartials.cpp @@ -21,6 +21,7 @@ #include "tudat/simulation/estimation.h" #include "tudat/simulation/estimation_setup.h" #include "tudat/simulation/estimation_setup/createEstimatableParameters.h" +#include "tudat/io/readOdfFile.h" #include "tudat/io/readOdfFile.h" #include "tudat/io/readTabulatedMediaCorrections.h" @@ -51,6 +52,7 @@ using namespace tudat::ground_stations; using namespace tudat::observation_models; using namespace tudat::ephemerides; using namespace tudat::electromagnetism; +using namespace tudat::input_output; using namespace tudat; namespace tudat @@ -117,12 +119,11 @@ void testPartials( totalPartial.setZero( ); for( int i = 0; i < singlePartialSet.size( ); i++ ) { - std::cout< numericalPartial = Eigen::Matrix< double, 1, 6 >::Zero( ); double positionPerturbation = 10000.0; double velocityPerturbation = 1.0; @@ -161,14 +162,10 @@ void testPartials( stateWrapper.referenceState_( i + 3 ) += velocityPerturbation; numericalPartial( i + 3 ) = ( upperturbedObservation( 0 ) - downperturbedObservation( 0 ) ) / ( 2.0 * velocityPerturbation ); } -// std::cout<<"ANA: "< > groundStations; + groundStations.resize( 2 ); + groundStations[ 0 ] = std::make_pair( "Earth", "Graz" ); + groundStations[ 1 ] = std::make_pair( "Mars", "MSL" ); + + + // Test partials with constant ephemerides (allows test of position partials) + { + // Create environment + SystemOfBodies bodies = setupEnvironment( groundStations, 1.0E7, 1.2E7, 1.1E7, true ); + + std::shared_ptr< system_models::VehicleSystems > vehicleSystems = std::make_shared< system_models::VehicleSystems >( ); + vehicleSystems->setTransponderTurnaroundRatio( [=]( FrequencyBands uplinkBand, FrequencyBands downlinkBand ){ return 1.0; } ); + bodies.getBody( "Earth" )->getGroundStation( "Graz" )->setVehicleSystems( vehicleSystems ); + bodies.getBody( "Mars" )->getGroundStation( "MSL" )->setTransmittingFrequencyCalculator( std::make_shared< ConstantFrequencyInterpolator >( 1.0 ) ); + + // Set link ends for observation model + LinkDefinition linkEnds; + linkEnds[ transmitter ] = groundStations[ 1 ]; + linkEnds[ reflector1 ] = groundStations[ 0 ]; + linkEnds[ receiver ] = groundStations[ 1 ]; + + + std::cout << "Case A " << std::endl; + // Generate one-way doppler model + std::shared_ptr< ObservationModel< 1 > > twoWayDopplerModel; + std::vector< std::string > perturbingBodies; + perturbingBodies.push_back( "Earth" ); + + twoWayDopplerModel = + observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( + std::make_shared< TwoWayDopplerObservationSettings > + ( std::make_shared< observation_models::OneWayDopplerObservationSettings >( + getUplinkFromTwoWayLinkEnds( linkEnds ), + std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( + perturbingBodies ), + std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Mars" ), + std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), + nullptr, std::make_shared< LightTimeConvergenceCriteria>( ), true ), + std::make_shared< observation_models::OneWayDopplerObservationSettings >( + getDownlinkFromTwoWayLinkEnds( linkEnds ), + std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( + perturbingBodies ), + std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), + std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Mars" ), + nullptr, std::make_shared< LightTimeConvergenceCriteria>( ), true ), + doppler_measured_frequency ), bodies ); + + // Create parameter objects. + std::shared_ptr< EstimatableParameterSet< double > > fullEstimatableParameterSet; + Eigen::VectorXd parameterPerturbationMultipliers = Eigen::Vector4d::Constant( 1.0 ); + fullEstimatableParameterSet = createEstimatableParameters( bodies, 1.1E7 ); + + printEstimatableParameterEntries( fullEstimatableParameterSet ); + + testObservationPartials< 1 >( + twoWayDopplerModel, bodies, fullEstimatableParameterSet, linkEnds, doppler_measured_frequency, 1.0E-5, + true, true, 10.0, parameterPerturbationMultipliers, getDopplerMeasuredFrequencyAncilliarySettings( + std::vector< FrequencyBands >{ x_band, x_band } ), 1.1E7, 100.0 ); + } + + // Test partials with real ephemerides (without test of position partials) + { + + // Create environment + SystemOfBodies bodies = setupEnvironment( groundStations, 1.0E7, 1.2E7, 1.1E7, false ); + + std::shared_ptr< system_models::VehicleSystems > vehicleSystems = std::make_shared< system_models::VehicleSystems >( ); + vehicleSystems->setTransponderTurnaroundRatio( [=]( FrequencyBands uplinkBand, FrequencyBands downlinkBand ){ return 1.0; } ); + bodies.getBody( "Earth" )->getGroundStation( "Graz" )->setVehicleSystems( vehicleSystems ); + bodies.getBody( "Mars" )->getGroundStation( "MSL" )->setTransmittingFrequencyCalculator( std::make_shared< ConstantFrequencyInterpolator >( 1.0 ) ); + + // Set link ends for observation model + LinkDefinition linkEnds; + linkEnds[ transmitter ] = groundStations[ 1 ]; + linkEnds[ reflector1 ] = groundStations[ 0 ]; + linkEnds[ receiver ] = groundStations[ 1 ]; + + std::cout << "Case B " << std::endl; + // Generate two-way doppler model + std::shared_ptr< ObservationModel< 1 > > twoWayDopplerModel; + std::vector< std::string > perturbingBodies; + perturbingBodies.push_back( "Earth" ); + + twoWayDopplerModel = + observation_models::ObservationModelCreator< 1, double, double >::createObservationModel( + std::make_shared< TwoWayDopplerObservationSettings > + ( std::make_shared< OneWayDopplerObservationSettings >( + getUplinkFromTwoWayLinkEnds( linkEnds ), + std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( + perturbingBodies ), + std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Mars" ), + std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), + nullptr, std::make_shared< LightTimeConvergenceCriteria>( ), true ), + std::make_shared< OneWayDopplerObservationSettings >( + getDownlinkFromTwoWayLinkEnds( linkEnds ), + std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( + perturbingBodies ), + std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), + std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Mars" ) , + nullptr, std::make_shared< LightTimeConvergenceCriteria>( ), true ), doppler_measured_frequency ), bodies ); + + // Create parameter objects. + std::shared_ptr< EstimatableParameterSet< double > > fullEstimatableParameterSet; + Eigen::VectorXd parameterPerturbationMultipliers = Eigen::Vector4d::Constant( 1.0 ); + fullEstimatableParameterSet = createEstimatableParameters( bodies, 1.1E7 ); + + printEstimatableParameterEntries( fullEstimatableParameterSet ); + + testObservationPartials< 1 >( + twoWayDopplerModel, bodies, fullEstimatableParameterSet, linkEnds, doppler_measured_frequency, 1.0E-4, false, true, + 1.0, parameterPerturbationMultipliers, getDopplerMeasuredFrequencyAncilliarySettings( + std::vector< FrequencyBands >{ x_band, x_band } ), 1.1E7, 100.0 ); + + } +} BOOST_AUTO_TEST_SUITE_END( ) diff --git a/tests/src/astro/orbit_determination/observation_partials/unitTestObservedFrequencyPartialsOld.cpp b/tests/src/astro/orbit_determination/observation_partials/unitTestObservedFrequencyPartialsOld.cpp deleted file mode 100644 index 32e47c19eb..0000000000 --- a/tests/src/astro/orbit_determination/observation_partials/unitTestObservedFrequencyPartialsOld.cpp +++ /dev/null @@ -1,180 +0,0 @@ -/* Copyright (c) 2010-2023, Delft University of Technology - * All rights 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_DYN_LINK -#define BOOST_TEST_MAIN - -#include -#include - -#include - -#include "tudat/basics/testMacros.h" -#include "tudat/simulation/estimation.h" -#include "tudat/simulation/estimation_setup.h" -#include "tudat/simulation/estimation_setup/createEstimatableParameters.h" - -#include "tudat/io/readOdfFile.h" -#include "tudat/io/readTabulatedMediaCorrections.h" -#include "tudat/io/readTabulatedWeatherData.h" -#include "tudat/simulation/estimation_setup/processOdfFile.h" -#include "tudat/simulation/estimation_setup/processTrackingTxtFile.h" - -#include - -#include "tudat/astro/ground_stations/transmittingFrequencies.h" -#include "tudat/support/observationPartialTestFunctions.h" - -using namespace tudat::propagators; -using namespace tudat::estimatable_parameters; -using namespace tudat::spice_interface; -using namespace tudat::ephemerides; -using namespace tudat::input_output; -using namespace tudat::observation_models; -using namespace tudat::simulation_setup; -using namespace tudat::numerical_integrators; -using namespace tudat::basic_astrodynamics; -using namespace tudat::reference_frames; -using namespace tudat; - -namespace tudat -{ -namespace unit_tests -{ - - - -BOOST_AUTO_TEST_SUITE( test_frequency_observation_partials) - -//! Test partial derivatives of observed frequency observable, using general test suite of observation partials. -BOOST_AUTO_TEST_CASE( testFrequencyDopplerPartials ) -{ - - Eigen::VectorXd parameterPerturbationMultipliers = - ( Eigen::VectorXd( 4 ) << 100.0, 100.0, 1.0, 100.0 ).finished( ); - - // Define and create ground stations. - std::vector< std::pair< std::string, std::string > > groundStations; - groundStations.resize( 2 ); - groundStations[ 0 ] = std::make_pair( "Earth", "DSS-55" ); - groundStations[ 1 ] = std::make_pair( "Mars", "MSL" ); - - double initialEphemerisTime = 544845633.0; - double finalEphemerisTime = 544869060.0; - double stateEvaluationTime = initialEphemerisTime + 8.0e3; - - // Read ODF file - used just for the automatic creation of ground station ramp frequency calculator - std::shared_ptr< OdfRawFileContents > rawOdfFileContents = - std::make_shared< OdfRawFileContents >( tudat::paths::getTudatTestDataPath( ) + "mromagr2017_097_1335xmmmv1.odf" ); - // Test partials with constant ephemerides (allows test of position partials) - { - // Create environment - SystemOfBodies bodies = setupEnvironment( groundStations, initialEphemerisTime, - finalEphemerisTime, stateEvaluationTime, true ); - - // Process ODF file - std::shared_ptr< ProcessedOdfFileContents< Time > > processedOdfFileContents = - std::make_shared< ProcessedOdfFileContents< Time > >( rawOdfFileContents, "MSL", true ); - // Create ground stations - setTransmittingFrequenciesInGroundStations( processedOdfFileContents, bodies.getBody( "Earth" ) ); - // Set turnaround ratios in spacecraft (ground station) - std::shared_ptr< system_models::VehicleSystems > vehicleSystems = std::make_shared< system_models::VehicleSystems >( ); - vehicleSystems->setTransponderTurnaroundRatio( &getDsnDefaultTurnaroundRatios ); - bodies.getBody( "Mars" )->getGroundStation( "MSL" )->setVehicleSystems( vehicleSystems ); - - // Set link ends for observation model - LinkEnds linkEnds; - linkEnds[ transmitter ] = groundStations[ 0 ]; - linkEnds[ retransmitter ] = groundStations[ 1 ]; - linkEnds[ receiver ] = groundStations[ 0 ]; - - // Generate DSN n-way averaged doppler model - std::vector< std::string > perturbingBodies; - perturbingBodies.push_back( "Earth" ); - std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList; - lightTimeCorrectionsList.push_back( - std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( perturbingBodies ) ); - - std::shared_ptr< ObservationModel< 1, double, Time > > observedDopplerModel = - observation_models::ObservationModelCreator< 1, double, Time >::createObservationModel( - dopplerMeasuredFrequencyObservationSettings( - linkEnds, - lightTimeCorrectionsList ) , bodies ); - - // Create parameter objects. - std::shared_ptr< EstimatableParameterSet< double > > fullEstimatableParameterSet = - createEstimatableParameters( bodies, stateEvaluationTime ); - - testObservationPartials< 1 >( - observedDopplerModel, bodies, fullEstimatableParameterSet, linkEnds, - doppler_measured_frequency, 1.0E-4, true, true, 1000.0, parameterPerturbationMultipliers, - getDopplerMeasuredFrequencyAncilliarySettings( - std::vector< FrequencyBands >{ x_band, x_band } ), - stateEvaluationTime ); - - - } -// -// // Test partials with real ephemerides (without test of position partials) -// { -// // Create environment -// SystemOfBodies bodies = setupEnvironment( groundStations, initialEphemerisTime, -// finalEphemerisTime, stateEvaluationTime, true ); -// -// // Process ODF file -// std::shared_ptr< ProcessedOdfFileContents< Time > > processedOdfFileContents = -// std::make_shared< ProcessedOdfFileContents< Time > >( rawOdfFileContents, "MSL", true ); -// // Create ground stations -// setTransmittingFrequenciesInGroundStations( processedOdfFileContents, bodies.getBody( "Earth" ) ); -// // Set turnaround ratios in spacecraft (ground station) -// std::shared_ptr< system_models::VehicleSystems > vehicleSystems = std::make_shared< system_models::VehicleSystems >( ); -// vehicleSystems->setTransponderTurnaroundRatio( &getDsnDefaultTurnaroundRatios ); -// bodies.getBody( "Mars" )->getGroundStation( "MSL" )->setVehicleSystems( vehicleSystems ); -// -// // Set link ends for observation model -// LinkEnds linkEnds; -// linkEnds[ transmitter ] = groundStations[ 0 ]; -// linkEnds[ retransmitter ] = groundStations[ 1 ]; -// linkEnds[ receiver ] = groundStations[ 0 ]; -// -// // Generate one-way range model -// std::vector< std::string > perturbingBodies; -// perturbingBodies.push_back( "Earth" ); -// std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList; -// lightTimeCorrectionsList.push_back( -// std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( perturbingBodies ) ); -// -// std::shared_ptr< ObservationModel< 1, double, Time > > dsnNWayAveragedDopplerModel = -// observation_models::ObservationModelCreator< 1, double, Time >::createObservationModel( -// std::make_shared< observation_models::DsnNWayAveragedDopplerObservationSettings >( -// linkEnds, -// lightTimeCorrectionsList ) , bodies ); -// -// // Create parameter objects. -// std::shared_ptr< EstimatableParameterSet< double > > fullEstimatableParameterSet = -// createEstimatableParameters( bodies, stateEvaluationTime ); -// -// testObservationPartials< 1 >( -// dsnNWayAveragedDopplerModel, bodies, fullEstimatableParameterSet, linkEnds, -// dsn_n_way_averaged_doppler, 1.0E-4, false, true, 1000.0, parameterPerturbationMultipliers, -// getDsnNWayAveragedDopplerAncillarySettings( -// std::vector< FrequencyBands >{ x_band, x_band }, x_band, 7.0e9, 60.0, getRetransmissionDelays( initialEphemerisTime, 1 ) ), -// stateEvaluationTime ); -// } -} - - -BOOST_AUTO_TEST_SUITE_END( ) - - -}// namespace unit_tests - -}// namespace tudat From e98ef0a43243281075c75d1dd036f121d823ddb8 Mon Sep 17 00:00:00 2001 From: DominicDirkx Date: Wed, 18 Dec 2024 21:33:36 +0100 Subject: [PATCH 20/24] Update unitTestRadiationPressureEstimation.cpp --- .../orbit_determination/unitTestRadiationPressureEstimation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp b/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp index f5e4ecbd2a..6daa4653d4 100644 --- a/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp +++ b/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp @@ -250,7 +250,7 @@ BOOST_AUTO_TEST_CASE( test_RadiationPressurePartialsFromEstimation ) } else { - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( numericalValue, analyticalValue, toleranceStates ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( numericalValue, analyticalValue, ( toleranceStates * 2.5 ) ); } } else From 9b196aaab4acae94d87ee59e17d10b1b2bf263f4 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Thu, 19 Dec 2024 09:40:04 +0100 Subject: [PATCH 21/24] Correcting test tolerance --- .../tudat/support/observationPartialTestFunctions.h | 2 +- .../unitTestRadiationPressureEstimation.cpp | 8 ++------ 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/tests/include/tudat/support/observationPartialTestFunctions.h b/tests/include/tudat/support/observationPartialTestFunctions.h index dbeaa91863..3221e08371 100644 --- a/tests/include/tudat/support/observationPartialTestFunctions.h +++ b/tests/include/tudat/support/observationPartialTestFunctions.h @@ -205,7 +205,7 @@ void testObservationPartials( { runSimulation = false; } - + // Remove retransmission delay from the retransmitting reference link end: computation of multi-leg light currently doesn't support // retransmission delays at the reference link end std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > modifiedAncilliarySettings; diff --git a/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp b/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp index 6daa4653d4..a4507b41e0 100644 --- a/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp +++ b/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp @@ -244,24 +244,20 @@ BOOST_AUTO_TEST_CASE( test_RadiationPressurePartialsFromEstimation ) if( parameterIndex < 6 ) { // Modify tolernace for geometrically poor term - if( ( test == 1 && parameterIndex == 3 ) || ( test == 3 && parameterIndex == 4 ) ) + if( ( test == 1 && parameterIndex == 3 ) || ( test == 1 && parameterIndex == 6 ) || ( test == 3 && parameterIndex == 4 ) ) { TUDAT_CHECK_MATRIX_CLOSE_FRACTION( numericalValue, analyticalValue, ( toleranceStates * 20.0 ) ); } else { - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( numericalValue, analyticalValue, ( toleranceStates * 2.5 ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( numericalValue, analyticalValue, ( toleranceStates ) ); } } else { TUDAT_CHECK_MATRIX_CLOSE_FRACTION( numericalValue, analyticalValue, ( toleranceParameter * 5.0 ) ); } - -// Eigen::VectorXd ratio = ( numericalValue - analyticalValue ).cwiseQuotient( analyticalValue ); -// std::cout< Date: Thu, 19 Dec 2024 09:57:48 +0100 Subject: [PATCH 22/24] Adding extra test output --- .../unitTestRadiationPressureEstimation.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp b/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp index a4507b41e0..616ff4fef9 100644 --- a/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp +++ b/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp @@ -241,20 +241,27 @@ BOOST_AUTO_TEST_CASE( test_RadiationPressurePartialsFromEstimation ) numericalValue( parameterIndex ) -= 1.0; } + std::cout< Date: Thu, 19 Dec 2024 11:15:47 +0100 Subject: [PATCH 23/24] Corrected tests --- .../unitTestRadiationPressureEstimation.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp b/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp index 616ff4fef9..a03b76f556 100644 --- a/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp +++ b/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp @@ -250,18 +250,18 @@ BOOST_AUTO_TEST_CASE( test_RadiationPressurePartialsFromEstimation ) // Modify tolernace for geometrically poor term if( ( test == 1 && parameterIndex == 3 ) || ( test == 1 && parameterIndex == 6 ) || ( test == 3 && parameterIndex == 4 ) ) { - std::cout<<"Test A "<( toleranceStates * 20.0 )< Date: Thu, 19 Dec 2024 11:56:50 +0100 Subject: [PATCH 24/24] Test working all along --- .../unitTestRadiationPressureEstimation.cpp | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp b/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp index a03b76f556..b61ad18be9 100644 --- a/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp +++ b/tests/src/astro/orbit_determination/unitTestRadiationPressureEstimation.cpp @@ -241,30 +241,27 @@ BOOST_AUTO_TEST_CASE( test_RadiationPressurePartialsFromEstimation ) numericalValue( parameterIndex ) -= 1.0; } - std::cout<