diff --git a/include/tudat/astro/observation_models/frequencyOfArrivalDifferenceObservationModel.h b/include/tudat/astro/observation_models/frequencyOfArrivalDifferenceObservationModel.h new file mode 100644 index 000000000..973a510ac --- /dev/null +++ b/include/tudat/astro/observation_models/frequencyOfArrivalDifferenceObservationModel.h @@ -0,0 +1,192 @@ +/* Copyright (c) 2010-2019, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_FREQUENCYOFARRIVALDIFFERENCEOBSERVATIONMODEL_H +#define TUDAT_FREQUENCYOFARRIVALDIFFERENCEOBSERVATIONMODEL_H + +#include +#include + +#include "tudat/math/basic/coordinateConversions.h" +#include "tudat/astro/observation_models/lightTimeSolution.h" +#include "tudat/astro/observation_models/oneWayDopplerObservationModel.h + +namespace tudat +{ + +namespace observation_models +{ + +//! Class for simulating time of arrival difference observables. +template< typename ObservationScalarType = double, typename TimeType = double > +class FrequencyOfArrivalDifferenceObservationModel: public ObservationModel< 1, ObservationScalarType, TimeType > +{ +public: + + typedef Eigen::Matrix< ObservationScalarType, 6, 1 > StateType; + typedef Eigen::Matrix< ObservationScalarType, 6, 1 > PositionType; + + //! Constructor. + /*! + * Constructor, + * \param dopplerModelFirstReceiver Object to compute the light-time (including any corrections w.r.t. Euclidean case) + * between source and first receiver + * \param dopplerModelSecondReceiver Object to compute the light-time (including any corrections w.r.t. Euclidean case) + * between source and second receiver + * \param observationBiasCalculator Object for calculating system-dependent errors in the + * observable, i.e. deviations from the physically ideal observable between reference points (default none). + */ + FrequencyOfArrivalDifferenceObservationModel( + const LinkEnds linkEnds, + const std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > dopplerModelFirstReceiver, + const std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > dopplerModelSecondReceiver, + const std::shared_ptr< ObservationBias< 1 > > observationBiasCalculator = nullptr ): + ObservationModel< 1, ObservationScalarType, TimeType >( time_difference_of_arrival, linkEnds, observationBiasCalculator ), + dopplerModelFirstReceiver_( dopplerModelFirstReceiver ), dopplerModelSecondReceiver_( dopplerModelSecondReceiver ) { } + + //! Destructor + ~FrequencyOfArrivalDifferenceObservationModel( ){ } + + //! Function to compute ideal TOA difference observation at given time, between two transmitters. + /*! + * This function compute ideal TOA difference observation at a given time, between two transmitters. The time argument can be either the + * reception or transmission time (defined by linkEndAssociatedWithTime input). + * Note that this observable does include e.g. light-time corrections, which represent physically true corrections. + * It does not include e.g. system-dependent measurement. + * The times and states of the link ends are also returned in full precision (determined by class template + * arguments). These states and times are returned by reference. + * \param time Time at which observation is to be simulated + * \param linkEndAssociatedWithTime Link end at which given time is valid, i.e. link end for which associated time + * is kept constant (to input value) + * \param linkEndTimes List of times at each link end during observation (returned by reference). + * \param linkEndStates List of states at each link end during observation (returned by reference). + * \return Calculated TOA difference observable values. + */ + Eigen::Matrix< ObservationScalarType, 1, 1 > computeIdealObservationsWithLinkEndData( + const TimeType time, + const LinkEndType linkEndAssociatedWithTime, + std::vector< double >& linkEndTimes, + std::vector< Eigen::Matrix< double, 6, 1 > >& linkEndStates, + const std::shared_ptr< ObservationAncilliarySimulationSettings > ancilliarySetings = nullptr ) + + { +// Eigen::Matrix< ObservationScalarType, 6, 1 > firstReceiverState; +// Eigen::Matrix< ObservationScalarType, 6, 1 > secondReceiverState; +// Eigen::Matrix< ObservationScalarType, 6, 1 > transmitterState; +// +// // Compute light-times and receiver/transmitters states. +// ObservationScalarType lightTimeFirstReceiver; +// ObservationScalarType lightTimeSecondReceiver; +// +// switch( linkEndAssociatedWithTime ) +// { +// case transmitter: +// lightTimeFirstReceiver = dopplerModelFirstReceiver_->calculateLightTimeWithLinkEndsStates( +// firstReceiverState, transmitterState, time, false, ancilliarySetings ); +// lightTimeSecondReceiver = dopplerModelSecondReceiver_->calculateLightTimeWithLinkEndsStates( +// secondReceiverState, transmitterState, time, false, ancilliarySetings ); +// +// linkEndTimes.push_back( static_cast< double >( time ) ); +// linkEndTimes.push_back( static_cast< double >( time + lightTimeFirstReceiver ) ); +// linkEndTimes.push_back( static_cast< double >( time + lightTimeSecondReceiver ) ); +// +// break; +// case receiver1: +// lightTimeFirstReceiver = dopplerModelFirstReceiver_->calculateLightTimeWithLinkEndsStates( +// firstReceiverState, transmitterState, time, true, ancilliarySetings ); +// lightTimeSecondReceiver = dopplerModelSecondReceiver_->calculateLightTimeWithLinkEndsStates( +// secondReceiverState, transmitterState, time - lightTimeFirstReceiver, false, ancilliarySetings ); +// +// linkEndTimes.push_back( static_cast< double >( time - lightTimeFirstReceiver ) ); +// linkEndTimes.push_back( static_cast< double >( time ) ); +// linkEndTimes.push_back( static_cast< double >( time - lightTimeFirstReceiver + lightTimeSecondReceiver ) ); +// +// break; +// case receiver2: +// lightTimeSecondReceiver = dopplerModelSecondReceiver_->calculateLightTimeWithLinkEndsStates( +// secondReceiverState, transmitterState, time, true, ancilliarySetings ); +// lightTimeFirstReceiver = dopplerModelFirstReceiver_->calculateLightTimeWithLinkEndsStates( +// firstReceiverState, transmitterState, time - lightTimeSecondReceiver, false, ancilliarySetings ); +// linkEndTimes.push_back( static_cast< double >( time - lightTimeSecondReceiver ) ); +// linkEndTimes.push_back( static_cast< double >( time - lightTimeSecondReceiver + lightTimeFirstReceiver ) ); +// linkEndTimes.push_back( static_cast< double >( time ) ); +// +// break; +// default: +// throw std::runtime_error( "Error when computing TOA difference observable, did not recognize reference link end " + getLinkEndTypeString( linkEndAssociatedWithTime ) ); +// } +// +// // Set link end times and states. +// linkEndTimes.clear( ); +// linkEndStates.clear( ); +// +// linkEndStates.push_back( transmitterState.template cast< double >( ) ); +// linkEndStates.push_back( firstReceiverState.template cast< double >( ) ); +// linkEndStates.push_back( secondReceiverState.template cast< double >( ) ); +// +// // Return observable +// return ( Eigen::Matrix< ObservationScalarType, 1, 1 >( ) << lightTimeSecondReceiver - lightTimeFirstReceiver ).finished( ); + } + + //! Function to get the object to calculate light time between first transmitter and receiver. + /*! + * Function to get the object to calculate light time between first transmitter and receiver. + * \return Object to calculate light time between first transmitter and receiver. + */ + std::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > getDopplerModelFirstReceiver( ) + { + return dopplerModelFirstReceiver_; + } + + //! Function to get the object to calculate light time between second transmitter and receiver. + /*! + * Function to get the object to calculate light time between second transmitter and receiver. + * \return Object to calculate light time between second transmitter and receiver. + */ + std::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > getDopplerModelSecondReceiver( ) + { + return dopplerModelSecondReceiver_; + } + + LinkEnds getFirstLinkEnds( ) + { + LinkEnds firstLinkEnds; + firstLinkEnds[ transmitter ] = this->linkEnds_[ transmitter ]; + firstLinkEnds[ receiver ] = this->linkEnds_[ receiver1 ]; + return firstLinkEnds; + } + + LinkEnds getSecondLinkEnds( ) + { + LinkEnds secondLinkEnds; + secondLinkEnds[ transmitter ] = this->linkEnds_[ transmitter ]; + secondLinkEnds[ receiver ] = this->linkEnds_[ receiver2 ]; + return secondLinkEnds; + } +private: + + //! Object to calculate light time between the first transmitter and receiver. + /*! + * Object to calculate light time between the first transmitter and receiver, including possible corrections from troposphere, relativistic corrections, etc. + */ + std::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > dopplerModelFirstReceiver_; + + //! Object to calculate light time between the second transmitter and receiver. + /*! + * Object to calculate light time between the second transmitter and receiver, including possible corrections from troposphere, relativistic corrections, etc. + */ + std::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > dopplerModelSecondReceiver_; +}; + +} // namespace observation_models + +} // namespace tudat + +#endif // TUDAT_FREQUENCYOFARRIVALDIFFERENCEOBSERVATIONMODEL_H diff --git a/include/tudat/astro/observation_models/linkTypeDefs.h b/include/tudat/astro/observation_models/linkTypeDefs.h index 44667aeb3..79f36654c 100644 --- a/include/tudat/astro/observation_models/linkTypeDefs.h +++ b/include/tudat/astro/observation_models/linkTypeDefs.h @@ -42,9 +42,11 @@ enum LinkEndType reflector4 = 4, retransmitter4 = reflector4, receiver = 5, + receiver1 = receiver, observed_body = 6, transmitter2 = 7, - observer = 8 + observer = 8, + receiver2 = 9 }; ////! Typedef for the identifier of a given link-end (body and reference points) diff --git a/include/tudat/astro/observation_models/observableTypes.h b/include/tudat/astro/observation_models/observableTypes.h index ba058b306..277ccd0a7 100644 --- a/include/tudat/astro/observation_models/observableTypes.h +++ b/include/tudat/astro/observation_models/observableTypes.h @@ -43,7 +43,9 @@ enum ObservableType dsn_one_way_averaged_doppler = 12, dsn_n_way_averaged_doppler = 13, doppler_measured_frequency = 14, - dsn_n_way_range = 15 + dsn_n_way_range = 15, + time_difference_of_arrival = 16, + frequency_difference_of_arrival = 17 }; diff --git a/include/tudat/astro/observation_models/timeOfArrivalDifferenceObservationModel.h b/include/tudat/astro/observation_models/timeOfArrivalDifferenceObservationModel.h new file mode 100644 index 000000000..b17ecc58e --- /dev/null +++ b/include/tudat/astro/observation_models/timeOfArrivalDifferenceObservationModel.h @@ -0,0 +1,192 @@ +/* Copyright (c) 2010-2019, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ + +#ifndef TUDAT_TIMEOFARRIVALDIFFERENCEOBSERVATIONMODEL_H +#define TUDAT_TIMEOFARRIVALDIFFERENCEOBSERVATIONMODEL_H + +#include +#include + +#include "tudat/math/basic/coordinateConversions.h" +#include "tudat/astro/observation_models/lightTimeSolution.h" +#include "tudat/astro/observation_models/observationModel.h" + +namespace tudat +{ + +namespace observation_models +{ + +//! Class for simulating time of arrival difference observables. +template< typename ObservationScalarType = double, typename TimeType = double > +class TimeOfArrivalDifferenceObservationModel: public ObservationModel< 1, ObservationScalarType, TimeType > +{ +public: + + typedef Eigen::Matrix< ObservationScalarType, 6, 1 > StateType; + typedef Eigen::Matrix< ObservationScalarType, 6, 1 > PositionType; + + //! Constructor. + /*! + * Constructor, + * \param lightTimeCalculatorFirstReceiver Object to compute the light-time (including any corrections w.r.t. Euclidean case) + * between source and first receiver + * \param lightTimeCalculatorSecondReceiver Object to compute the light-time (including any corrections w.r.t. Euclidean case) + * between source and second receiver + * \param observationBiasCalculator Object for calculating system-dependent errors in the + * observable, i.e. deviations from the physically ideal observable between reference points (default none). + */ + TimeOfArrivalDifferenceObservationModel( + const LinkEnds linkEnds, + const std::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > lightTimeCalculatorFirstReceiver, + const std::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > lightTimeCalculatorSecondReceiver, + const std::shared_ptr< ObservationBias< 1 > > observationBiasCalculator = nullptr ): + ObservationModel< 1, ObservationScalarType, TimeType >( time_difference_of_arrival, linkEnds, observationBiasCalculator ), + lightTimeCalculatorFirstReceiver_( lightTimeCalculatorFirstReceiver ), lightTimeCalculatorSecondReceiver_( lightTimeCalculatorSecondReceiver ) { } + + //! Destructor + ~TimeOfArrivalDifferenceObservationModel( ){ } + + //! Function to compute ideal TOA difference observation at given time, between two transmitters. + /*! + * This function compute ideal TOA difference observation at a given time, between two transmitters. The time argument can be either the + * reception or transmission time (defined by linkEndAssociatedWithTime input). + * Note that this observable does include e.g. light-time corrections, which represent physically true corrections. + * It does not include e.g. system-dependent measurement. + * The times and states of the link ends are also returned in full precision (determined by class template + * arguments). These states and times are returned by reference. + * \param time Time at which observation is to be simulated + * \param linkEndAssociatedWithTime Link end at which given time is valid, i.e. link end for which associated time + * is kept constant (to input value) + * \param linkEndTimes List of times at each link end during observation (returned by reference). + * \param linkEndStates List of states at each link end during observation (returned by reference). + * \return Calculated TOA difference observable values. + */ + Eigen::Matrix< ObservationScalarType, 1, 1 > computeIdealObservationsWithLinkEndData( + const TimeType time, + const LinkEndType linkEndAssociatedWithTime, + std::vector< double >& linkEndTimes, + std::vector< Eigen::Matrix< double, 6, 1 > >& linkEndStates, + const std::shared_ptr< ObservationAncilliarySimulationSettings > ancilliarySetings = nullptr ) + + { + Eigen::Matrix< ObservationScalarType, 6, 1 > firstReceiverState; + Eigen::Matrix< ObservationScalarType, 6, 1 > secondReceiverState; + Eigen::Matrix< ObservationScalarType, 6, 1 > transmitterState; + + // Compute light-times and receiver/transmitters states. + ObservationScalarType lightTimeFirstReceiver; + ObservationScalarType lightTimeSecondReceiver; + + switch( linkEndAssociatedWithTime ) + { + case transmitter: + lightTimeFirstReceiver = lightTimeCalculatorFirstReceiver_->calculateLightTimeWithLinkEndsStates( + firstReceiverState, transmitterState, time, false, ancilliarySetings ); + lightTimeSecondReceiver = lightTimeCalculatorSecondReceiver_->calculateLightTimeWithLinkEndsStates( + secondReceiverState, transmitterState, time, false, ancilliarySetings ); + + linkEndTimes.push_back( static_cast< double >( time ) ); + linkEndTimes.push_back( static_cast< double >( time + lightTimeFirstReceiver ) ); + linkEndTimes.push_back( static_cast< double >( time + lightTimeSecondReceiver ) ); + + break; + case receiver1: + lightTimeFirstReceiver = lightTimeCalculatorFirstReceiver_->calculateLightTimeWithLinkEndsStates( + firstReceiverState, transmitterState, time, true, ancilliarySetings ); + lightTimeSecondReceiver = lightTimeCalculatorSecondReceiver_->calculateLightTimeWithLinkEndsStates( + secondReceiverState, transmitterState, time - lightTimeFirstReceiver, false, ancilliarySetings ); + + linkEndTimes.push_back( static_cast< double >( time - lightTimeFirstReceiver ) ); + linkEndTimes.push_back( static_cast< double >( time ) ); + linkEndTimes.push_back( static_cast< double >( time - lightTimeFirstReceiver + lightTimeSecondReceiver ) ); + + break; + case receiver2: + lightTimeSecondReceiver = lightTimeCalculatorSecondReceiver_->calculateLightTimeWithLinkEndsStates( + secondReceiverState, transmitterState, time, true, ancilliarySetings ); + lightTimeFirstReceiver = lightTimeCalculatorFirstReceiver_->calculateLightTimeWithLinkEndsStates( + firstReceiverState, transmitterState, time - lightTimeSecondReceiver, false, ancilliarySetings ); + linkEndTimes.push_back( static_cast< double >( time - lightTimeSecondReceiver ) ); + linkEndTimes.push_back( static_cast< double >( time - lightTimeSecondReceiver + lightTimeFirstReceiver ) ); + linkEndTimes.push_back( static_cast< double >( time ) ); + + break; + default: + throw std::runtime_error( "Error when computing TOA difference observable, did not recognize reference link end " + getLinkEndTypeString( linkEndAssociatedWithTime ) ); + } + + // Set link end times and states. + linkEndTimes.clear( ); + linkEndStates.clear( ); + + linkEndStates.push_back( transmitterState.template cast< double >( ) ); + linkEndStates.push_back( firstReceiverState.template cast< double >( ) ); + linkEndStates.push_back( secondReceiverState.template cast< double >( ) ); + + // Return observable + return ( Eigen::Matrix< ObservationScalarType, 1, 1 >( ) << lightTimeSecondReceiver - lightTimeFirstReceiver ).finished( ); + } + + //! Function to get the object to calculate light time between first transmitter and receiver. + /*! + * Function to get the object to calculate light time between first transmitter and receiver. + * \return Object to calculate light time between first transmitter and receiver. + */ + std::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > getLightTimeCalculatorFirstReceiver( ) + { + return lightTimeCalculatorFirstReceiver_; + } + + //! Function to get the object to calculate light time between second transmitter and receiver. + /*! + * Function to get the object to calculate light time between second transmitter and receiver. + * \return Object to calculate light time between second transmitter and receiver. + */ + std::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > getLightTimeCalculatorSecondReceiver( ) + { + return lightTimeCalculatorSecondReceiver_; + } + + LinkEnds getFirstLinkEnds( ) + { + LinkEnds firstLinkEnds; + firstLinkEnds[ transmitter ] = this->linkEnds_[ transmitter ]; + firstLinkEnds[ receiver ] = this->linkEnds_[ receiver1 ]; + return firstLinkEnds; + } + + LinkEnds getSecondLinkEnds( ) + { + LinkEnds secondLinkEnds; + secondLinkEnds[ transmitter ] = this->linkEnds_[ transmitter ]; + secondLinkEnds[ receiver ] = this->linkEnds_[ receiver2 ]; + return secondLinkEnds; + } +private: + + //! Object to calculate light time between the first transmitter and receiver. + /*! + * Object to calculate light time between the first transmitter and receiver, including possible corrections from troposphere, relativistic corrections, etc. + */ + std::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > lightTimeCalculatorFirstReceiver_; + + //! Object to calculate light time between the second transmitter and receiver. + /*! + * Object to calculate light time between the second transmitter and receiver, including possible corrections from troposphere, relativistic corrections, etc. + */ + std::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > lightTimeCalculatorSecondReceiver_; +}; + +} // namespace observation_models + +} // namespace tudat + +#endif // TUDAT_TIMEOFARRIVALDIFFERENCEOBSERVATIONMODEL_H diff --git a/include/tudat/simulation/estimation_setup/createObservationModel.h b/include/tudat/simulation/estimation_setup/createObservationModel.h index 992d487ee..9b85d9587 100644 --- a/include/tudat/simulation/estimation_setup/createObservationModel.h +++ b/include/tudat/simulation/estimation_setup/createObservationModel.h @@ -1961,12 +1961,6 @@ class ObservationModelCreator< 1, ObservationScalarType, TimeType > 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 ) { @@ -2008,6 +2002,53 @@ class ObservationModelCreator< 1, ObservationScalarType, TimeType > break; } + case time_difference_of_arrival: + { + // Check consistency input. + if( linkEnds.size( ) != 3 ) + { + std::string errorMessage = + "Error when making TOA observation model, " + + std::to_string( linkEnds.size( ) ) + " link ends found"; + throw std::runtime_error( errorMessage ); + } + if( linkEnds.count( receiver1 ) == 0 ) + { + throw std::runtime_error( "Error when making TOA observation model, no receiver1 found" ); + } + if( linkEnds.count( receiver2 ) == 0 ) + { + throw std::runtime_error( "Error when making TOA observation model, no receiver2 found" ); + } + if( linkEnds.count( transmitter ) == 0 ) + { + throw std::runtime_error( "Error when making TOA observation model, no transmitter found" ); + } + + 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< OneWayDifferencedRangeObservationModel< + ObservationScalarType, TimeType > >( + linkEnds, + createLightTimeCalculator< ObservationScalarType, TimeType >( + linkEnds, transmitter, receiver1, + bodies, topLevelObservableType, observationSettings->lightTimeCorrectionsList_, + observationSettings->lightTimeConvergenceCriteria_ ), + createLightTimeCalculator< ObservationScalarType, TimeType >( + linkEnds, transmitter, receiver2, + bodies, topLevelObservableType, observationSettings->lightTimeCorrectionsList_, + observationSettings->lightTimeConvergenceCriteria_ ), + observationBias ); + + break; + } case n_way_range: { // Check consistency input.