From 79373dab5f23e78a388512874b1ed29f757860a0 Mon Sep 17 00:00:00 2001 From: Valerio Date: Tue, 12 Nov 2024 15:54:43 +0100 Subject: [PATCH 1/9] Fix and enhance observation model setup - Included necessary headers for TwoWayDopplerObservationModel and OneWayDopplerObservationModel in include/tudat/astro/observation_models/dopplerMeasuredFrequencyObservationModel.h (To be verified) - Corrected file name typo. - Corrected function name typo ("Obsevatio" -> "observation"). - Added dsnNWayRangeObservationSettings function. Changes: - modified: include/tudat/astro/observation_models/dopplerMeasuredFrequencyObservationModel.h - modified: include/tudat/astro/observation_models/observationModel.h - modified: include/tudat/interface/json/support/keys.h - modified: include/tudat/simulation/estimation_setup/createDirectObservationPartials.h - modified: include/tudat/simulation/estimation_setup/createDopplerPartials.h - modified: include/tudat/simulation/estimation_setup/createNWayRangePartials.h - renamed: include/tudat/simulation/estimation_setup/createObsevationBiasPartial.h -> include/tudat/simulation/estimation_setup/createObservationBiasPartial.h - modified: include/tudat/simulation/estimation_setup/createObservationModel.h - modified: include/tudat/simulation/estimation_setup/createObservationPartials.h - modified: include/tudat/simulation/estimation_setup/orbitDeterminationManager.h - modified: src/interface/json/estimation/observation.cpp - modified: src/interface/json/support/keys.cpp - modified: tests/src/interface/json/inputs/unitTestObservation/observationOutput.json - modified: tests/src/interface/json/unitTestObservation.cpp --- ...dopplerMeasuredFrequencyObservationModel.h | 219 +- .../observation_models/observationModel.h | 22 + include/tudat/interface/json/support/keys.h | 176 +- .../createDirectObservationPartials.h | 360 +- .../estimation_setup/createDopplerPartials.h | 285 +- .../createNWayRangePartials.h | 191 +- ...rtial.h => createObservationBiasPartial.h} | 0 .../estimation_setup/createObservationModel.h | 4371 ++++++++++------- .../createObservationPartials.h | 989 ++-- .../orbitDeterminationManager.h | 2 +- src/interface/json/estimation/observation.cpp | 646 +-- src/interface/json/support/keys.cpp | 213 +- .../observationOutput.json | 4 +- .../interface/json/unitTestObservation.cpp | 131 +- 14 files changed, 4348 insertions(+), 3261 deletions(-) rename include/tudat/simulation/estimation_setup/{createObsevationBiasPartial.h => createObservationBiasPartial.h} (100%) diff --git a/include/tudat/astro/observation_models/dopplerMeasuredFrequencyObservationModel.h b/include/tudat/astro/observation_models/dopplerMeasuredFrequencyObservationModel.h index f7f4ab7641..643413c392 100644 --- a/include/tudat/astro/observation_models/dopplerMeasuredFrequencyObservationModel.h +++ b/include/tudat/astro/observation_models/dopplerMeasuredFrequencyObservationModel.h @@ -16,11 +16,12 @@ #include #include -#include "tudat/simulation/simulation.h" - +#include "tudat/astro/observation_models/nWayRangeObservationModel.h" #include "tudat/astro/observation_models/observableTypes.h" #include "tudat/astro/observation_models/observationFrequencies.h" -#include "tudat/astro/observation_models/nWayRangeObservationModel.h" +#include "tudat/astro/observation_models/oneWayDopplerObservationModel.h" // Include this header +#include "tudat/astro/observation_models/twoWayDopplerObservationModel.h" // Include this header +#include "tudat/simulation/simulation.h" namespace tudat { @@ -28,113 +29,133 @@ namespace tudat namespace observation_models { - template< typename ObservationScalarType = double, typename TimeType = Time > -class DopplerMeasuredFrequencyObservationModel : public ObservationModel< 1, ObservationScalarType, TimeType > +class DopplerMeasuredFrequencyObservationModel + : public ObservationModel< 1, ObservationScalarType, TimeType > { -public: + public: typedef Eigen::Matrix< ObservationScalarType, 6, 1 > StateType; /*! Constructor * Constructor for Doppler observation model. * \param linkEnds Link ends for observation model. - * \param rangeObservationModel N-way range observation model associated with the Doppler measurement. - * \param transmittingFrequencyCalculator Object returning the transmitted frequency as the transmitting link end - * \param turnaroundRatio Function returning the turnaround ratio for given uplink and downlink bands - * \param observationBiasCalculator Object for calculating system-dependent errors in the observation + * \param rangeObservationModel N-way range observation model associated with the Doppler + * measurement. \param transmittingFrequencyCalculator Object returning the transmitted + * frequency as the transmitting link end \param turnaroundRatio Function returning the + * turnaround ratio for given uplink and downlink bands \param observationBiasCalculator Object + * for calculating system-dependent errors in the observation */ DopplerMeasuredFrequencyObservationModel( - const LinkEnds& linkEnds, - const std::shared_ptr< TwoWayDopplerObservationModel > twoWayDopplerModel, - const std::shared_ptr< ground_stations::StationFrequencyInterpolator > transmittingFrequencyCalculator, - const std::function< double(observation_models::FrequencyBands uplinkBand, observation_models::FrequencyBands downlinkBand) >& turnaroundRatio, - const std::shared_ptr< ObservationBias< 1 > > observationBiasCalculator = nullptr, - const std::map< LinkEndType, std::shared_ptr< ground_stations::GroundStationState > > groundStationStates = - std::map< LinkEndType, std::shared_ptr< ground_stations::GroundStationState > >( ) ) : - ObservationModel< 1, ObservationScalarType, TimeType >(doppler_measured_frequency, linkEnds, observationBiasCalculator), - twoWayDopplerModel_(twoWayDopplerModel), - numberOfLinkEnds_(linkEnds.size()), - transmittingFrequencyCalculator_(transmittingFrequencyCalculator), - turnaroundRatio_(turnaroundRatio), - stationStates_( groundStationStates ) + const LinkEnds& linkEnds, + const std::shared_ptr< TwoWayDopplerObservationModel< ObservationScalarType, + TimeType > > twoWayDopplerModel, + const std::shared_ptr< ground_stations::StationFrequencyInterpolator > + transmittingFrequencyCalculator, + const std::function< double( observation_models::FrequencyBands uplinkBand, + observation_models::FrequencyBands downlinkBand ) >& + turnaroundRatio, + const std::shared_ptr< ObservationBias< 1 > > observationBiasCalculator = nullptr, + const std::map< LinkEndType, std::shared_ptr< ground_stations::GroundStationState > > + groundStationStates = std::map< + LinkEndType, + std::shared_ptr< ground_stations::GroundStationState > >( ) ) : + ObservationModel< 1, ObservationScalarType, TimeType >( doppler_measured_frequency, + linkEnds, + observationBiasCalculator ), + twoWayDopplerModel_( twoWayDopplerModel ), numberOfLinkEnds_( linkEnds.size( ) ), + transmittingFrequencyCalculator_( transmittingFrequencyCalculator ), + turnaroundRatio_( turnaroundRatio ), stationStates_( groundStationStates ) { - if (numberOfLinkEnds_ != 3) + if( numberOfLinkEnds_ != 3 ) { throw std::runtime_error( - "Error when defining Doppler Measured Frequency Model: model allows exactly 3 link ends, " + - std::to_string(numberOfLinkEnds_) + "were selected."); + "Error when defining Doppler Measured Frequency Model: model allows exactly 3 " + "link ends, " + + std::to_string( numberOfLinkEnds_ ) + "were selected." ); } uplinkDopplerModel_ = twoWayDopplerModel_->getUplinkDopplerCalculator( ); downlinkDopplerModel_ = twoWayDopplerModel_->getDownlinkDopplerCalculator( ); - std::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > - uplinkLightTimeCalculator = uplinkDopplerModel_->getLightTimeCalculator(); - std::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > - downlinkLightTimeCalculator = downlinkDopplerModel_->getLightTimeCalculator(); - - std::vector< std::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > > - lightTimeCalculators = { uplinkLightTimeCalculator, downlinkLightTimeCalculator }; - lighTimeCalculator_ = std::make_shared< observation_models::MultiLegLightTimeCalculator< ObservationScalarType, TimeType > > - (lightTimeCalculators ); + std::shared_ptr< + observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > + uplinkLightTimeCalculator = uplinkDopplerModel_->getLightTimeCalculator( ); + std::shared_ptr< + observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > + downlinkLightTimeCalculator = downlinkDopplerModel_->getLightTimeCalculator( ); + + std::vector< std::shared_ptr< + observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > > + lightTimeCalculators = { uplinkLightTimeCalculator, downlinkLightTimeCalculator }; + lighTimeCalculator_ = std::make_shared< + observation_models::MultiLegLightTimeCalculator< ObservationScalarType, + TimeType > >( + lightTimeCalculators ); terrestrialTimeScaleConverter_ = earth_orientation::createDefaultTimeConverter( ); } //! Destructor - ~DopplerMeasuredFrequencyObservationModel() { } + ~DopplerMeasuredFrequencyObservationModel( ) { } /*! * Function to compute Measured Frequency for a doppler observation model * \param time Time at which observable is to be evaluated. - * \param linkEndAssociatedWithTime Link end at which given time is valid, i.e. link end for which associated time - * is kept constant (to input value) - * \param linkEndTimes List of times at each link end during observation. Set in this mothod. - * \param linkEndStates List of states at each link end during observation. Set in this method. - * \param ancillarySettings Ancillary settings for the observation model - * \return Measured Frequency for a doppler observation model + * \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. Set in this mothod. \param linkEndStates List of states at + * each link end during observation. Set in this method. \param ancillarySettings Ancillary + * settings for the observation model \return Measured Frequency for a doppler observation model */ 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 > ancillarySettings = nullptr) + const TimeType time, + const LinkEndType linkEndAssociatedWithTime, + std::vector< double >& linkEndTimes, + std::vector< Eigen::Matrix< double, 6, 1 > >& linkEndStates, + const std::shared_ptr< ObservationAncilliarySimulationSettings > ancillarySettings = + nullptr ) { // Check if selected reference link end is valid - if (linkEndAssociatedWithTime != receiver) + if( linkEndAssociatedWithTime != receiver ) { throw std::runtime_error( - "Error when computing Doppler measured frequency observables: the selected reference link end (" + - getLinkEndTypeString(linkEndAssociatedWithTime) + ") is not valid. Must be the receiver."); + "Error when computing Doppler measured frequency observables: the selected " + "reference link end (" + + getLinkEndTypeString( linkEndAssociatedWithTime ) + + ") is not valid. Must be the receiver." ); } // Check if ancillary settings were provided - if (ancillarySettings == nullptr) + if( ancillarySettings == nullptr ) { throw std::runtime_error( - "Error when simulating Doppler Measured Frequency observable; no ancillary settings found. "); + "Error when simulating Doppler Measured Frequency observable; no ancillary " + "settings found. " ); } std::vector< FrequencyBands > frequencyBands; try { - frequencyBands = convertDoubleVectorToFrequencyBands(ancillarySettings->getAncilliaryDoubleVectorData(frequency_bands)); + frequencyBands = convertDoubleVectorToFrequencyBands( + ancillarySettings->getAncilliaryDoubleVectorData( frequency_bands ) ); } - catch (std::runtime_error& caughtException) + catch( std::runtime_error& caughtException ) { throw std::runtime_error( - "Error when retrieving ancillary settings for Doppler measured frequency observable: " + - std::string(caughtException.what())); + "Error when retrieving ancillary settings for Doppler measured frequency " + "observable: " + + std::string( caughtException.what( ) ) ); } - if (frequencyBands.size() != numberOfLinkEnds_ - 1) + if( frequencyBands.size( ) != numberOfLinkEnds_ - 1 ) { throw std::runtime_error( - "Error when retrieving frequency bands ancillary settings: " - "size (" + std::to_string(frequencyBands.size()) + ") is inconsistent with number of links (" + - std::to_string(numberOfLinkEnds_ - 1) + ")."); + "Error when retrieving frequency bands ancillary settings: " + "size (" + + std::to_string( frequencyBands.size( ) ) + + ") is inconsistent with number of links (" + + std::to_string( numberOfLinkEnds_ - 1 ) + ")." ); } FrequencyBands uplinkBand = frequencyBands.at( 0 ); @@ -142,67 +163,89 @@ class DopplerMeasuredFrequencyObservationModel : public ObservationModel< 1, Obs // Calculate the light time TimeType lightTime = lighTimeCalculator_->calculateLightTimeWithLinkEndsStates( - time, linkEndAssociatedWithTime, linkEndTimes, linkEndStates, ancillarySettings ); + time, linkEndAssociatedWithTime, linkEndTimes, linkEndStates, ancillarySettings ); // Get the time when the signal left the transmitter Eigen::Vector3d nominalTransmittingStationState = - ( stationStates_.count( transmitter ) == 0 ) ? - Eigen::Vector3d::Zero( ) : stationStates_.at( transmitter )->getNominalCartesianPosition( ); + ( stationStates_.count( transmitter ) == 0 ) + ? Eigen::Vector3d::Zero( ) + : stationStates_.at( transmitter )->getNominalCartesianPosition( ); TimeType transmitterTime = time - lightTime; TimeType transmitterUtcTime = terrestrialTimeScaleConverter_->getCurrentTime< TimeType >( - basic_astrodynamics::tdb_scale, basic_astrodynamics::utc_scale, transmitterTime, nominalTransmittingStationState ); + basic_astrodynamics::tdb_scale, + basic_astrodynamics::utc_scale, + transmitterTime, + nominalTransmittingStationState ); // Get the frequency of the transmitter ObservationScalarType transmittedFrequency = - transmittingFrequencyCalculator_->getTemplatedCurrentFrequency( transmitterUtcTime ); + transmittingFrequencyCalculator_ + ->getTemplatedCurrentFrequency< ObservationScalarType, TimeType >( + transmitterUtcTime ); // Calculate the Doppler observable - ObservationScalarType dopplerMultiplicationTerm = twoWayDopplerModel_->getMultiplicationTerm(); - ObservationScalarType twoWayDoppler = twoWayDopplerModel_->computeIdealObservationsWithLinkEndData( - time, linkEndAssociatedWithTime, linkEndTimes, linkEndStates, ancillarySettings)( 0, 0 ) / dopplerMultiplicationTerm; + ObservationScalarType dopplerMultiplicationTerm = + twoWayDopplerModel_->getMultiplicationTerm( ); + ObservationScalarType twoWayDoppler = + twoWayDopplerModel_->computeIdealObservationsWithLinkEndData( + time, + linkEndAssociatedWithTime, + linkEndTimes, + linkEndStates, + ancillarySettings )( 0, 0 ) / + dopplerMultiplicationTerm; ObservationScalarType receivedFrequency = - ( transmittedFrequency * ( mathematical_constants::getFloatingInteger< ObservationScalarType >( 1 ) + twoWayDoppler ) ) * - turnaroundRatio_(uplinkBand, downlinkBand); - - Eigen::Matrix< ObservationScalarType, 1, 1 > observation = (Eigen::Matrix< ObservationScalarType, 1, 1 >() << receivedFrequency).finished(); + ( transmittedFrequency * + ( mathematical_constants::getFloatingInteger< ObservationScalarType >( 1 ) + + twoWayDoppler ) ) * + turnaroundRatio_( uplinkBand, downlinkBand ); + + Eigen::Matrix< ObservationScalarType, 1, 1 > observation = + ( Eigen::Matrix< ObservationScalarType, 1, 1 >( ) << receivedFrequency ) + .finished( ); return observation; } -private: - + private: // Doppler observation model associated with the measurement - std::shared_ptr< TwoWayDopplerObservationModel< ObservationScalarType, TimeType > > twoWayDopplerModel_; + std::shared_ptr< TwoWayDopplerObservationModel< ObservationScalarType, TimeType > > + twoWayDopplerModel_; // Number of link ends unsigned int numberOfLinkEnds_; // Object returning the transmitted frequency as the transmitting link end - std::shared_ptr< ground_stations::StationFrequencyInterpolator > transmittingFrequencyCalculator_; + std::shared_ptr< ground_stations::StationFrequencyInterpolator > + transmittingFrequencyCalculator_; // Function returning the turnaround ratio for given uplink and downlink bands - std::function< double(FrequencyBands uplinkBand, FrequencyBands downlinkBand) > turnaroundRatio_; + std::function< double( FrequencyBands uplinkBand, FrequencyBands downlinkBand ) > + turnaroundRatio_; // Individual doppler observation models - std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > uplinkDopplerModel_; + std::shared_ptr< + observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > + uplinkDopplerModel_; - std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > downlinkDopplerModel_; + std::shared_ptr< + observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > + downlinkDopplerModel_; // Light time calculator - std::shared_ptr< observation_models::MultiLegLightTimeCalculator< ObservationScalarType, TimeType > > lighTimeCalculator_; + std::shared_ptr< + observation_models::MultiLegLightTimeCalculator< ObservationScalarType, TimeType > > + lighTimeCalculator_; - std::shared_ptr< earth_orientation::TerrestrialTimeScaleConverter > terrestrialTimeScaleConverter_; + std::shared_ptr< earth_orientation::TerrestrialTimeScaleConverter > + terrestrialTimeScaleConverter_; std::map< LinkEndType, std::shared_ptr< ground_stations::GroundStationState > > stationStates_; - }; +} // namespace observation_models +} // namespace tudat -} // namespace observation_models - -} // namespace tudat - - -#endif //TUDAT_DOPPLERMEASUREDFREQUENCYOBSERVATIONMODEL_H +#endif // TUDAT_DOPPLERMEASUREDFREQUENCYOBSERVATIONMODEL_H diff --git a/include/tudat/astro/observation_models/observationModel.h b/include/tudat/astro/observation_models/observationModel.h index c51c1274e9..08c75deb46 100644 --- a/include/tudat/astro/observation_models/observationModel.h +++ b/include/tudat/astro/observation_models/observationModel.h @@ -293,6 +293,28 @@ getDsnNWayAveragedDopplerAncillarySettings( return ancillarySettings; } +inline std::shared_ptr< ObservationAncilliarySimulationSettings > getDsnNWayRangeAncillarySettings( + const std::vector< FrequencyBands > &frequencyBands, + const double referenceFrequency, + const double lowestRangingComponent, + const std::vector< double > linkEndsDelays = std::vector< double >( ) ) + +{ + std::shared_ptr< ObservationAncilliarySimulationSettings > ancillarySettings = + std::make_shared< ObservationAncilliarySimulationSettings >( ); + + ancillarySettings->setAncilliaryDoubleData( sequential_range_lowest_ranging_component, + lowestRangingComponent ); + ancillarySettings->setAncilliaryDoubleData( sequential_range_reference_frequency, + referenceFrequency ); + + ancillarySettings->setAncilliaryDoubleVectorData( + frequency_bands, convertFrequencyBandsToDoubleVector( frequencyBands ) ); + ancillarySettings->setAncilliaryDoubleVectorData( link_ends_delays, linkEndsDelays ); + + return ancillarySettings; +} + inline std::shared_ptr< ObservationAncilliarySimulationSettings > getDopplerMeasuredFrequencyAncilliarySettings( const std::vector< FrequencyBands > &frequencyBands ) { diff --git a/include/tudat/interface/json/support/keys.h b/include/tudat/interface/json/support/keys.h index c09716cea5..44ade893ff 100644 --- a/include/tudat/interface/json/support/keys.h +++ b/include/tudat/interface/json/support/keys.h @@ -20,8 +20,7 @@ namespace json_interface { //! Special keys (used internally by json_interface, can't be used in JSON files). -struct SpecialKeys -{ +struct SpecialKeys { static const std::string root; static const char dot; static const std::string up; @@ -33,8 +32,7 @@ struct SpecialKeys }; //! Keys recognised by json_interface. -struct Keys -{ +struct Keys { static const std::string simulationType; static const std::string initialEpoch; static const std::string finalEpoch; @@ -42,8 +40,7 @@ struct Keys static const std::string globalFrameOrientation; static const std::string spice; - struct Spice - { + struct Spice { static const std::string useStandardKernels; static const std::string alternativeKernels; static const std::string kernels; @@ -53,8 +50,7 @@ struct Keys }; static const std::string bodies; - struct Body - { + struct Body { static const std::string useDefaultSettings; static const std::string initialState; static const std::string initialStateOrigin; @@ -62,8 +58,7 @@ struct Keys static const std::string rotationalState; static const std::string referenceArea; - struct State - { + struct State { static const std::string type; // Cartesian static const std::string x; @@ -102,8 +97,7 @@ struct Keys }; static const std::string aerodynamics; - struct Aerodynamics - { + struct Aerodynamics { static const std::string coefficientsType; static const std::string referenceLength; static const std::string referenceArea; @@ -127,8 +121,7 @@ struct Keys }; static const std::string atmosphere; - struct Atmosphere - { + struct Atmosphere { static const std::string type; static const std::string densityScaleHeight; static const std::string constantTemperature; @@ -143,8 +136,7 @@ struct Keys }; static const std::string ephemeris; - struct Ephemeris - { + struct Ephemeris { static const std::string type; static const std::string frameOrigin; static const std::string frameOrientation; @@ -170,8 +162,7 @@ struct Keys }; static const std::string gravityField; - struct GravityField - { + struct GravityField { static const std::string type; static const std::string gravitationalParameter; static const std::string referenceRadius; @@ -187,8 +178,7 @@ struct Keys }; static const std::string rotationModel; - struct RotationModel - { + struct RotationModel { static const std::string type; static const std::string originalFrame; static const std::string targetFrame; @@ -200,8 +190,7 @@ struct Keys }; static const std::string shapeModel; - struct ShapeModel - { + struct ShapeModel { static const std::string type; static const std::string radius; static const std::string equatorialRadius; @@ -209,8 +198,7 @@ struct Keys }; static const std::string radiationPressure; - struct RadiationPressure - { + struct RadiationPressure { static const std::string type; static const std::string sourceBody; static const std::string occultingBodies; @@ -219,8 +207,7 @@ struct Keys }; static const std::string gravityFieldVariation; - struct GravityFieldVariation - { + struct GravityFieldVariation { static const std::string bodyDeformationType; static const std::string deformingBodies; static const std::string loveNumbers; @@ -233,17 +220,14 @@ struct Keys }; static const std::string groundStation; - struct GroundStation - { + struct GroundStation { static const std::string stationPosition; static const std::string positionElementType; static const std::string stationName; }; }; - - struct Variable - { + struct Variable { static const std::string type; static const std::string dependentVariableType; static const std::string body; @@ -252,10 +236,10 @@ struct Keys static const std::string componentIndices; static const std::string useAccelerationNorm; static const std::string accelerationType; -// static const std::string bodyUndergoingAcceleration; + // static const std::string bodyUndergoingAcceleration; static const std::string bodyExertingAcceleration; static const std::string torqueType; -// static const std::string bodyUndergoingTorque; + // static const std::string bodyUndergoingTorque; static const std::string bodyExertingTorque; static const std::string baseFrame; static const std::string targetFrame; @@ -267,8 +251,7 @@ struct Keys }; static const std::string parametersToEstimate; - struct Parameter - { + struct Parameter { static const std::string parameterType; static const std::string associatedBody; static const std::string secondaryIdentifier; @@ -290,7 +273,7 @@ struct Keys static const std::string linkEnds; static const std::string referenceLinkEnd; - static const std::string componentsToEstimate; + static const std::string componentsToEstimate; static const std::string degree; static const std::string orders; @@ -298,8 +281,7 @@ struct Keys }; static const std::string observations; - struct Observation - { + struct Observation { static const std::string observableType; static const std::string lightTimeCorrectionSettingsList; static const std::string biasSettings; @@ -309,7 +291,7 @@ struct Keys static const std::string constantIntegrationTime; - static const std::string oneWayRangeObsevationSettings; + static const std::string oneWayRangeObservationSettings; static const std::string retransmissionTimes; static const std::string uplinkOneWayDopplerSettings; @@ -331,8 +313,7 @@ struct Keys }; static const std::string estimationSettings; - struct Estimation - { + struct Estimation { static const std::string inverseAprioriCovariance; static const std::string reintegrateEquationsOnFirstIteration; static const std::string reintegrateVariationalEquations; @@ -349,8 +330,7 @@ struct Keys static const std::string dataWeights; }; - struct ObservationBias - { + struct ObservationBias { static const std::string biasType; static const std::string multipleBiasesList; static const std::string constantBias; @@ -358,12 +338,10 @@ struct Keys static const std::string arcWiseBiasList; static const std::string arcStartTimes; static const std::string referenceLinkEnd; - }; static const std::string propagators; - struct Propagator - { + struct Propagator { static const std::string integratedStateType; static const std::string initialStates; static const std::string bodiesToPropagate; @@ -373,8 +351,7 @@ struct Keys static const std::string centralBodies; static const std::string accelerations; - struct Acceleration - { + struct Acceleration { static const std::string type; static const std::string maximumDegree; static const std::string maximumOrder; @@ -393,11 +370,9 @@ struct Keys static const std::string sineAcceleration; static const std::string cosineAcceleration; - struct Thrust - { + struct Thrust { static const std::string direction; - struct Direction - { + struct Direction { static const std::string type; static const std::string relativeBody; static const std::string colinearWithVelocity; @@ -405,8 +380,7 @@ struct Keys }; static const std::string magnitude; - struct Magnitude - { + struct Magnitude { static const std::string type; static const std::string originID; static const std::string constantMagnitude; @@ -423,23 +397,20 @@ struct Keys }; static const std::string massRateModels; - struct MassRateModel - { + struct MassRateModel { static const std::string type; static const std::string useAllThrustModels; static const std::string associatedThrustSource; }; static const std::string torques; - struct Torque - { + struct Torque { static const std::string type; }; }; static const std::string termination; - struct Termination - { + struct Termination { static const std::string anyOf; static const std::string allOf; static const std::string variable; @@ -448,8 +419,7 @@ struct Keys }; static const std::string integrator; - struct Integrator - { + struct Integrator { static const std::string type; static const std::string initialTime; static const std::string stepSize; @@ -472,10 +442,8 @@ struct Keys static const std::string maximumOrder; }; - struct Interpolation - { - struct DataMap - { + struct Interpolation { + struct DataMap { static const std::string map; static const std::string file; static const std::string independentVariableValues; @@ -483,8 +451,7 @@ struct Keys static const std::string dependentVariableFirstDerivativeValues; }; - struct Interpolator - { + struct Interpolator { static const std::string type; static const std::string lookupScheme; static const std::string useLongDoubleTimeStep; @@ -493,14 +460,12 @@ struct Keys static const std::string lagrangeBoundaryHandling; }; - struct DataInterpolation - { + struct DataInterpolation { static const std::string data; static const std::string interpolator; }; - struct ModelInterpolation - { + struct ModelInterpolation { static const std::string initialTime; static const std::string finalTime; static const std::string timeStep; @@ -509,8 +474,7 @@ struct Keys }; static const std::string xport; - struct Export - { + struct Export { static const std::string file; static const std::string variables; static const std::string header; @@ -522,8 +486,7 @@ struct Keys }; static const std::string options; - struct Options - { + struct Options { static const std::string notifyOnPropagationStart; static const std::string notifyOnPropagationTermination; static const std::string statePrintInterval; @@ -534,7 +497,6 @@ struct Keys }; }; - //! Get the int-value of an int-convertible key. /*! * @copybrief indexFromKey @@ -545,18 +507,19 @@ int indexFromKey( const std::string& key ); //! Class for specifying a key pat used to access data from `json` objects. /*! - * Class for specifying a key path (key.subkey.subsubkey ...) used to access data from `json` objects. + * Class for specifying a key path (key.subkey.subsubkey ...) used to access data from `json` + * objects. */ class KeyPath : public std::vector< std::string > { -public: + public: //! Empty constructor. KeyPath( ) : std::vector< std::string >( ) { } //! Constructor from vector. KeyPath( const std::vector< std::string >& vector ) : std::vector< std::string >( ) { - for ( const std::string key : vector ) + for( const std::string key: vector ) { push_back( key ); } @@ -565,7 +528,8 @@ class KeyPath : public std::vector< std::string > //! Constructor with a single key path string representation. /*! * Constructor with a single key path string representation. - * \param keyPathStringRepresentation The key path string representation, such as "key", "key.subkey", "key[1]". + * \param keyPathStringRepresentation The key path string representation, such as "key", + * "key.subkey", "key[1]". */ KeyPath( const std::string& keyPathStringRepresentation ); @@ -590,7 +554,7 @@ class KeyPath : public std::vector< std::string > */ bool isAbsolute( ) const { - if ( size( ) == 0 ) + if( size( ) == 0 ) { return false; } @@ -600,102 +564,102 @@ class KeyPath : public std::vector< std::string > //! Get the canonical representation of the key path. /*! * Get the canonical representation of the key path, optionally relative to \p basePath. - * This method is used to construct absolute paths, also navigating up and removing `SpecialKeys::up`. - * \param basePath Key path with respect to which the path is to be constructed. - * \return Canonical representation of the key path. + * This method is used to construct absolute paths, also navigating up and removing + * `SpecialKeys::up`. \param basePath Key path with respect to which the path is to be + * constructed. \return Canonical representation of the key path. */ KeyPath canonical( const KeyPath& basePath ) const; }; //! String representation for `KeyPath`, as key.subkey.vectorIndex.subsubkey ... -std::ostream& operator << ( std::ostream& stringRepresentation, KeyPath const& keyPath ); +std::ostream& operator<<( std::ostream& stringRepresentation, KeyPath const& keyPath ); -inline KeyPath operator / ( KeyPath path1, const KeyPath& path2 ) +inline KeyPath operator/( KeyPath path1, const KeyPath& path2 ) { - for ( std::string subkey : path2 ) + for( std::string subkey: path2 ) { path1.push_back( subkey ); } return path1; } -inline KeyPath operator / ( const KeyPath& path, const std::string& str ) +inline KeyPath operator/( const KeyPath& path, const std::string& str ) { return path / KeyPath( str ); } -inline KeyPath operator / ( const std::string& str, const KeyPath& path ) +inline KeyPath operator/( const std::string& str, const KeyPath& path ) { return KeyPath( str ) / path; } -inline KeyPath operator / ( const std::string& str1, const std::string& str2 ) +inline KeyPath operator/( const std::string& str1, const std::string& str2 ) { return KeyPath( str1 ) / KeyPath( str2 ); } -inline KeyPath operator / ( const KeyPath& path, const char* str ) +inline KeyPath operator/( const KeyPath& path, const char* str ) { return path / KeyPath( str ); } -inline KeyPath operator / ( const char* str, const KeyPath& path ) +inline KeyPath operator/( const char* str, const KeyPath& path ) { return KeyPath( str ) / path; } -inline KeyPath operator / ( const KeyPath& path, const unsigned int vectorIndex ) +inline KeyPath operator/( const KeyPath& path, const unsigned int vectorIndex ) { return path / KeyPath( vectorIndex ); } -inline KeyPath operator / ( const unsigned int vectorIndex, const KeyPath& path ) +inline KeyPath operator/( const unsigned int vectorIndex, const KeyPath& path ) { return KeyPath( vectorIndex ) / path; } -inline KeyPath operator / ( const std::string& str, const unsigned int vectorIndex ) +inline KeyPath operator/( const std::string& str, const unsigned int vectorIndex ) { return KeyPath( str ) / vectorIndex; } -inline KeyPath operator / ( const unsigned int vectorIndex, const std::string& str ) +inline KeyPath operator/( const unsigned int vectorIndex, const std::string& str ) { return vectorIndex / KeyPath( str ); } -inline KeyPath operator / ( const KeyPath& path, const int vectorIndex ) +inline KeyPath operator/( const KeyPath& path, const int vectorIndex ) { return path / KeyPath( vectorIndex ); } -inline KeyPath operator / ( const int vectorIndex, const KeyPath& path ) +inline KeyPath operator/( const int vectorIndex, const KeyPath& path ) { return KeyPath( vectorIndex ) / path; } -inline KeyPath operator / ( const std::string& str, const int vectorIndex ) +inline KeyPath operator/( const std::string& str, const int vectorIndex ) { return KeyPath( str ) / vectorIndex; } -inline KeyPath operator / ( const int vectorIndex, const std::string& str ) +inline KeyPath operator/( const int vectorIndex, const std::string& str ) { return vectorIndex / KeyPath( str ); } -inline KeyPath operator / ( const char* str1, const std::string& str2 ) +inline KeyPath operator/( const char* str1, const std::string& str2 ) { return KeyPath( str1 ) / str2; } -inline KeyPath operator / ( const std::string& str1, const char* str2 ) +inline KeyPath operator/( const std::string& str1, const char* str2 ) { return str1 / KeyPath( str2 ); } -} // namespace json_interface +} // namespace json_interface -} // namespace tudat +} // namespace tudat -#endif // TUDAT_JSONINTERFACE_KEYS_H +#endif // TUDAT_JSONINTERFACE_KEYS_H diff --git a/include/tudat/simulation/estimation_setup/createDirectObservationPartials.h b/include/tudat/simulation/estimation_setup/createDirectObservationPartials.h index ee63479e58..22ad7af5ca 100644 --- a/include/tudat/simulation/estimation_setup/createDirectObservationPartials.h +++ b/include/tudat/simulation/estimation_setup/createDirectObservationPartials.h @@ -11,33 +11,30 @@ #ifndef TUDAT_CREATEDIRECTOBSERVATIONPARTIALS_H #define TUDAT_CREATEDIRECTOBSERVATIONPARTIALS_H -#include +#include #include - #include +#include -#include - -#include "tudat/math/interpolators/interpolator.h" - -#include "tudat/astro/observation_models/corrections/lightTimeCorrection.h" -#include "tudat/simulation/estimation_setup/createCartesianStatePartials.h" -#include "tudat/astro/orbit_determination/estimatable_parameters/initialTranslationalState.h" -#include "tudat/astro/orbit_determination/observation_partials/directObservationPartial.h" -#include "tudat/astro/orbit_determination/observation_partials/angularPositionPartial.h" -#include "tudat/astro/orbit_determination/observation_partials/oneWayRangePartial.h" -#include "tudat/astro/orbit_determination/observation_partials/oneWayDopplerPartial.h" #include "tudat/astro/observation_models.h" +#include "tudat/astro/observation_models/angularPositionObservationModel.h" +#include "tudat/astro/observation_models/corrections/lightTimeCorrection.h" #include "tudat/astro/observation_models/observableTypes.h" #include "tudat/astro/observation_models/observationModel.h" -#include "tudat/astro/observation_models/oneWayRangeObservationModel.h" -#include "tudat/astro/observation_models/angularPositionObservationModel.h" #include "tudat/astro/observation_models/oneWayDopplerObservationModel.h" +#include "tudat/astro/observation_models/oneWayRangeObservationModel.h" +#include "tudat/astro/orbit_determination/estimatable_parameters/initialTranslationalState.h" +#include "tudat/astro/orbit_determination/observation_partials/angularPositionPartial.h" +#include "tudat/astro/orbit_determination/observation_partials/directObservationPartial.h" +#include "tudat/astro/orbit_determination/observation_partials/oneWayDopplerPartial.h" +#include "tudat/astro/orbit_determination/observation_partials/oneWayRangePartial.h" +#include "tudat/math/interpolators/interpolator.h" +#include "tudat/simulation/estimation_setup/createCartesianStatePartials.h" +#include "tudat/simulation/estimation_setup/createClockPartials.h" #include "tudat/simulation/estimation_setup/createLightTimeCorrectionPartials.h" -#include "tudat/simulation/estimation_setup/createPositionPartialScaling.h" +#include "tudat/simulation/estimation_setup/createObservationBiasPartial.h" #include "tudat/simulation/estimation_setup/createObservationModel.h" -#include "tudat/simulation/estimation_setup/createClockPartials.h" -#include "tudat/simulation/estimation_setup/createObsevationBiasPartial.h" +#include "tudat/simulation/estimation_setup/createPositionPartialScaling.h" namespace tudat { @@ -45,43 +42,49 @@ namespace tudat namespace observation_partials { - //! Function to generate observation partial wrt a single parameter. /*! - * Function to generate observation partial wrt a single parameter, for a single link ends (which must contain a - * transmitter and receiever linkEndType). - * \tparam ParameterType Type of parameter (double for size 1, VectorXd for larger size). - * \param oneWayLinkEnds Link ends (transmitter and receiever) for which observation partials are to be calculated - * (i.e. for which observation observations are to be processed). - * \param bodies List of all bodies, for creating observation partial. - * \param parameterToEstimate Object of current parameter that is to be estimated. - * \param positionPartialScaler Object scale position partials to observation partials for current link ends. - * \param lightTimeCorrectionPartialObjects List of light time correction partials to be used (empty by default) - * \return observation partial object wrt a single parameter (is nullptr if no parameter dependency exists). + * Function to generate observation partial wrt a single parameter, for a single link ends (which + * must contain a transmitter and receiever linkEndType). \tparam ParameterType Type of parameter + * (double for size 1, VectorXd for larger size). \param oneWayLinkEnds Link ends (transmitter and + * receiever) for which observation partials are to be calculated (i.e. for which observation + * observations are to be processed). \param bodies List of all bodies, for creating observation + * partial. \param parameterToEstimate Object of current parameter that is to be estimated. \param + * positionPartialScaler Object scale position partials to observation partials for current link + * ends. \param lightTimeCorrectionPartialObjects List of light time correction partials to be used + * (empty by default) \return observation partial object wrt a single parameter (is nullptr if no + * parameter dependency exists). */ template< typename ParameterType, int ObservationSize > std::shared_ptr< ObservationPartial< ObservationSize > > createObservationPartialWrtParameter( const observation_models::LinkEnds oneWayLinkEnds, const simulation_setup::SystemOfBodies& bodies, - const std::shared_ptr< estimatable_parameters::EstimatableParameter< ParameterType > > parameterToEstimate, - const std::shared_ptr< DirectPositionPartialScaling< ObservationSize > > positionPartialScaler, + const std::shared_ptr< estimatable_parameters::EstimatableParameter< ParameterType > > + parameterToEstimate, + const std::shared_ptr< DirectPositionPartialScaling< ObservationSize > > + positionPartialScaler, const std::vector< std::shared_ptr< observation_partials::LightTimeCorrectionPartial > >& - lightTimeCorrectionPartialObjects = - std::vector< std::shared_ptr< observation_partials::LightTimeCorrectionPartial > >( ) ) + lightTimeCorrectionPartialObjects = std::vector< + std::shared_ptr< observation_partials::LightTimeCorrectionPartial > >( ) ) { std::shared_ptr< ObservationPartial< ObservationSize > > observationPartial; { - std::map< observation_models::LinkEndType, std::shared_ptr< CartesianStatePartial > > positionPartials = - createCartesianStatePartialsWrtParameter( oneWayLinkEnds, bodies, parameterToEstimate ); + std::map< observation_models::LinkEndType, std::shared_ptr< CartesianStatePartial > > + positionPartials = createCartesianStatePartialsWrtParameter( + oneWayLinkEnds, bodies, parameterToEstimate ); - // Create observation partials if any position partials are created (i.e. if any dependency exists). - std::shared_ptr< DirectObservationPartial< ObservationSize > > testObservationPartial = + // Create observation partials if any position partials are created (i.e. if any dependency + // exists). + std::shared_ptr< DirectObservationPartial< ObservationSize > > testObservationPartial = std::make_shared< DirectObservationPartial< ObservationSize > >( - positionPartialScaler, positionPartials, parameterToEstimate->getParameterName( ), - lightTimeCorrectionPartialObjects ); - if( positionPartials.size( ) > 0 || testObservationPartial->getNumberOfLighTimeCorrectionPartialsFunctions( ) > 0 || - testObservationPartial->useLinkIndependentPartials()) + positionPartialScaler, + positionPartials, + parameterToEstimate->getParameterName( ), + lightTimeCorrectionPartialObjects ); + if( positionPartials.size( ) > 0 || + testObservationPartial->getNumberOfLighTimeCorrectionPartialsFunctions( ) > 0 || + testObservationPartial->useLinkIndependentPartials( ) ) { observationPartial = testObservationPartial; } @@ -93,38 +96,43 @@ std::shared_ptr< ObservationPartial< ObservationSize > > createObservationPartia //! Function to generate observation partial wrt a position of a body. /*! - * Function to generate observation partial wrt a position of a body, for a single link ends (which must contain a - * transmitter and receiever linkEndType). - * \param oneWayLinkEnds Link ends (transmitter and receiever) for which observation partials are to be calculated - * (i.e. for which observation observations are to be processed). - * \param bodies List of all bodies, for creating observation partial. - * \param bodyToEstimate Name of body wrt position of which a partial is to be created. - * \param positionPartialScaler Object scale position partials to observation partials for current link ends. - * \param lightTimeCorrectionPartialObjects List of light time correction partials to be used (empty by default) - * \return observation partial object wrt a current position of a body (is nullptr if no parameter dependency exists). + * Function to generate observation partial wrt a position of a body, for a single link ends (which + * must contain a transmitter and receiever linkEndType). \param oneWayLinkEnds Link ends + * (transmitter and receiever) for which observation partials are to be calculated (i.e. for which + * observation observations are to be processed). \param bodies List of all bodies, for creating + * observation partial. \param bodyToEstimate Name of body wrt position of which a partial is to be + * created. \param positionPartialScaler Object scale position partials to observation partials for + * current link ends. \param lightTimeCorrectionPartialObjects List of light time correction + * partials to be used (empty by default) \return observation partial object wrt a current position + * of a body (is nullptr if no parameter dependency exists). */ template< int ObservationSize > std::shared_ptr< ObservationPartial< ObservationSize > > createObservationPartialWrtBodyPosition( const observation_models::LinkEnds oneWayLinkEnds, const simulation_setup::SystemOfBodies& bodies, const std::string bodyToEstimate, - const std::shared_ptr< DirectPositionPartialScaling< ObservationSize > > positionPartialScaler, + const std::shared_ptr< DirectPositionPartialScaling< ObservationSize > > + positionPartialScaler, const std::vector< std::shared_ptr< observation_partials::LightTimeCorrectionPartial > >& - lightTimeCorrectionPartialObjects = - std::vector< std::shared_ptr< observation_partials::LightTimeCorrectionPartial > >( ) ) + lightTimeCorrectionPartialObjects = std::vector< + std::shared_ptr< observation_partials::LightTimeCorrectionPartial > >( ) ) { // Create position partials of link ends for current body position - std::map< observation_models::LinkEndType, std::shared_ptr< CartesianStatePartial > > positionPartials = - createCartesianStatePartialsWrtBodyState( oneWayLinkEnds, bodies, bodyToEstimate ); + std::map< observation_models::LinkEndType, std::shared_ptr< CartesianStatePartial > > + positionPartials = createCartesianStatePartialsWrtBodyState( + oneWayLinkEnds, bodies, bodyToEstimate ); - // Create observation partials if any position partials are created (i.e. if any dependency exists). + // Create observation partials if any position partials are created (i.e. if any dependency + // exists). std::shared_ptr< DirectObservationPartial< ObservationSize > > observationPartial; if( positionPartials.size( ) > 0 ) { observationPartial = std::make_shared< DirectObservationPartial< ObservationSize > >( - positionPartialScaler, positionPartials, std::make_pair( - estimatable_parameters::initial_body_state, std::make_pair( bodyToEstimate, "" ) ), - lightTimeCorrectionPartialObjects ); + positionPartialScaler, + positionPartials, + std::make_pair( estimatable_parameters::initial_body_state, + std::make_pair( bodyToEstimate, "" ) ), + lightTimeCorrectionPartialObjects ); } // Return observation partial object (nullptr if no dependency exists). @@ -133,68 +141,76 @@ std::shared_ptr< ObservationPartial< ObservationSize > > createObservationPartia //! Function to generate observation partial wrt a rotational state of a body. /*! - * Function to generate observation partial wrt a rotational state of a body, for a single link ends (which must contain a - * transmitter and receiever linkEndType). - * \param oneWayLinkEnds Link ends (transmitter and receiever) for which observation partials are to be calculated - * (i.e. for which observation observations are to be processed). - * \param bodies List of all bodies, for creating observation partial. - * \param bodyToEstimate Name of body wrt rotational state of which a partial is to be created. - * \param positionPartialScaler Object scale position partials to observation partials for current link ends. - * \param lightTimeCorrectionPartialObjects List of light time correction partials to be used (empty by default) - * \return observation partial object wrt a current rotational state of a body (is nullptr if no parameter dependency exists). + * Function to generate observation partial wrt a rotational state of a body, for a single link + * ends (which must contain a transmitter and receiever linkEndType). \param oneWayLinkEnds Link + * ends (transmitter and receiever) for which observation partials are to be calculated (i.e. for + * which observation observations are to be processed). \param bodies List of all bodies, for + * creating observation partial. \param bodyToEstimate Name of body wrt rotational state of which a + * partial is to be created. \param positionPartialScaler Object scale position partials to + * observation partials for current link ends. \param lightTimeCorrectionPartialObjects List of + * light time correction partials to be used (empty by default) \return observation partial object + * wrt a current rotational state of a body (is nullptr if no parameter dependency exists). */ template< int ObservationSize > -std::shared_ptr< ObservationPartial< ObservationSize > > createObservationPartialWrtBodyRotationalState( +std::shared_ptr< ObservationPartial< ObservationSize > > +createObservationPartialWrtBodyRotationalState( const observation_models::LinkEnds oneWayLinkEnds, const simulation_setup::SystemOfBodies& bodies, const std::string bodyToEstimate, - const std::shared_ptr< DirectPositionPartialScaling< ObservationSize > > positionPartialScaler, + const std::shared_ptr< DirectPositionPartialScaling< ObservationSize > > + positionPartialScaler, const std::vector< std::shared_ptr< observation_partials::LightTimeCorrectionPartial > >& - lightTimeCorrectionPartialObjects ) + lightTimeCorrectionPartialObjects ) { // Create position partials of link ends for current body position - std::map< observation_models::LinkEndType, std::shared_ptr< CartesianStatePartial > > positionPartials = - createCartesianStatePartialsWrtBodyRotationalState( oneWayLinkEnds, bodies, bodyToEstimate ); + std::map< observation_models::LinkEndType, std::shared_ptr< CartesianStatePartial > > + positionPartials = createCartesianStatePartialsWrtBodyRotationalState( + oneWayLinkEnds, bodies, bodyToEstimate ); - // Create observation partials if any position partials are created (i.e. if any dependency exists). + // Create observation partials if any position partials are created (i.e. if any dependency + // exists). std::shared_ptr< DirectObservationPartial< ObservationSize > > observationPartial; if( positionPartials.size( ) > 0 ) { observationPartial = std::make_shared< DirectObservationPartial< ObservationSize > >( - positionPartialScaler, positionPartials, std::make_pair( - estimatable_parameters::initial_body_state, std::make_pair( bodyToEstimate, "" ) ), - lightTimeCorrectionPartialObjects ); + positionPartialScaler, + positionPartials, + std::make_pair( estimatable_parameters::initial_body_state, + std::make_pair( bodyToEstimate, "" ) ), + lightTimeCorrectionPartialObjects ); } // Return observation partial object (nullptr if no dependency exists). return observationPartial; } - - //! Function to generate observation partials and associated scaler for single link ends. /*! - * Function to generate observation partials and associated scaler for all parameters that are to be estimated, - * for a single link ends set. - * The set of parameters and bodies that are to be estimated, as well as the set of link ends - * (each of which must contain a transmitter and receiever linkEndType) that are to be used. - * \param oneWayLinkEnds Link ends (transmitter and receiever) for which observation partials are to be calculated - * (i.e. for which observation observations are to be processed). - * \param bodies List of all bodies, for creating observation partials. - * \param parametersToEstimate Set of parameters that are to be estimated - * \param lightTimeCorrections List of light time correction partials to be used (empty by default) - * \param useBiasPartials Boolean to denote whether this function should create partials w.r.t. observation bias parameters - * \return Set of observation partials with associated indices in complete vector of parameters that are estimated, - * representing all necessary observation partials of a single link end, and ObservationPartial< ObservationSize >, object, used for - * scaling the position partial members of all ObservationPartials in link end. + * Function to generate observation partials and associated scaler for all parameters that are to + * be estimated, for a single link ends set. The set of parameters and bodies that are to be + * estimated, as well as the set of link ends (each of which must contain a transmitter and + * receiever linkEndType) that are to be used. \param oneWayLinkEnds Link ends (transmitter and + * receiever) for which observation partials are to be calculated (i.e. for which observation + * observations are to be processed). \param bodies List of all bodies, for creating observation + * partials. \param parametersToEstimate Set of parameters that are to be estimated \param + * lightTimeCorrections List of light time correction partials to be used (empty by default) \param + * useBiasPartials Boolean to denote whether this function should create partials w.r.t. observation + * bias parameters \return Set of observation partials with associated indices in complete vector of + * parameters that are estimated, representing all necessary observation partials of a single link + * end, and ObservationPartial< ObservationSize >, object, used for scaling the position partial + * members of all ObservationPartials in link end. */ -template< typename ParameterType, int ObservationSize, typename TimeType = double > -std::pair< std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< ObservationSize > > >, -std::shared_ptr< PositionPartialScaling > > +template< typename ParameterType, int ObservationSize, typename TimeType = double > +std::pair< + std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< ObservationSize > > >, + std::shared_ptr< PositionPartialScaling > > createSingleLinkObservationPartials( - const std::shared_ptr< observation_models::ObservationModel< ObservationSize, ParameterType, TimeType > > observationModel, + const std::shared_ptr< + observation_models::ObservationModel< ObservationSize, ParameterType, TimeType > > + observationModel, const simulation_setup::SystemOfBodies& bodies, - const std::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > parametersToEstimate, + const std::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > + parametersToEstimate, const bool isPartialForDifferencedObservable = false, const bool isPartialForConcatenatedObservable = false ) { @@ -203,13 +219,15 @@ createSingleLinkObservationPartials( std::vector< std::shared_ptr< observation_models::LightTimeCorrection > > lightTimeCorrections; { - auto fullLightTimeCorrections= + auto fullLightTimeCorrections = observation_models::getLightTimeCorrections( observationModel ); if( fullLightTimeCorrections.size( ) > 1 ) { lightTimeCorrections = fullLightTimeCorrections.at( 0 ); - throw std::runtime_error( "Error when creatin direct observation partial, light time corrections list is of incorrect size." ); + throw std::runtime_error( + "Error when creatin direct observation partial, light time corrections list is " + "of incorrect size." ); } else if( fullLightTimeCorrections.size( ) == 1 ) { @@ -217,45 +235,58 @@ createSingleLinkObservationPartials( } } - std::vector< std::shared_ptr< observation_partials::LightTimeCorrectionPartial > > lightTimeCorrectionPartialObjects; + std::vector< std::shared_ptr< observation_partials::LightTimeCorrectionPartial > > + lightTimeCorrectionPartialObjects; if( lightTimeCorrections.size( ) > 0 ) { - lightTimeCorrectionPartialObjects = observation_partials::createLightTimeCorrectionPartials( lightTimeCorrections ); + lightTimeCorrectionPartialObjects = + observation_partials::createLightTimeCorrectionPartials( lightTimeCorrections ); } // Create scaling object, to be used for all observation partials in current link end. - // std::shared_ptr< ObservationPartialScalingCreator< ObservationSize > > observationPartialScalingCreator = + // std::shared_ptr< ObservationPartialScalingCreator< ObservationSize > > + // observationPartialScalingCreator = // std::make_shared< ObservationPartialScalingCreator< ObservationSize > >; std::shared_ptr< DirectPositionPartialScaling< ObservationSize > > positionScaling = ObservationPartialScalingCreator< ObservationSize >::createPositionScalingObject( - oneWayLinkEnds, observableType, bodies, observationModel ); + oneWayLinkEnds, observableType, bodies, observationModel ); // Initialize vector index variables. int currentIndex = 0; std::pair< int, int > currentPair = std::pair< int, int >( currentIndex, 1 ); - std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< ObservationSize > > > observationPartials; + std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< ObservationSize > > > + observationPartials; std::vector< std::shared_ptr< estimatable_parameters::EstimatableParameter< - Eigen::Matrix< ParameterType, Eigen::Dynamic, 1 > > > > initialDynamicalParameters = - parametersToEstimate->getEstimatedInitialStateParameters( ); + Eigen::Matrix< ParameterType, Eigen::Dynamic, 1 > > > > + initialDynamicalParameters = + parametersToEstimate->getEstimatedInitialStateParameters( ); - // Iterate over list of bodies of which the partials of the accelerations acting on them are required. + // Iterate over list of bodies of which the partials of the accelerations acting on them are + // required. for( unsigned int i = 0; i < initialDynamicalParameters.size( ); i++ ) { std::string acceleratedBody; - if( initialDynamicalParameters.at( i )->getParameterName( ).first == estimatable_parameters::initial_body_state || - initialDynamicalParameters.at( i )->getParameterName( ).first == estimatable_parameters::arc_wise_initial_body_state ) + if( initialDynamicalParameters.at( i )->getParameterName( ).first == + estimatable_parameters::initial_body_state || + initialDynamicalParameters.at( i )->getParameterName( ).first == + estimatable_parameters::arc_wise_initial_body_state ) { acceleratedBody = initialDynamicalParameters.at( i )->getParameterName( ).second.first; // Create position observation partial for current body std::shared_ptr< ObservationPartial< ObservationSize > > currentObservationPartial = createObservationPartialWrtBodyPosition< ObservationSize >( - oneWayLinkEnds, bodies, acceleratedBody, positionScaling, lightTimeCorrectionPartialObjects ); - - // Check if partial is non-null (i.e. whether dependency exists between current observable and current body) + oneWayLinkEnds, + bodies, + acceleratedBody, + positionScaling, + lightTimeCorrectionPartialObjects ); + + // Check if partial is non-null (i.e. whether dependency exists between current + // observable and current body) if( currentObservationPartial != nullptr ) { // Add partial to the list. @@ -266,16 +297,22 @@ createSingleLinkObservationPartials( // Increment current index by size of body initial state (6). currentIndex += 6; } - else if( initialDynamicalParameters.at( i )->getParameterName( ).first == estimatable_parameters::initial_rotational_body_state ) + else if( initialDynamicalParameters.at( i )->getParameterName( ).first == + estimatable_parameters::initial_rotational_body_state ) { acceleratedBody = initialDynamicalParameters.at( i )->getParameterName( ).second.first; // Create position observation partial for current body std::shared_ptr< ObservationPartial< ObservationSize > > currentObservationPartial = createObservationPartialWrtBodyRotationalState< ObservationSize >( - oneWayLinkEnds, bodies, acceleratedBody, positionScaling, lightTimeCorrectionPartialObjects ); - - // Check if partial is non-null (i.e. whether dependency exists between current observable and current body) + oneWayLinkEnds, + bodies, + acceleratedBody, + positionScaling, + lightTimeCorrectionPartialObjects ); + + // Check if partial is non-null (i.e. whether dependency exists between current + // observable and current body) if( currentObservationPartial != nullptr ) { // Add partial to the list. @@ -288,26 +325,33 @@ createSingleLinkObservationPartials( } else { - throw std::runtime_error( "Error when making observation partials, could not identify parameter " + - std::to_string( initialDynamicalParameters.at( i )->getParameterName( ).first ) ); + throw std::runtime_error( + "Error when making observation partials, could not identify parameter " + + std::to_string( + initialDynamicalParameters.at( i )->getParameterName( ).first ) ); } } // Iterate over all double parameters that are to be estimated. - std::map< int, std::shared_ptr< estimatable_parameters::EstimatableParameter< double > > > doubleParametersToEstimate = - parametersToEstimate->getDoubleParameters( ); - for( std::map< int, std::shared_ptr< estimatable_parameters::EstimatableParameter< double > > >::iterator - parameterIterator = - doubleParametersToEstimate.begin( ); parameterIterator != doubleParametersToEstimate.end( ); parameterIterator++ ) + std::map< int, std::shared_ptr< estimatable_parameters::EstimatableParameter< double > > > + doubleParametersToEstimate = parametersToEstimate->getDoubleParameters( ); + for( std::map< int, + std::shared_ptr< estimatable_parameters::EstimatableParameter< double > > >:: + iterator parameterIterator = doubleParametersToEstimate.begin( ); + parameterIterator != doubleParametersToEstimate.end( ); + parameterIterator++ ) { // Create position observation partial for current parameter std::shared_ptr< ObservationPartial< ObservationSize > > currentObservationPartial = createObservationPartialWrtParameter< double, ObservationSize >( - oneWayLinkEnds, bodies, parameterIterator->second, - positionScaling, lightTimeCorrectionPartialObjects ); - + oneWayLinkEnds, + bodies, + parameterIterator->second, + positionScaling, + lightTimeCorrectionPartialObjects ); - // Check if partial is non-nullptr (i.e. whether dependency exists between current observable and current parameter) + // Check if partial is non-nullptr (i.e. whether dependency exists between current + // observable and current parameter) if( currentObservationPartial != nullptr ) { // Add partial to the list. @@ -317,32 +361,55 @@ createSingleLinkObservationPartials( } // Iterate over all vector parameters that are to be estimated. - std::map< int, std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > > - vectorParametersToEstimate = - parametersToEstimate->getVectorParameters( ); - for( std::map< int, std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > >::iterator - parameterIterator = vectorParametersToEstimate.begin( ); parameterIterator != vectorParametersToEstimate.end( ); parameterIterator++ ) + std::map< int, + std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > > + vectorParametersToEstimate = parametersToEstimate->getVectorParameters( ); + for( std::map< int, + std::shared_ptr< estimatable_parameters::EstimatableParameter< + Eigen::VectorXd > > >::iterator parameterIterator = + vectorParametersToEstimate.begin( ); + parameterIterator != vectorParametersToEstimate.end( ); + parameterIterator++ ) { - // Create observation partial for current parameter + // Create observation partial for current parameter std::shared_ptr< ObservationPartial< ObservationSize > > currentObservationPartial; - if( !isParameterObservationLinkProperty( parameterIterator->second->getParameterName( ).first ) ) + if( !isParameterObservationLinkProperty( + parameterIterator->second->getParameterName( ).first ) ) { - currentObservationPartial = createObservationPartialWrtParameter< Eigen::VectorXd, ObservationSize >( - oneWayLinkEnds, bodies, parameterIterator->second, positionScaling, - lightTimeCorrectionPartialObjects ); + currentObservationPartial = + createObservationPartialWrtParameter< Eigen::VectorXd, ObservationSize >( + oneWayLinkEnds, + bodies, + parameterIterator->second, + positionScaling, + lightTimeCorrectionPartialObjects ); } else { - std::function< std::shared_ptr< ObservationPartial< ObservationSize > >( const std::string& ) > partialWrtStateCreationFunction = - std::bind( &createObservationPartialWrtBodyPosition< ObservationSize >, - oneWayLinkEnds, bodies, std::placeholders::_1, positionScaling, lightTimeCorrectionPartialObjects ); + std::function< std::shared_ptr< ObservationPartial< ObservationSize > >( + const std::string& ) > + partialWrtStateCreationFunction = + std::bind( &createObservationPartialWrtBodyPosition< ObservationSize >, + oneWayLinkEnds, + bodies, + std::placeholders::_1, + positionScaling, + lightTimeCorrectionPartialObjects ); currentObservationPartial = createObservationPartialWrtLinkProperty< ObservationSize >( - oneWayLinkEnds, observableType, parameterIterator->second, bodies, isPartialForDifferencedObservable, isPartialForConcatenatedObservable, - observationPartials, partialWrtStateCreationFunction, observationModel->getObservationBiasCalculator( ) ); + oneWayLinkEnds, + observableType, + parameterIterator->second, + bodies, + isPartialForDifferencedObservable, + isPartialForConcatenatedObservable, + observationPartials, + partialWrtStateCreationFunction, + observationModel->getObservationBiasCalculator( ) ); } - // Check if partial is non-nullptr (i.e. whether dependency exists between current observable and current parameter) + // Check if partial is non-nullptr (i.e. whether dependency exists between current + // observable and current parameter) if( currentObservationPartial != nullptr ) { // Add partial to the list. @@ -356,9 +423,8 @@ createSingleLinkObservationPartials( return std::make_pair( observationPartials, positionScaling ); } -} - -} +} // namespace observation_partials +} // namespace tudat -#endif // TUDAT_CREATEDIRECTOBSERVATIONPARTIALS_H +#endif // TUDAT_CREATEDIRECTOBSERVATIONPARTIALS_H diff --git a/include/tudat/simulation/estimation_setup/createDopplerPartials.h b/include/tudat/simulation/estimation_setup/createDopplerPartials.h index 5032d51ddd..f718198f28 100644 --- a/include/tudat/simulation/estimation_setup/createDopplerPartials.h +++ b/include/tudat/simulation/estimation_setup/createDopplerPartials.h @@ -11,28 +11,24 @@ #ifndef TUDAT_CREATEONEWAYDOPPLERPARTIALS_H #define TUDAT_CREATEONEWAYDOPPLERPARTIALS_H - -#include +#include #include - #include - -#include - -#include "tudat/math/interpolators/interpolator.h" +#include #include "tudat/astro/observation_models/corrections/lightTimeCorrection.h" -#include "tudat/simulation/estimation_setup/createCartesianStatePartials.h" -#include "tudat/simulation/estimation_setup/createLightTimeCalculator.h" -#include "tudat/simulation/estimation_setup/createDirectObservationPartials.h" -#include "tudat/astro/orbit_determination/observation_partials/oneWayDopplerPartial.h" -#include "tudat/astro/orbit_determination/observation_partials/twoWayDopplerPartial.h" -#include "tudat/astro/orbit_determination/estimatable_parameters/initialTranslationalState.h" #include "tudat/astro/observation_models/linkTypeDefs.h" #include "tudat/astro/observation_models/observableTypes.h" #include "tudat/astro/observation_models/twoWayDopplerObservationModel.h" +#include "tudat/astro/orbit_determination/estimatable_parameters/initialTranslationalState.h" +#include "tudat/astro/orbit_determination/observation_partials/oneWayDopplerPartial.h" +#include "tudat/astro/orbit_determination/observation_partials/twoWayDopplerPartial.h" +#include "tudat/math/interpolators/interpolator.h" +#include "tudat/simulation/estimation_setup/createCartesianStatePartials.h" +#include "tudat/simulation/estimation_setup/createDirectObservationPartials.h" +#include "tudat/simulation/estimation_setup/createLightTimeCalculator.h" #include "tudat/simulation/estimation_setup/createLightTimeCorrectionPartials.h" -#include "tudat/simulation/estimation_setup/createObsevationBiasPartial.h" +#include "tudat/simulation/estimation_setup/createObservationBiasPartial.h" namespace tudat { @@ -42,38 +38,42 @@ namespace observation_partials //! Function to generate tow-way Doppler partials and associated scaler for single link ends. /*! - * Function to generate tow-way Doppler partials and associated scaler for all parameters that are to be estimated, - * for a single link ends set. - * The set of parameters and bodies that are to be estimated, as well as the set of link ends - * (each of which must contain a transmitter and receiever linkEndType) that are to be used. - * The tow-way Doppler partials are built from one-way Doppler partials of the constituent links - * \param twoWayDopplerLinkEnds Link ends (transmitter and receiever) for which tow-way Doppler partials are to be calculated - * (i.e. for which tow-way Doppler observations are to be processed). - * \param twoWayObservationModel Observation model for two-way Doppler for which partials are to be created - * \param bodies List of all bodies, for creating tow-way Doppler partials. - * \param parametersToEstimate Set of parameters that are to be estimated (in addition to initial states of - * requested bodies) - * \param lightTimeCorrections List of light time correction partials to be used (empty by default). First vector entry is - * index of link in 2-way link ends (up and downlink), second vector is list of light-time corrections. - * \return Set of observation partials with associated indices in complete vector of parameters that are estimated, - * representing all necessary two-way Doppler partials of a single link end, and TwoWayDopplerScaling, object, used for - * scaling the position partial members of all TwoWayDopplerPartials in link end. + * Function to generate tow-way Doppler partials and associated scaler for all parameters that are + * to be estimated, for a single link ends set. The set of parameters and bodies that are to be + * estimated, as well as the set of link ends (each of which must contain a transmitter and + * receiever linkEndType) that are to be used. The tow-way Doppler partials are built from one-way + * Doppler partials of the constituent links \param twoWayDopplerLinkEnds Link ends (transmitter and + * receiever) for which tow-way Doppler partials are to be calculated (i.e. for which tow-way + * Doppler observations are to be processed). \param twoWayObservationModel Observation model for + * two-way Doppler for which partials are to be created \param bodies List of all bodies, for + * creating tow-way Doppler partials. \param parametersToEstimate Set of parameters that are to be + * estimated (in addition to initial states of requested bodies) \param lightTimeCorrections List of + * light time correction partials to be used (empty by default). First vector entry is index of link + * in 2-way link ends (up and downlink), second vector is list of light-time corrections. \return + * Set of observation partials with associated indices in complete vector of parameters that are + * estimated, representing all necessary two-way Doppler partials of a single link end, and + * TwoWayDopplerScaling, object, used for scaling the position partial members of all + * TwoWayDopplerPartials in link end. */ template< typename ParameterType, typename TimeType > -std::pair< SingleLinkObservationPartialList, std::shared_ptr< PositionPartialScaling > > createTwoWayDopplerPartials( - const std::shared_ptr< observation_models::ObservationModel< 1, ParameterType, TimeType > > observationModel, +std::pair< SingleLinkObservationPartialList, std::shared_ptr< PositionPartialScaling > > +createTwoWayDopplerPartials( + const std::shared_ptr< observation_models::ObservationModel< 1, ParameterType, TimeType > > + observationModel, const simulation_setup::SystemOfBodies& bodies, - const std::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > parametersToEstimate, + const std::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > + parametersToEstimate, const bool isPartialForDifferencedObservable = false ) -{ - std::shared_ptr< observation_models::TwoWayDopplerObservationModel< ParameterType, TimeType > > twoWayObservationModel = - std::dynamic_pointer_cast< observation_models::TwoWayDopplerObservationModel< ParameterType, TimeType > >( - observationModel ); +{ + std::shared_ptr< observation_models::TwoWayDopplerObservationModel< ParameterType, TimeType > > + twoWayObservationModel = std::dynamic_pointer_cast< + observation_models::TwoWayDopplerObservationModel< ParameterType, TimeType > >( + observationModel ); observation_models::LinkEnds twoWayDopplerLinkEnds = twoWayObservationModel->getLinkEnds( ); - // Define list of constituent one-way partials. - typedef std::vector< std::pair< SingleLinkObservationPartialList, std::shared_ptr< PositionPartialScaling > > > + typedef std::vector< std::pair< SingleLinkObservationPartialList, + std::shared_ptr< PositionPartialScaling > > > OneWayPartialList; OneWayPartialList constituentOneWayDopplerPartials; OneWayPartialList constituentOneWayRangePartials; @@ -82,169 +82,226 @@ std::pair< SingleLinkObservationPartialList, std::shared_ptr< PositionPartialSca observation_models::LinkEnds currentLinkEnds; // Iterate over all links in the two-way Doppler observable - std::vector< std::function< double( const double, const observation_models::LinkEndType ) > > oneWayDopplerModels; - std::vector< std::shared_ptr< observation_models::LightTimeCorrection > > currentLightTimeCorrections; + std::vector< std::function< double( const double, const observation_models::LinkEndType ) > > + oneWayDopplerModels; + std::vector< std::shared_ptr< observation_models::LightTimeCorrection > > + currentLightTimeCorrections; for( int i = 0; i < numberOfLinkEnds - 1; i++ ) { if( i == 0 ) { - currentLightTimeCorrections = - twoWayObservationModel->getUplinkDopplerCalculator( )->getLightTimeCalculator( )->getLightTimeCorrection( ); + currentLightTimeCorrections = twoWayObservationModel->getUplinkDopplerCalculator( ) + ->getLightTimeCalculator( ) + ->getLightTimeCorrection( ); } else { - currentLightTimeCorrections = - twoWayObservationModel->getDownlinkDopplerCalculator( )->getLightTimeCalculator( )->getLightTimeCorrection( ); + currentLightTimeCorrections = twoWayObservationModel->getDownlinkDopplerCalculator( ) + ->getLightTimeCalculator( ) + ->getLightTimeCorrection( ); } // Define links for current one-way Doppler link currentLinkEnds.clear( ); - currentLinkEnds[ observation_models::transmitter ] = twoWayDopplerLinkEnds.at( - observation_models::getNWayLinkEnumFromIndex( i, 3 ) ); + currentLinkEnds[ observation_models::transmitter ] = + twoWayDopplerLinkEnds.at( observation_models::getNWayLinkEnumFromIndex( i, 3 ) ); currentLinkEnds[ observation_models::receiver ] = twoWayDopplerLinkEnds.at( - observation_models::getNWayLinkEnumFromIndex( i + 1, 3 ) ); - + observation_models::getNWayLinkEnumFromIndex( i + 1, 3 ) ); - std::shared_ptr< observation_models::OneWayDopplerObservationModel< ParameterType, TimeType > > currentDopplerModel; + std::shared_ptr< + observation_models::OneWayDopplerObservationModel< ParameterType, TimeType > > + currentDopplerModel; if( i == 0 ) { oneWayDopplerModels.push_back( - observation_models::getSizeOneObservationFunctionAtDoublePrecisionFromObservationModel< - ParameterType, TimeType >( twoWayObservationModel->getUplinkDopplerCalculator( ) ) ); + observation_models:: + getSizeOneObservationFunctionAtDoublePrecisionFromObservationModel< + ParameterType, + TimeType >( + twoWayObservationModel->getUplinkDopplerCalculator( ) ) ); currentDopplerModel = twoWayObservationModel->getUplinkDopplerCalculator( ); - } else if( i == 1 ) { oneWayDopplerModels.push_back( - observation_models::getSizeOneObservationFunctionAtDoublePrecisionFromObservationModel< - ParameterType, TimeType >( twoWayObservationModel->getDownlinkDopplerCalculator( ) ) ); + observation_models:: + getSizeOneObservationFunctionAtDoublePrecisionFromObservationModel< + ParameterType, + TimeType >( + twoWayObservationModel->getDownlinkDopplerCalculator( ) ) ); currentDopplerModel = twoWayObservationModel->getDownlinkDopplerCalculator( ); - } - // Create one-way Doppler partials for current link constituentOneWayDopplerPartials.push_back( - createSingleLinkObservationPartials< ParameterType, 1, TimeType >( + createSingleLinkObservationPartials< ParameterType, 1, TimeType >( currentDopplerModel, bodies, parametersToEstimate, false ) ); constituentOneWayRangePartials.push_back( - createSingleLinkObservationPartials< ParameterType, 1, TimeType >( - std::make_shared< observation_models::OneWayRangeObservationModel< ParameterType, TimeType > >( - currentLinkEnds, currentDopplerModel->getLightTimeCalculator( ) ), bodies, - parametersToEstimate, false ) ); + createSingleLinkObservationPartials< ParameterType, 1, TimeType >( + std::make_shared< + observation_models::OneWayRangeObservationModel< ParameterType, + TimeType > >( + currentLinkEnds, currentDopplerModel->getLightTimeCalculator( ) ), + bodies, + parametersToEstimate, + false ) ); } - // Retrieve sorted (by parameter index and link index) one-way range partials and (by link index) opne-way range partials + // Retrieve sorted (by parameter index and link index) one-way range partials and (by link + // index) opne-way range partials std::vector< std::shared_ptr< OneWayDopplerScaling > > oneWayDopplerScalers; std::vector< std::shared_ptr< OneWayRangeScaling > > oneWayRangeScalings; - std::map< std::pair< int, int >, std::map< int, std::shared_ptr< ObservationPartial< 1 > > > > sortedOneWayDopplerPartials; - std::map< std::pair< int, int >, std::map< int, std::shared_ptr< ObservationPartial< 1 > > > > sortedOneWayRangePartials; + std::map< std::pair< int, int >, std::map< int, std::shared_ptr< ObservationPartial< 1 > > > > + sortedOneWayDopplerPartials; + std::map< std::pair< int, int >, std::map< int, std::shared_ptr< ObservationPartial< 1 > > > > + sortedOneWayRangePartials; - std::map< std::pair< int, int >, estimatable_parameters::EstimatebleParameterIdentifier > parameterIdList; + std::map< std::pair< int, int >, estimatable_parameters::EstimatebleParameterIdentifier > + parameterIdList; for( unsigned int i = 0; i < constituentOneWayDopplerPartials.size( ); i++ ) { // Retrieve one-way Doppler paritals - oneWayDopplerScalers.push_back( std::dynamic_pointer_cast< OneWayDopplerScaling > - ( constituentOneWayDopplerPartials.at( i ).second ) ); - oneWayRangeScalings.push_back( std::dynamic_pointer_cast< OneWayRangeScaling > - ( constituentOneWayRangePartials.at( i ).second ) ); + oneWayDopplerScalers.push_back( std::dynamic_pointer_cast< OneWayDopplerScaling >( + constituentOneWayDopplerPartials.at( i ).second ) ); + oneWayRangeScalings.push_back( std::dynamic_pointer_cast< OneWayRangeScaling >( + constituentOneWayRangePartials.at( i ).second ) ); // Iterate over all one-way Doppler partials of current link - for( SingleLinkObservationPartialList::iterator parameterIterator = constituentOneWayDopplerPartials.at( i ).first.begin( ); - parameterIterator != constituentOneWayDopplerPartials.at( i ).first.end( ); parameterIterator++ ) + for( SingleLinkObservationPartialList::iterator parameterIterator = + constituentOneWayDopplerPartials.at( i ).first.begin( ); + parameterIterator != constituentOneWayDopplerPartials.at( i ).first.end( ); + parameterIterator++ ) { - sortedOneWayDopplerPartials[ parameterIterator->first ][ i ] = parameterIterator->second; + sortedOneWayDopplerPartials[ parameterIterator->first ][ i ] = + parameterIterator->second; if( parameterIdList.count( parameterIterator->first ) == 0 ) { - parameterIdList[ parameterIterator->first ] = parameterIterator->second->getParameterIdentifier( ); + parameterIdList[ parameterIterator->first ] = + parameterIterator->second->getParameterIdentifier( ); } - else if( parameterIdList.at( parameterIterator->first ) != parameterIterator->second->getParameterIdentifier( ) ) + else if( parameterIdList.at( parameterIterator->first ) != + parameterIterator->second->getParameterIdentifier( ) ) { - throw std::runtime_error( "Error when making two way Doppler partial, parameter indices in Doppler are inconsistent" ); + throw std::runtime_error( + "Error when making two way Doppler partial, parameter indices in Doppler " + "are inconsistent" ); } } // Iterate over all one-way range partials of current link - for( SingleLinkObservationPartialList::iterator parameterIterator = constituentOneWayRangePartials.at( i ).first.begin( ); - parameterIterator != constituentOneWayRangePartials.at( i ).first.end( ); parameterIterator++ ) + for( SingleLinkObservationPartialList::iterator parameterIterator = + constituentOneWayRangePartials.at( i ).first.begin( ); + parameterIterator != constituentOneWayRangePartials.at( i ).first.end( ); + parameterIterator++ ) { sortedOneWayRangePartials[ parameterIterator->first ][ i ] = parameterIterator->second; if( parameterIdList.count( parameterIterator->first ) == 0 ) { - parameterIdList[ parameterIterator->first ] = parameterIterator->second->getParameterIdentifier( ); + parameterIdList[ parameterIterator->first ] = + parameterIterator->second->getParameterIdentifier( ); } - else if( parameterIdList.at( parameterIterator->first ) != parameterIterator->second->getParameterIdentifier( ) ) + else if( parameterIdList.at( parameterIterator->first ) != + parameterIterator->second->getParameterIdentifier( ) ) { - throw std::runtime_error( "Error when making two way Doppler partial, parameter indices in range are inconsistent" ); + throw std::runtime_error( + "Error when making two way Doppler partial, parameter indices in range are " + "inconsistent" ); } } } // Create two-way Doppler scaling object - std::shared_ptr< TwoWayDopplerScaling > twoWayDopplerScaler = std::make_shared< TwoWayDopplerScaling >( - oneWayDopplerScalers, oneWayRangeScalings, oneWayDopplerModels, - twoWayObservationModel->getNormalizeWithSpeedOfLight( ) ? 1.0 : physical_constants::SPEED_OF_LIGHT ); + std::shared_ptr< TwoWayDopplerScaling > twoWayDopplerScaler = + std::make_shared< TwoWayDopplerScaling >( + oneWayDopplerScalers, + oneWayRangeScalings, + oneWayDopplerModels, + twoWayObservationModel->getNormalizeWithSpeedOfLight( ) + ? 1.0 + : physical_constants::SPEED_OF_LIGHT ); // Define return partial list SingleLinkObservationPartialList twoWayDopplerPartialList; // Create two-way Doppler partial objects - for( std::map< std::pair< int, int >, std::map< int, std::shared_ptr< ObservationPartial< 1 > > > >::iterator sortedPartialIterator = - sortedOneWayDopplerPartials.begin( ); sortedPartialIterator != sortedOneWayDopplerPartials.end( ); sortedPartialIterator++ ) + for( std::map< std::pair< int, int >, + std::map< int, std::shared_ptr< ObservationPartial< 1 > > > >::iterator + sortedPartialIterator = sortedOneWayDopplerPartials.begin( ); + sortedPartialIterator != sortedOneWayDopplerPartials.end( ); + sortedPartialIterator++ ) { - std::map< int, std::shared_ptr< ObservationPartial< 1 > > > currentDopplerPartialList = sortedPartialIterator->second; + std::map< int, std::shared_ptr< ObservationPartial< 1 > > > currentDopplerPartialList = + sortedPartialIterator->second; std::map< int, std::shared_ptr< ObservationPartial< 1 > > > currentRangePartialList; if( sortedOneWayRangePartials.count( sortedPartialIterator->first ) != 0 ) { currentRangePartialList = sortedOneWayRangePartials.at( sortedPartialIterator->first ); } - twoWayDopplerPartialList[ sortedPartialIterator->first ] = std::make_shared< TwoWayDopplerPartial >( - twoWayDopplerScaler, currentDopplerPartialList, currentRangePartialList, - parameterIdList.at( sortedPartialIterator->first ), numberOfLinkEnds ); + twoWayDopplerPartialList[ sortedPartialIterator->first ] = + std::make_shared< TwoWayDopplerPartial >( + twoWayDopplerScaler, + currentDopplerPartialList, + currentRangePartialList, + parameterIdList.at( sortedPartialIterator->first ), + numberOfLinkEnds ); } // Create two-way Doppler partial objects that only have range dependencies. - for( std::map< std::pair< int, int >, std::map< int, std::shared_ptr< ObservationPartial< 1 > > > >::iterator sortedPartialIterator = - sortedOneWayRangePartials.begin( ); sortedPartialIterator != sortedOneWayRangePartials.end( ); sortedPartialIterator++ ) + for( std::map< std::pair< int, int >, + std::map< int, std::shared_ptr< ObservationPartial< 1 > > > >::iterator + sortedPartialIterator = sortedOneWayRangePartials.begin( ); + sortedPartialIterator != sortedOneWayRangePartials.end( ); + sortedPartialIterator++ ) { if( sortedOneWayDopplerPartials.count( sortedPartialIterator->first ) == 0 ) { std::map< int, std::shared_ptr< ObservationPartial< 1 > > > currentDopplerPartialList; - std::map< int, std::shared_ptr< ObservationPartial< 1 > > > currentRangePartialList = sortedPartialIterator->second; - - twoWayDopplerPartialList[ sortedPartialIterator->first ] = std::make_shared< TwoWayDopplerPartial >( - twoWayDopplerScaler, currentDopplerPartialList, currentRangePartialList, - parameterIdList.at( sortedPartialIterator->first ), numberOfLinkEnds ); + std::map< int, std::shared_ptr< ObservationPartial< 1 > > > currentRangePartialList = + sortedPartialIterator->second; + + twoWayDopplerPartialList[ sortedPartialIterator->first ] = + std::make_shared< TwoWayDopplerPartial >( + twoWayDopplerScaler, + currentDopplerPartialList, + currentRangePartialList, + parameterIdList.at( sortedPartialIterator->first ), + numberOfLinkEnds ); } } // Create two-way Doppler partials - std::map< int, std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > > - vectorParametersToEstimate = parametersToEstimate->getVectorParameters( ); - for( std::map< int, std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > >::iterator - parameterIterator = - vectorParametersToEstimate.begin( ); parameterIterator != vectorParametersToEstimate.end( ); parameterIterator++ ) + std::map< int, + std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > > + vectorParametersToEstimate = parametersToEstimate->getVectorParameters( ); + for( std::map< int, + std::shared_ptr< estimatable_parameters::EstimatableParameter< + Eigen::VectorXd > > >::iterator parameterIterator = + vectorParametersToEstimate.begin( ); + parameterIterator != vectorParametersToEstimate.end( ); + parameterIterator++ ) { - std::shared_ptr< ObservationPartial< 1 > > currentTwoWayDopplerPartial; - if( isParameterObservationLinkProperty( parameterIterator->second->getParameterName( ).first ) ) + if( isParameterObservationLinkProperty( + parameterIterator->second->getParameterName( ).first ) ) { currentTwoWayDopplerPartial = createObservationPartialWrtLinkProperty< 1 >( - twoWayDopplerLinkEnds, observation_models::two_way_doppler, parameterIterator->second, bodies, - isPartialForDifferencedObservable ); + twoWayDopplerLinkEnds, + observation_models::two_way_doppler, + parameterIterator->second, + bodies, + isPartialForDifferencedObservable ); } - - // Check if partial is non-nullptr (i.e. whether dependency exists between current doppler and current parameter) + // Check if partial is non-nullptr (i.e. whether dependency exists between current doppler + // and current parameter) if( currentTwoWayDopplerPartial != nullptr ) { // Add partial to the list. - std::pair< double, double > currentPair = std::pair< int, int >( parameterIterator->first, - parameterIterator->second->getParameterSize( ) ); + std::pair< double, double > currentPair = std::pair< int, int >( + parameterIterator->first, parameterIterator->second->getParameterSize( ) ); twoWayDopplerPartialList[ currentPair ] = currentTwoWayDopplerPartial; } } @@ -252,10 +309,8 @@ std::pair< SingleLinkObservationPartialList, std::shared_ptr< PositionPartialSca return std::make_pair( twoWayDopplerPartialList, twoWayDopplerScaler ); } +} // namespace observation_partials -} - -} - -#endif // TUDAT_CREATEONEWAYDOPPLERPARTIALS_H +} // namespace tudat +#endif // TUDAT_CREATEONEWAYDOPPLERPARTIALS_H diff --git a/include/tudat/simulation/estimation_setup/createNWayRangePartials.h b/include/tudat/simulation/estimation_setup/createNWayRangePartials.h index 30306e8dca..0ea17a38c9 100755 --- a/include/tudat/simulation/estimation_setup/createNWayRangePartials.h +++ b/include/tudat/simulation/estimation_setup/createNWayRangePartials.h @@ -11,20 +11,17 @@ #ifndef TUDAT_CREATENWAYRANGEPARTIALS_H #define TUDAT_CREATENWAYRANGEPARTIALS_H -#include +#include #include - #include +#include -#include - -#include "tudat/math/interpolators/interpolator.h" - -#include "tudat/simulation/estimation_setup/createDirectObservationPartials.h" -#include "tudat/astro/orbit_determination/observation_partials/nWayRangePartial.h" #include "tudat/astro/observation_models/linkTypeDefs.h" #include "tudat/astro/observation_models/observableTypes.h" -#include "tudat/simulation/estimation_setup/createObsevationBiasPartial.h" +#include "tudat/astro/orbit_determination/observation_partials/nWayRangePartial.h" +#include "tudat/math/interpolators/interpolator.h" +#include "tudat/simulation/estimation_setup/createDirectObservationPartials.h" +#include "tudat/simulation/estimation_setup/createObservationBiasPartial.h" namespace tudat { @@ -34,48 +31,54 @@ namespace observation_partials //! Function to generate n-way range partials and associated scaler for single link ends. /*! - * Function to generate n-way range partials and associated scaler for all parameters that are to be estimated, - * for a single link ends set. - * The set of parameters and bodies that are to be estimated, as well as the set of link ends - * (each of which must contain a transmitter and receiever linkEndType) that are to be used. - * The n-way range partials are built from one-way range partials of the constituent links - * \param nWayRangeLinkEnds Link ends (transmitter and receiever) for which n-way range partials are to be calculated - * (i.e. for which n-way range observations are to be processed). - * \param bodies List of all bodies, for creating n-way range partials. - * \param parametersToEstimate Set of parameters that are to be estimated (in addition to initial states of + * Function to generate n-way range partials and associated scaler for all parameters that are to + * be estimated, for a single link ends set. The set of parameters and bodies that are to be + * estimated, as well as the set of link ends (each of which must contain a transmitter and + * receiever linkEndType) that are to be used. The n-way range partials are built from one-way range + * partials of the constituent links \param nWayRangeLinkEnds Link ends (transmitter and receiever) + * for which n-way range partials are to be calculated (i.e. for which n-way range observations are + * to be processed). \param bodies List of all bodies, for creating n-way range partials. \param + * parametersToEstimate Set of parameters that are to be estimated (in addition to initial states of * requested bodies) - * \param lightTimeCorrections List of light time correction partials to be used (empty by default). First vector entry is - * index of link in n-way link ends, second vector is list of light-time corrections. - * \return Set of observation partials with associated indices in complete vector of parameters that are estimated, - * representing all necessary n-way range partials of a single link end, and NWayRangeScaling, object, used for - * scaling the position partial members of all NWayRangePartials in link end. + * \param lightTimeCorrections List of light time correction partials to be used (empty by + * default). First vector entry is index of link in n-way link ends, second vector is list of + * light-time corrections. \return Set of observation partials with associated indices in complete + * vector of parameters that are estimated, representing all necessary n-way range partials of a + * single link end, and NWayRangeScaling, object, used for scaling the position partial members of + * all NWayRangePartials in link end. */ template< typename ParameterType, typename TimeType > -std::pair< SingleLinkObservationPartialList, std::shared_ptr< PositionPartialScaling > > createNWayRangePartials( - const std::shared_ptr< observation_models::ObservationModel< 1, ParameterType, TimeType > > observationModel, +std::pair< SingleLinkObservationPartialList, std::shared_ptr< PositionPartialScaling > > +createNWayRangePartials( + const std::shared_ptr< observation_models::ObservationModel< 1, ParameterType, TimeType > > + observationModel, const simulation_setup::SystemOfBodies& bodies, - const std::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > parametersToEstimate, + const std::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > + parametersToEstimate, const bool isPartialForDifferencedObservable ) { using namespace observation_models; std::shared_ptr< observation_models::NWayRangeObservationModel< ParameterType, TimeType > > - nWayRangeObservationModel = - std::dynamic_pointer_cast< observation_models::NWayRangeObservationModel< ParameterType, TimeType > >( - observationModel ); + nWayRangeObservationModel = std::dynamic_pointer_cast< + observation_models::NWayRangeObservationModel< ParameterType, TimeType > >( + observationModel ); if( nWayRangeObservationModel == nullptr ) { - throw std::runtime_error( "Error when creating n-way range partials; input observation model is not n-way range" ); + throw std::runtime_error( + "Error when creating n-way range partials; input observation model is not n-way " + "range" ); } observation_models::LinkEnds nWayRangeLinkEnds = nWayRangeObservationModel->getLinkEnds( ); - // Define return partial list SingleLinkObservationPartialList nWayRangePartialList; // Define list of constituent one-way partials. - typedef std::map< int, std::pair< SingleLinkObservationPartialList, std::shared_ptr< PositionPartialScaling > > > + typedef std::map< int, + std::pair< SingleLinkObservationPartialList, + std::shared_ptr< PositionPartialScaling > > > OneWayRangePartialList; OneWayRangePartialList constituentOneWayRangePartials; @@ -84,91 +87,122 @@ std::pair< SingleLinkObservationPartialList, std::shared_ptr< PositionPartialSca int numberOfLinkEnds = nWayRangeLinkEnds.size( ); // Iterate over all links in the n-way range observable - std::vector< std::shared_ptr< observation_models::LightTimeCorrection > > currentLightTimeCorrections; + std::vector< std::shared_ptr< observation_models::LightTimeCorrection > > + currentLightTimeCorrections; for( int i = 0; i < numberOfLinkEnds - 1; i++ ) { currentLightTimeCorrections.clear( ); - currentLightTimeCorrections = nWayRangeObservationModel->getLightTimeCalculators( ).at( i )->getLightTimeCorrection( ); + currentLightTimeCorrections = nWayRangeObservationModel->getLightTimeCalculators( ) + .at( i ) + ->getLightTimeCorrection( ); // Define links for current one-way range link currentLinkEnds.clear( ); currentLinkEnds[ observation_models::transmitter ] = nWayRangeLinkEnds.at( - observation_models::getNWayLinkEnumFromIndex( i, numberOfLinkEnds ) ); + observation_models::getNWayLinkEnumFromIndex( i, numberOfLinkEnds ) ); currentLinkEnds[ observation_models::receiver ] = nWayRangeLinkEnds.at( - observation_models::getNWayLinkEnumFromIndex( i + 1, numberOfLinkEnds ) ); + observation_models::getNWayLinkEnumFromIndex( i + 1, numberOfLinkEnds ) ); // Create onw-way range partials for current link constituentOneWayRangePartials[ i ] = - createSingleLinkObservationPartials< ParameterType, 1, TimeType > - ( std::make_shared< OneWayRangeObservationModel< ParameterType, TimeType > >( - currentLinkEnds, nWayRangeObservationModel->getLightTimeCalculators( ).at( i ) ), - bodies, parametersToEstimate, false, true ); + createSingleLinkObservationPartials< ParameterType, 1, TimeType >( + std::make_shared< OneWayRangeObservationModel< ParameterType, TimeType > >( + currentLinkEnds, + nWayRangeObservationModel->getLightTimeCalculators( ).at( i ) ), + bodies, + parametersToEstimate, + false, + true ); } - // Retrieve sorted (by parameter index and link index) one-way range partials and (by link index) opne-way range partials + // Retrieve sorted (by parameter index and link index) one-way range partials and (by link + // index) opne-way range partials std::map< int, std::shared_ptr< OneWayRangeScaling > > oneWayRangeScalers; - std::map< std::pair< int, int >, std::map< int, std::shared_ptr< ObservationPartial< 1 > > > > sortedOneWayRangePartials; - std::map< std::pair< int, int >, estimatable_parameters::EstimatebleParameterIdentifier > parameterIdList; - for( OneWayRangePartialList::iterator oneWayPartialIterator = constituentOneWayRangePartials.begin( ); - oneWayPartialIterator != constituentOneWayRangePartials.end( ); oneWayPartialIterator++ ) + std::map< std::pair< int, int >, std::map< int, std::shared_ptr< ObservationPartial< 1 > > > > + sortedOneWayRangePartials; + std::map< std::pair< int, int >, estimatable_parameters::EstimatebleParameterIdentifier > + parameterIdList; + for( OneWayRangePartialList::iterator oneWayPartialIterator = + constituentOneWayRangePartials.begin( ); + oneWayPartialIterator != constituentOneWayRangePartials.end( ); + oneWayPartialIterator++ ) { // Retrieve one-way range paritals - oneWayRangeScalers[ oneWayPartialIterator->first ] = std::dynamic_pointer_cast< OneWayRangeScaling > - ( oneWayPartialIterator->second.second ); + oneWayRangeScalers[ oneWayPartialIterator->first ] = + std::dynamic_pointer_cast< OneWayRangeScaling >( + oneWayPartialIterator->second.second ); // Iterate over all one-way range partials of current link - for( SingleLinkObservationPartialList::iterator parameterIterator = oneWayPartialIterator->second.first.begin( ); - parameterIterator != oneWayPartialIterator->second.first.end( ); parameterIterator++ ) + for( SingleLinkObservationPartialList::iterator parameterIterator = + oneWayPartialIterator->second.first.begin( ); + parameterIterator != oneWayPartialIterator->second.first.end( ); + parameterIterator++ ) { - sortedOneWayRangePartials[ parameterIterator->first ][ oneWayPartialIterator->first ] = parameterIterator->second; + sortedOneWayRangePartials[ parameterIterator->first ][ oneWayPartialIterator->first ] = + parameterIterator->second; if( parameterIdList.count( parameterIterator->first ) == 0 ) { - parameterIdList[ parameterIterator->first ] = parameterIterator->second->getParameterIdentifier( ); + parameterIdList[ parameterIterator->first ] = + parameterIterator->second->getParameterIdentifier( ); } -// else if( parameterIdList.at( parameterIterator->first ) != parameterIterator->second->getParameterIdentifier( ) ) -// { -// throw std::runtime_error( "Error when making n way range partial, parameter indices are inconsistent" ); -// } + // else if( parameterIdList.at( parameterIterator->first ) != + // parameterIterator->second->getParameterIdentifier( ) ) + // { + // throw std::runtime_error( "Error when making n way range partial, + // parameter indices are inconsistent" ); + // } } } - // Create n-way range scaling object - std::shared_ptr< NWayRangeScaling > nWayRangeScaler = std::make_shared< NWayRangeScaling >( - oneWayRangeScalers, nWayRangeLinkEnds.size( ) ); + std::shared_ptr< NWayRangeScaling > nWayRangeScaler = + std::make_shared< NWayRangeScaling >( oneWayRangeScalers, nWayRangeLinkEnds.size( ) ); // Create n-way range partial object - for( std::map< std::pair< int, int >, std::map< int, std::shared_ptr< ObservationPartial< 1 > > > >::iterator sortedPartialIterator = - sortedOneWayRangePartials.begin( ); sortedPartialIterator != sortedOneWayRangePartials.end( ); sortedPartialIterator++ ) + for( std::map< std::pair< int, int >, + std::map< int, std::shared_ptr< ObservationPartial< 1 > > > >::iterator + sortedPartialIterator = sortedOneWayRangePartials.begin( ); + sortedPartialIterator != sortedOneWayRangePartials.end( ); + sortedPartialIterator++ ) { nWayRangePartialList[ sortedPartialIterator->first ] = std::make_shared< NWayRangePartial >( - nWayRangeScaler, sortedPartialIterator->second, parameterIdList.at( sortedPartialIterator->first ), - numberOfLinkEnds ); + nWayRangeScaler, + sortedPartialIterator->second, + parameterIdList.at( sortedPartialIterator->first ), + numberOfLinkEnds ); } - std::map< int, std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > > - vectorParametersToEstimate = parametersToEstimate->getVectorParameters( ); - for( std::map< int, std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > >::iterator - parameterIterator = - vectorParametersToEstimate.begin( ); parameterIterator != vectorParametersToEstimate.end( ); parameterIterator++ ) + std::map< int, + std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > > + vectorParametersToEstimate = parametersToEstimate->getVectorParameters( ); + for( std::map< int, + std::shared_ptr< estimatable_parameters::EstimatableParameter< + Eigen::VectorXd > > >::iterator parameterIterator = + vectorParametersToEstimate.begin( ); + parameterIterator != vectorParametersToEstimate.end( ); + parameterIterator++ ) { - std::shared_ptr< ObservationPartial< 1 > > currentNWayRangePartial; - if( isParameterObservationLinkProperty( parameterIterator->second->getParameterName( ).first ) && - !isParameterObservationLinkTimeProperty( parameterIterator->second->getParameterName( ).first ) && - !isPartialForDifferencedObservable ) + if( isParameterObservationLinkProperty( + parameterIterator->second->getParameterName( ).first ) && + !isParameterObservationLinkTimeProperty( + parameterIterator->second->getParameterName( ).first ) && + !isPartialForDifferencedObservable ) { - currentNWayRangePartial = createObservationPartialWrtLinkProperty< 1 >( - nWayRangeLinkEnds, observation_models::n_way_range, parameterIterator->second, bodies ); + currentNWayRangePartial = + createObservationPartialWrtLinkProperty< 1 >( nWayRangeLinkEnds, + observation_models::n_way_range, + parameterIterator->second, + bodies ); } // Check if partial is non-nullptr if( currentNWayRangePartial != nullptr ) { // Add partial to the list. - std::pair< double, double > currentPair = std::pair< int, int >( parameterIterator->first, - parameterIterator->second->getParameterSize( ) ); + std::pair< double, double > currentPair = std::pair< int, int >( + parameterIterator->first, parameterIterator->second->getParameterSize( ) ); nWayRangePartialList[ currentPair ] = currentNWayRangePartial; } } @@ -176,9 +210,8 @@ std::pair< SingleLinkObservationPartialList, std::shared_ptr< PositionPartialSca return std::make_pair( nWayRangePartialList, nWayRangeScaler ); } +} // namespace observation_partials -} - -} +} // namespace tudat -#endif // TUDAT_CREATENWAYRANGEPARTIALS_H +#endif // TUDAT_CREATENWAYRANGEPARTIALS_H diff --git a/include/tudat/simulation/estimation_setup/createObsevationBiasPartial.h b/include/tudat/simulation/estimation_setup/createObservationBiasPartial.h similarity index 100% rename from include/tudat/simulation/estimation_setup/createObsevationBiasPartial.h rename to include/tudat/simulation/estimation_setup/createObservationBiasPartial.h diff --git a/include/tudat/simulation/estimation_setup/createObservationModel.h b/include/tudat/simulation/estimation_setup/createObservationModel.h index 992d487eea..ae8f981aeb 100644 --- a/include/tudat/simulation/estimation_setup/createObservationModel.h +++ b/include/tudat/simulation/estimation_setup/createObservationModel.h @@ -11,36 +11,32 @@ #ifndef TUDAT_CREATEOBSERVATIONMODEL_H #define TUDAT_CREATEOBSERVATIONMODEL_H -#include - #include +#include - - -#include "tudat/astro/observation_models/observationModel.h" +#include "tudat/astro/observation_models/angularPositionObservationModel.h" +#include "tudat/astro/observation_models/dopplerMeasuredFrequencyObservationModel.h" +#include "tudat/astro/observation_models/dsnNWayAveragedDopplerObservationModel.h" +#include "tudat/astro/observation_models/dsnNWayRangeObservationModel.h" +#include "tudat/astro/observation_models/eulerAngleObservationModel.h" #include "tudat/astro/observation_models/linkTypeDefs.h" -#include "tudat/simulation/estimation_setup/createLightTimeCorrection.h" #include "tudat/astro/observation_models/nWayRangeObservationModel.h" #include "tudat/astro/observation_models/nWayRangeRateObservationModel.h" -#include "tudat/astro/observation_models/oneWayRangeObservationModel.h" -#include "tudat/astro/observation_models/oneWayDopplerObservationModel.h" -#include "tudat/astro/observation_models/twoWayDopplerObservationModel.h" +#include "tudat/astro/observation_models/observationModel.h" +#include "tudat/astro/observation_models/observationSimulator.h" #include "tudat/astro/observation_models/oneWayDifferencedRangeRateObservationModel.h" -#include "tudat/astro/observation_models/angularPositionObservationModel.h" -#include "tudat/astro/observation_models/relativeAngularPositionObservationModel.h" +#include "tudat/astro/observation_models/oneWayDopplerObservationModel.h" +#include "tudat/astro/observation_models/oneWayRangeObservationModel.h" #include "tudat/astro/observation_models/positionObservationModel.h" +#include "tudat/astro/observation_models/relativeAngularPositionObservationModel.h" #include "tudat/astro/observation_models/relativePositionObservationModel.h" -#include "tudat/astro/observation_models/eulerAngleObservationModel.h" +#include "tudat/astro/observation_models/twoWayDopplerObservationModel.h" #include "tudat/astro/observation_models/velocityObservationModel.h" -#include "tudat/astro/observation_models/observationSimulator.h" -#include "tudat/astro/observation_models/dsnNWayAveragedDopplerObservationModel.h" -#include "tudat/astro/observation_models/dsnNWayRangeObservationModel.h" -#include "tudat/astro/observation_models/dopplerMeasuredFrequencyObservationModel.h" #include "tudat/simulation/environment_setup/body.h" #include "tudat/simulation/estimation_setup/createLightTimeCalculator.h" +#include "tudat/simulation/estimation_setup/createLightTimeCorrection.h" #include "tudat/simulation/estimation_setup/createObservationViability.h" - namespace tudat { @@ -49,76 +45,76 @@ namespace observation_models //! Base class to define settings for creation of an observation bias model. /*! - * Base class to define settings for creation of an observation bias model. For each specific bias type, a derived class - * is to be implemented, in which the specific properties of the bias model are given + * Base class to define settings for creation of an observation bias model. For each specific bias + * type, a derived class is to be implemented, in which the specific properties of the bias model + * are given */ class ObservationBiasSettings { -public: - + public: //! Constructor /*! * Constructor * \param observationBiasType Type of bias model that is to be created. */ - ObservationBiasSettings( - const observation_models::ObservationBiasTypes observationBiasType ): - observationBiasType_( observationBiasType ){ } + ObservationBiasSettings( const observation_models::ObservationBiasTypes observationBiasType ) : + observationBiasType_( observationBiasType ) + { } //! Destructor - virtual ~ObservationBiasSettings( ){ } + virtual ~ObservationBiasSettings( ) { } //! Type of bias model that is to be created. observation_models::ObservationBiasTypes observationBiasType_; }; //! Class for defining settings for the creation of a multiple biases for a single observable -class MultipleObservationBiasSettings: public ObservationBiasSettings +class MultipleObservationBiasSettings : public ObservationBiasSettings { -public: - + public: //! Constructor /*! * Constructor * \param biasSettingsList List of settings for bias objects that are to be created. */ MultipleObservationBiasSettings( - const std::vector< std::shared_ptr< ObservationBiasSettings > > biasSettingsList ): + const std::vector< std::shared_ptr< ObservationBiasSettings > > biasSettingsList ) : ObservationBiasSettings( multiple_observation_biases ), - biasSettingsList_( biasSettingsList ){ } + biasSettingsList_( biasSettingsList ) + { } //! Destructor - ~MultipleObservationBiasSettings( ){ } + ~MultipleObservationBiasSettings( ) { } //! List of settings for bias objects that are to be created. std::vector< std::shared_ptr< ObservationBiasSettings > > biasSettingsList_; }; //! Class for defining settings for the creation of a constant absolute or relative observation bias model -class ConstantObservationBiasSettings: public ObservationBiasSettings +class ConstantObservationBiasSettings : public ObservationBiasSettings { -public: - + public: //! Constuctor /*! * Constuctor - * \param observationBias Constant bias that is to be added to the observable. The size of this vector must be equal to the - * size of the observable to which it is assigned. - * \param useAbsoluteBias Boolean to denote whether an absolute or relative bias is to be created. + * \param observationBias Constant bias that is to be added to the observable. The size of this + * vector must be equal to the size of the observable to which it is assigned. \param + * useAbsoluteBias Boolean to denote whether an absolute or relative bias is to be created. */ - ConstantObservationBiasSettings( - const Eigen::VectorXd& observationBias, - const bool useAbsoluteBias ): - ObservationBiasSettings( ( useAbsoluteBias == true ) ? ( constant_absolute_bias ) : ( constant_relative_bias ) ), - observationBias_( observationBias ), useAbsoluteBias_( useAbsoluteBias ){ } + ConstantObservationBiasSettings( const Eigen::VectorXd& observationBias, + const bool useAbsoluteBias ) : + ObservationBiasSettings( ( useAbsoluteBias == true ) ? ( constant_absolute_bias ) + : ( constant_relative_bias ) ), + observationBias_( observationBias ), useAbsoluteBias_( useAbsoluteBias ) + { } //! Destructor - ~ConstantObservationBiasSettings( ){ } + ~ConstantObservationBiasSettings( ) { } //! Constant bias that is to be added to the observable. /*! - * Constant bias that is to be added to the observable. The size of this vector must be equal to the - * size of the observable to which it is assigned. + * Constant bias that is to be added to the observable. The size of this vector must be equal + * to the size of the observable to which it is assigned. */ Eigen::VectorXd observationBias_; @@ -127,47 +123,51 @@ class ConstantObservationBiasSettings: public ObservationBiasSettings }; //! Class for defining settings for the creation of an arc-wise constant absolute or relative observation bias model -class ArcWiseConstantObservationBiasSettings: public ObservationBiasSettings +class ArcWiseConstantObservationBiasSettings : public ObservationBiasSettings { -public: - + public: //! Constuctor /*! * Constuctor * \param arcStartTimes Start times for arcs in which biases (observationBiases) are used * \param observationBiases List of observation biases per arc - * \param linkEndForTime Link end at which time is to be evaluated to determine current time (and current arc) - * \param useAbsoluteBias Boolean to denote whether an absolute or relative bias is to be created. + * \param linkEndForTime Link end at which time is to be evaluated to determine current time + * (and current arc) \param useAbsoluteBias Boolean to denote whether an absolute or relative + * bias is to be created. */ - ArcWiseConstantObservationBiasSettings( - const std::vector< double >& arcStartTimes, - const std::vector< Eigen::VectorXd >& observationBiases, - const LinkEndType linkEndForTime, - const bool useAbsoluteBias ): - ObservationBiasSettings( ( useAbsoluteBias == true ) ? - ( arc_wise_constant_absolute_bias ) : ( arc_wise_constant_relative_bias ) ), - arcStartTimes_( arcStartTimes ), observationBiases_( observationBiases ), linkEndForTime_( linkEndForTime ), - useAbsoluteBias_( useAbsoluteBias ){ } + ArcWiseConstantObservationBiasSettings( const std::vector< double >& arcStartTimes, + const std::vector< Eigen::VectorXd >& observationBiases, + const LinkEndType linkEndForTime, + const bool useAbsoluteBias ) : + ObservationBiasSettings( ( useAbsoluteBias == true ) + ? ( arc_wise_constant_absolute_bias ) + : ( arc_wise_constant_relative_bias ) ), + arcStartTimes_( arcStartTimes ), observationBiases_( observationBiases ), + linkEndForTime_( linkEndForTime ), useAbsoluteBias_( useAbsoluteBias ) + { } //! Constuctor /*! * Constuctor - * \param observationBiases Map of observation biases per arc, with bias as map value, and arc start time as map key - * \param linkEndForTime Link end at which time is to be evaluated to determine current time (and current arc) - * \param useAbsoluteBias Boolean to denote whether an absolute or relative bias is to be created. + * \param observationBiases Map of observation biases per arc, with bias as map value, and arc + * start time as map key \param linkEndForTime Link end at which time is to be evaluated to + * determine current time (and current arc) \param useAbsoluteBias Boolean to denote whether an + * absolute or relative bias is to be created. */ ArcWiseConstantObservationBiasSettings( const std::map< double, Eigen::VectorXd >& observationBiases, const LinkEndType linkEndForTime, - const bool useAbsoluteBias ): - ObservationBiasSettings( ( useAbsoluteBias == true ) ? - ( arc_wise_constant_absolute_bias ) : ( arc_wise_constant_relative_bias ) ), + const bool useAbsoluteBias ) : + ObservationBiasSettings( ( useAbsoluteBias == true ) + ? ( arc_wise_constant_absolute_bias ) + : ( arc_wise_constant_relative_bias ) ), arcStartTimes_( utilities::createVectorFromMapKeys( observationBiases ) ), - observationBiases_( utilities::createVectorFromMapValues( observationBiases ) ), linkEndForTime_( linkEndForTime ), - useAbsoluteBias_( useAbsoluteBias ){ } + observationBiases_( utilities::createVectorFromMapValues( observationBiases ) ), + linkEndForTime_( linkEndForTime ), useAbsoluteBias_( useAbsoluteBias ) + { } //! Destructor - ~ArcWiseConstantObservationBiasSettings( ){ } + ~ArcWiseConstantObservationBiasSettings( ) { } //! Start times for arcs in which biases (observationBiases) are used std::vector< double > arcStartTimes_; @@ -183,32 +183,32 @@ class ArcWiseConstantObservationBiasSettings: public ObservationBiasSettings }; //! Class for defining settings for the creation of a constant time drift model -class ConstantTimeDriftBiasSettings: public ObservationBiasSettings +class ConstantTimeDriftBiasSettings : public ObservationBiasSettings { -public: - + public: //! Constuctor /*! * Constuctor - * \param observationBias Constant time drift bias that is to be considered for the observation time. The size of this vector must be equal to the - * size of the observable to which it is assigned. - * \param linkEndForTime Link end at which time is to be evaluated to determine current time - * \param referenceEpoch Reference epoch at which the effect of the time drift is initialised. + * \param observationBias Constant time drift bias that is to be considered for the observation + * time. The size of this vector must be equal to the size of the observable to which it is + * assigned. \param linkEndForTime Link end at which time is to be evaluated to determine + * current time \param referenceEpoch Reference epoch at which the effect of the time drift is + * initialised. */ - ConstantTimeDriftBiasSettings( - const Eigen::VectorXd& timeDriftBias, - const LinkEndType linkEndForTime, - const double referenceEpoch ): - ObservationBiasSettings( constant_time_drift_bias ), - timeDriftBias_( timeDriftBias ), linkEndForTime_( linkEndForTime ), referenceEpoch_( referenceEpoch ){ } + ConstantTimeDriftBiasSettings( const Eigen::VectorXd& timeDriftBias, + const LinkEndType linkEndForTime, + const double referenceEpoch ) : + ObservationBiasSettings( constant_time_drift_bias ), timeDriftBias_( timeDriftBias ), + linkEndForTime_( linkEndForTime ), referenceEpoch_( referenceEpoch ) + { } //! Destructor - ~ConstantTimeDriftBiasSettings( ){ } + ~ConstantTimeDriftBiasSettings( ) { } //! Constant time drift bias that is to be considered for the observation time. /*! - * Constant time drift bias that is to be considered for the observation time. The size of this vector must be equal to the - * size of the observable to which it is assigned. + * Constant time drift bias that is to be considered for the observation time. The size of this + * vector must be equal to the size of the observable to which it is assigned. */ Eigen::VectorXd timeDriftBias_; @@ -220,45 +220,46 @@ class ConstantTimeDriftBiasSettings: public ObservationBiasSettings }; //! Class for defining settings for the creation of an arc-wise time drift bias model -class ArcWiseTimeDriftBiasSettings: public ObservationBiasSettings +class ArcWiseTimeDriftBiasSettings : public ObservationBiasSettings { -public: - + public: //! Constuctor /*! * Constuctor * \param arcStartTimes Start times for arcs in which biases (timeDriftBiases) are used * \param timeDriftBiases List of time drift biases per arc - * \param linkEndForTime Link end at which time is to be evaluated to determine current time (and current arc) - * \param referenceEpochs Reference epochs (per arc) at which the time drifts are initialised. + * \param linkEndForTime Link end at which time is to be evaluated to determine current time + * (and current arc) \param referenceEpochs Reference epochs (per arc) at which the time drifts + * are initialised. */ - ArcWiseTimeDriftBiasSettings( - const std::vector< double >& arcStartTimes, - const std::vector< Eigen::VectorXd >& timeDriftBiases, - const LinkEndType linkEndForTime, - const std::vector< double > referenceEpochs ): - ObservationBiasSettings( arc_wise_time_drift_bias ), - arcStartTimes_( arcStartTimes ), timeDriftBiases_( timeDriftBiases ), linkEndForTime_( linkEndForTime ), - referenceEpochs_( referenceEpochs ){ } + ArcWiseTimeDriftBiasSettings( const std::vector< double >& arcStartTimes, + const std::vector< Eigen::VectorXd >& timeDriftBiases, + const LinkEndType linkEndForTime, + const std::vector< double > referenceEpochs ) : + ObservationBiasSettings( arc_wise_time_drift_bias ), arcStartTimes_( arcStartTimes ), + timeDriftBiases_( timeDriftBiases ), linkEndForTime_( linkEndForTime ), + referenceEpochs_( referenceEpochs ) + { } //! Constuctor /*! * Constuctor - * \param timeDriftBiases Map of observation biases per arc, with bias as map value, and arc start time as map key - * \param linkEndForTime Link end at which time is to be evaluated to determine current time (and current arc) - * \param referenceEpochs Reference epochs (per arc) at which the time drifts are initialised. + * \param timeDriftBiases Map of observation biases per arc, with bias as map value, and arc + * start time as map key \param linkEndForTime Link end at which time is to be evaluated to + * determine current time (and current arc) \param referenceEpochs Reference epochs (per arc) at + * which the time drifts are initialised. */ - ArcWiseTimeDriftBiasSettings( - const std::map< double, Eigen::VectorXd >& timeDriftBiases, - const LinkEndType linkEndForTime, - const std::vector< double > referenceEpochs ): - ObservationBiasSettings( arc_wise_time_drift_bias ), - arcStartTimes_( utilities::createVectorFromMapKeys( timeDriftBiases ) ), - timeDriftBiases_( utilities::createVectorFromMapValues( timeDriftBiases ) ), linkEndForTime_( linkEndForTime ), - referenceEpochs_( referenceEpochs ){ } + ArcWiseTimeDriftBiasSettings( const std::map< double, Eigen::VectorXd >& timeDriftBiases, + const LinkEndType linkEndForTime, + const std::vector< double > referenceEpochs ) : + ObservationBiasSettings( arc_wise_time_drift_bias ), + arcStartTimes_( utilities::createVectorFromMapKeys( timeDriftBiases ) ), + timeDriftBiases_( utilities::createVectorFromMapValues( timeDriftBiases ) ), + linkEndForTime_( linkEndForTime ), referenceEpochs_( referenceEpochs ) + { } //! Destructor - ~ArcWiseTimeDriftBiasSettings( ){ } + ~ArcWiseTimeDriftBiasSettings( ) { } //! Start times for arcs in which biases (observationBiases) are used std::vector< double > arcStartTimes_; @@ -274,25 +275,23 @@ class ArcWiseTimeDriftBiasSettings: public ObservationBiasSettings }; //! Class for defining settings for the creation of a constant time bias -class ConstantTimeBiasSettings: public ObservationBiasSettings +class ConstantTimeBiasSettings : public ObservationBiasSettings { -public: - + public: //! Constuctor /*! * Constuctor - * \param timeBias Constant time bias that is to be considered for the observation time. The size of this vector must be equal to the - * size of the observable to which it is assigned. + * \param timeBias Constant time bias that is to be considered for the observation time. The + * size of this vector must be equal to the size of the observable to which it is assigned. * \param linkEndForTime Link end at which time is to be evaluated to determine current time */ - ConstantTimeBiasSettings( - const double timeBias, - const LinkEndType linkEndForTime ): - ObservationBiasSettings( constant_time_bias ), - timeBias_( timeBias ), linkEndForTime_( linkEndForTime ){ } + ConstantTimeBiasSettings( const double timeBias, const LinkEndType linkEndForTime ) : + ObservationBiasSettings( constant_time_bias ), timeBias_( timeBias ), + linkEndForTime_( linkEndForTime ) + { } //! Destructor - ~ConstantTimeBiasSettings( ){ } + ~ConstantTimeBiasSettings( ) { } //! Constant time bias that is to be considered for the observation time. /*! @@ -302,43 +301,44 @@ class ConstantTimeBiasSettings: public ObservationBiasSettings //! Link end at which time is to be evaluated to determine current time (and current arc) LinkEndType linkEndForTime_; - }; //! Class for defining settings for the creation of an arc-wise time bias model -class ArcWiseTimeBiasSettings: public ObservationBiasSettings +class ArcWiseTimeBiasSettings : public ObservationBiasSettings { -public: - + public: //! Constuctor /*! * Constuctor * \param arcStartTimes Start times for arcs in which biases (timeBiases) are used * \param timeBiases List of time biases per arc - * \param linkEndForTime Link end at which time is to be evaluated to determine current time (and current arc) + * \param linkEndForTime Link end at which time is to be evaluated to determine current time + * (and current arc) */ - ArcWiseTimeBiasSettings( - const std::vector< double >& arcStartTimes, - const std::vector< double >& timeBiases, - const LinkEndType linkEndForTime ): - ObservationBiasSettings( arc_wise_time_bias ), - arcStartTimes_( arcStartTimes ), timeBiases_( timeBiases ), linkEndForTime_( linkEndForTime ){ } + ArcWiseTimeBiasSettings( const std::vector< double >& arcStartTimes, + const std::vector< double >& timeBiases, + const LinkEndType linkEndForTime ) : + ObservationBiasSettings( arc_wise_time_bias ), arcStartTimes_( arcStartTimes ), + timeBiases_( timeBiases ), linkEndForTime_( linkEndForTime ) + { } //! Constuctor /*! * Constuctor - * \param timeBiases Map of observation biases per arc, with bias as map value, and arc start time as map key - * \param linkEndForTime Link end at which time is to be evaluated to determine current time (and current arc) + * \param timeBiases Map of observation biases per arc, with bias as map value, and arc start + * time as map key \param linkEndForTime Link end at which time is to be evaluated to determine + * current time (and current arc) */ - ArcWiseTimeBiasSettings( - const std::map< double, double >& timeBiases, - const LinkEndType linkEndForTime ): - ObservationBiasSettings( arc_wise_time_bias ), - arcStartTimes_( utilities::createVectorFromMapKeys( timeBiases ) ), - timeBiases_( utilities::createVectorFromMapValues( timeBiases ) ), linkEndForTime_( linkEndForTime ){ } + ArcWiseTimeBiasSettings( const std::map< double, double >& timeBiases, + const LinkEndType linkEndForTime ) : + ObservationBiasSettings( arc_wise_time_bias ), + arcStartTimes_( utilities::createVectorFromMapKeys( timeBiases ) ), + timeBiases_( utilities::createVectorFromMapValues( timeBiases ) ), + linkEndForTime_( linkEndForTime ) + { } //! Destructor - ~ArcWiseTimeBiasSettings( ){ } + ~ArcWiseTimeBiasSettings( ) { } //! Start times for arcs in which biases (observationBiases) are used std::vector< double > arcStartTimes_; @@ -348,71 +348,63 @@ class ArcWiseTimeBiasSettings: public ObservationBiasSettings //! Link end at which time is to be evaluated to determine current time (and current arc) LinkEndType linkEndForTime_; - }; - //! Class for defining settings for the creation of a multiple biases for a single observable -class TiminigSystemBiasSettings: public ObservationBiasSettings +class TiminigSystemBiasSettings : public ObservationBiasSettings { -public: - + public: //! Constructor /*! * Constructor * \param biasSettingsList List of settings for bias objects that are to be created. */ - TiminigSystemBiasSettings( - const std::string& bodyName, - const std::string& stationName ): - ObservationBiasSettings( clock_induced_bias ), - bodyName_( bodyName ), stationName_( stationName ){ } + TiminigSystemBiasSettings( const std::string& bodyName, const std::string& stationName ) : + ObservationBiasSettings( clock_induced_bias ), bodyName_( bodyName ), + stationName_( stationName ) + { } //! Destructor - ~TiminigSystemBiasSettings( ){ } + ~TiminigSystemBiasSettings( ) { } const std::string bodyName_; const std::string stationName_; }; -inline std::shared_ptr< ObservationBiasSettings > clockInducedBias( - const std::string& bodyName, const std::string& stationName ) +inline std::shared_ptr< ObservationBiasSettings > clockInducedBias( const std::string& bodyName, + const std::string& stationName ) { - return std::make_shared< TiminigSystemBiasSettings >( - bodyName, stationName ); + return std::make_shared< TiminigSystemBiasSettings >( bodyName, stationName ); } inline std::shared_ptr< ObservationBiasSettings > constantAbsoluteBias( const Eigen::VectorXd& observationBias ) { - return std::make_shared< ConstantObservationBiasSettings >( - observationBias, true ); + return std::make_shared< ConstantObservationBiasSettings >( observationBias, true ); } inline std::shared_ptr< ObservationBiasSettings > constantRelativeBias( const Eigen::VectorXd& observationBias ) { - return std::make_shared< ConstantObservationBiasSettings >( - observationBias, false ); + return std::make_shared< ConstantObservationBiasSettings >( observationBias, false ); } inline std::shared_ptr< ObservationBiasSettings > arcWiseAbsoluteBias( const std::vector< double >& arcStartTimes, const std::vector< Eigen::VectorXd >& observationBiases, - const LinkEndType linkEndForTime) + const LinkEndType linkEndForTime ) { return std::make_shared< ArcWiseConstantObservationBiasSettings >( - arcStartTimes, observationBiases, linkEndForTime, true ); + arcStartTimes, observationBiases, linkEndForTime, true ); } - inline std::shared_ptr< ObservationBiasSettings > arcWiseAbsoluteBias( const std::map< double, Eigen::VectorXd >& observationBiases, - const LinkEndType linkEndForTime) + const LinkEndType linkEndForTime ) { return std::make_shared< ArcWiseConstantObservationBiasSettings >( - observationBiases, linkEndForTime, true ); + observationBiases, linkEndForTime, true ); } inline std::shared_ptr< ObservationBiasSettings > arcWiseRelativeBias( @@ -421,7 +413,7 @@ inline std::shared_ptr< ObservationBiasSettings > arcWiseRelativeBias( const LinkEndType linkEndForTime ) { return std::make_shared< ArcWiseConstantObservationBiasSettings >( - arcStartTimes, observationBiases, linkEndForTime, false ); + arcStartTimes, observationBiases, linkEndForTime, false ); } inline std::shared_ptr< ObservationBiasSettings > arcWiseRelativeBias( @@ -429,14 +421,13 @@ inline std::shared_ptr< ObservationBiasSettings > arcWiseRelativeBias( const LinkEndType linkEndForTime ) { return std::make_shared< ArcWiseConstantObservationBiasSettings >( - observationBiases, linkEndForTime, false ); + observationBiases, linkEndForTime, false ); } inline std::shared_ptr< ObservationBiasSettings > multipleObservationBiasSettings( - const std::vector< std::shared_ptr< ObservationBiasSettings > > biasSettingsList ) + const std::vector< std::shared_ptr< ObservationBiasSettings > > biasSettingsList ) { - return std::make_shared< MultipleObservationBiasSettings >( - biasSettingsList ); + return std::make_shared< MultipleObservationBiasSettings >( biasSettingsList ); } inline std::shared_ptr< ObservationBiasSettings > constantTimeDriftBias( @@ -444,7 +435,8 @@ inline std::shared_ptr< ObservationBiasSettings > constantTimeDriftBias( const LinkEndType linkEndForTime, const double referenceEpoch ) { - return std::make_shared< ConstantTimeDriftBiasSettings >( timeDriftBias, linkEndForTime, referenceEpoch ); + return std::make_shared< ConstantTimeDriftBiasSettings >( + timeDriftBias, linkEndForTime, referenceEpoch ); } inline std::shared_ptr< ObservationBiasSettings > arcWiseTimeDriftBias( @@ -453,7 +445,8 @@ inline std::shared_ptr< ObservationBiasSettings > arcWiseTimeDriftBias( const LinkEndType linkEndForTime, const std::vector< double >& referenceEpochs ) { - return std::make_shared< ArcWiseTimeDriftBiasSettings >( arcStartTimes, timeDriftBiases, linkEndForTime, referenceEpochs ); + return std::make_shared< ArcWiseTimeDriftBiasSettings >( + arcStartTimes, timeDriftBiases, linkEndForTime, referenceEpochs ); } inline std::shared_ptr< ObservationBiasSettings > arcWiseTimeDriftBias( @@ -461,7 +454,8 @@ inline std::shared_ptr< ObservationBiasSettings > arcWiseTimeDriftBias( const LinkEndType linkEndForTime, const std::vector< double > referenceEpochs ) { - return std::make_shared< ArcWiseTimeDriftBiasSettings >( timeDriftBiases, linkEndForTime, referenceEpochs ); + return std::make_shared< ArcWiseTimeDriftBiasSettings >( + timeDriftBiases, linkEndForTime, referenceEpochs ); } inline std::shared_ptr< ObservationBiasSettings > constantTimeBias( @@ -472,14 +466,13 @@ inline std::shared_ptr< ObservationBiasSettings > constantTimeBias( } inline std::shared_ptr< ObservationBiasSettings > arcWiseTimeBias( - const std::vector< double >& arcStartTimes, - const LinkEndType linkEndForTime ) + const std::vector< double >& arcStartTimes, + const LinkEndType linkEndForTime ) { std::vector< double > timeBiases = std::vector< double >( arcStartTimes.size( ), 0.0 ); return std::make_shared< ArcWiseTimeBiasSettings >( arcStartTimes, timeBiases, linkEndForTime ); } - inline std::shared_ptr< ObservationBiasSettings > arcWiseTimeBias( const std::vector< double >& timeBiases, const std::vector< double >& arcStartTimes, @@ -495,41 +488,37 @@ inline std::shared_ptr< ObservationBiasSettings > arcWiseTimeBias( return std::make_shared< ArcWiseTimeBiasSettings >( timeBiases, linkEndForTime ); } -inline std::shared_ptr< ObservationBiasSettings > twoWayTimeScaleRangeBias( ) +inline std::shared_ptr< ObservationBiasSettings > twoWayTimeScaleRangeBias( ) { return std::make_shared< ObservationBiasSettings >( two_way_range_time_scale_bias ); } - //! Class used for defining the settings for an observation model that is to be created. /*! - * Class used for defining the settings for an observation model that is to be created. This class allows the type, light-time - * corrections and bias for the observation to be set. For observation models that require additional information (e.g. - * integration time, retransmission time, etc.), a specific derived class must be implemented. + * Class used for defining the settings for an observation model that is to be created. This class + * allows the type, light-time corrections and bias for the observation to be set. For observation + * models that require additional information (e.g. integration time, retransmission time, etc.), a + * specific derived class must be implemented. */ class ObservationModelSettings { -public: - - + public: //! Constructor /*! * Constructor (single light-time correction) * \param observableType Type of observation model that is to be created - * \param lightTimeCorrections Settings for a single light-time correction that is to be used for the observation model - * (nullptr if none) - * \param biasSettings Settings for the observation bias model that is to be used (default none: nullptr) + * \param lightTimeCorrections Settings for a single light-time correction that is to be used + * for the observation model (nullptr if none) \param biasSettings Settings for the observation + * bias model that is to be used (default none: nullptr) */ ObservationModelSettings( const observation_models::ObservableType observableType, const LinkDefinition linkEnds, const std::shared_ptr< LightTimeCorrectionSettings > lightTimeCorrections, const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr, - const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria - = std::make_shared< LightTimeConvergenceCriteria >( ) ): - observableType_( observableType ), - linkEnds_( linkEnds ), - biasSettings_( biasSettings ), + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ) ) : + observableType_( observableType ), linkEnds_( linkEnds ), biasSettings_( biasSettings ), lightTimeConvergenceCriteria_( lightTimeConvergenceCriteria ) { if( lightTimeCorrections != nullptr ) @@ -542,26 +531,26 @@ class ObservationModelSettings /*! * Constructor (multiple light-time correction) * \param observableType Type of observation model that is to be created - * \param lightTimeCorrectionsList List of settings for a single light-time correction that is to be used for the observation - * model - * \param biasSettings Settings for the observation bias model that is to be used (default none: nullptr) + * \param lightTimeCorrectionsList List of settings for a single light-time correction that is + * to be used for the observation model \param biasSettings Settings for the observation bias + * model that is to be used (default none: nullptr) */ ObservationModelSettings( const observation_models::ObservableType observableType, const LinkDefinition linkEnds, - const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList = - std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + 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 >( ) ): - observableType_( observableType ), - linkEnds_( linkEnds ), - lightTimeCorrectionsList_( lightTimeCorrectionsList ), - biasSettings_( biasSettings ), - lightTimeConvergenceCriteria_( lightTimeConvergenceCriteria ){ } + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ) ) : + observableType_( observableType ), linkEnds_( linkEnds ), + lightTimeCorrectionsList_( lightTimeCorrectionsList ), biasSettings_( biasSettings ), + lightTimeConvergenceCriteria_( lightTimeConvergenceCriteria ) + { } //! Destructor - virtual ~ObservationModelSettings( ){ } + virtual ~ObservationModelSettings( ) { } //! Type of observation model that is to be created observation_models::ObservableType observableType_; @@ -580,11 +569,8 @@ class ObservationModelSettings std::vector< LinkDefinition > getObservationModelListLinkEnds( const std::vector< std::shared_ptr< ObservationModelSettings > >& observationModelList ); - - //! Enum defining all possible types of proper time rate computations in one-way Doppler -enum DopplerProperTimeRateType -{ +enum DopplerProperTimeRateType { custom_doppler_proper_time_rate, direct_first_order_doppler_proper_time_rate }; @@ -592,97 +578,112 @@ enum DopplerProperTimeRateType //! Base class to define the settings for proper time rate (at a single link end) in one-way Doppler mode. class DopplerProperTimeRateSettings { -public: - DopplerProperTimeRateSettings( const DopplerProperTimeRateType dopplerProperTimeRateType ): - dopplerProperTimeRateType_( dopplerProperTimeRateType ){ } + public: + DopplerProperTimeRateSettings( const DopplerProperTimeRateType dopplerProperTimeRateType ) : + dopplerProperTimeRateType_( dopplerProperTimeRateType ) + { } - virtual ~DopplerProperTimeRateSettings( ){ } + virtual ~DopplerProperTimeRateSettings( ) { } DopplerProperTimeRateType dopplerProperTimeRateType_; }; //! Class to define the settings for first-order, single body, proper time rate (at a single link end) in one-way Doppler mode. -class DirectFirstOrderDopplerProperTimeRateSettings: public DopplerProperTimeRateSettings +class DirectFirstOrderDopplerProperTimeRateSettings : public DopplerProperTimeRateSettings { -public: - + public: //! Constructor /*! * Constructor - * \param centralBodyName 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 + * \param centralBodyName 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 */ - DirectFirstOrderDopplerProperTimeRateSettings( - const std::string centralBodyName ): + DirectFirstOrderDopplerProperTimeRateSettings( const std::string centralBodyName ) : DopplerProperTimeRateSettings( direct_first_order_doppler_proper_time_rate ), - centralBodyName_( centralBodyName ){ } + centralBodyName_( centralBodyName ) + { } //! Destructor. - ~DirectFirstOrderDopplerProperTimeRateSettings( ){ } + ~DirectFirstOrderDopplerProperTimeRateSettings( ) { } //! Name of central body /*! - * 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 + * 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_; }; //! Class to define the settings for one-way Doppler observable -class OneWayDopplerObservationSettings: public ObservationModelSettings +class OneWayDopplerObservationSettings : public ObservationModelSettings { -public: - + public: //! Constructor /*! * Constructor - * \param lightTimeCorrections Settings for a single light-time correction that is to be used for the observation model - * (nullptr if none) - * \param transmitterProperTimeRateSettings Settings for proper time rate at transmitter - * \param receiverProperTimeRateSettings Settings for proper time rate at receiver - * \param biasSettings Settings for the observation bias model that is to be used (default none: NUL + * \param lightTimeCorrections Settings for a single light-time correction that is to be used + * for the observation model (nullptr if none) \param transmitterProperTimeRateSettings Settings + * for proper time rate at transmitter \param receiverProperTimeRateSettings Settings for proper + * time rate at receiver \param biasSettings Settings for the observation bias model that is to + * be used (default none: NUL */ OneWayDopplerObservationSettings( const LinkDefinition& linkEnds, const std::shared_ptr< LightTimeCorrectionSettings > lightTimeCorrections, - const std::shared_ptr< DopplerProperTimeRateSettings > transmitterProperTimeRateSettings = nullptr, - const std::shared_ptr< DopplerProperTimeRateSettings > receiverProperTimeRateSettings = nullptr, + const std::shared_ptr< DopplerProperTimeRateSettings > + transmitterProperTimeRateSettings = nullptr, + const std::shared_ptr< DopplerProperTimeRateSettings > receiverProperTimeRateSettings = + nullptr, const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr, - const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria - = std::make_shared< LightTimeConvergenceCriteria >( ), - const bool normalizeWithSpeedOfLight = false ): - ObservationModelSettings( - one_way_doppler, linkEnds, lightTimeCorrections, biasSettings, lightTimeConvergenceCriteria ), + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ), + const bool normalizeWithSpeedOfLight = false ) : + ObservationModelSettings( one_way_doppler, + linkEnds, + lightTimeCorrections, + biasSettings, + lightTimeConvergenceCriteria ), transmitterProperTimeRateSettings_( transmitterProperTimeRateSettings ), receiverProperTimeRateSettings_( receiverProperTimeRateSettings ), - normalizeWithSpeedOfLight_( normalizeWithSpeedOfLight ){ } + normalizeWithSpeedOfLight_( normalizeWithSpeedOfLight ) + { } //! Constructor /*! * Constructor - * \param lightTimeCorrectionsList List of settings for a single light-time correction that is to be used for the observation - * model (empty if none) - * \param transmitterProperTimeRateSettings Settings for proper time rate at transmitter - * \param receiverProperTimeRateSettings Settings for proper time rate at receiver - * \param biasSettings Settings for the observation bias model that is to be used (default none: NUL + * \param lightTimeCorrectionsList List of settings for a single light-time correction that is + * to be used for the observation model (empty if none) \param transmitterProperTimeRateSettings + * Settings for proper time rate at transmitter \param receiverProperTimeRateSettings Settings + * for proper time rate at receiver \param biasSettings Settings for the observation bias model + * that is to be used (default none: NUL */ OneWayDopplerObservationSettings( const LinkDefinition& linkEnds, - const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList = - std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), - const std::shared_ptr< DopplerProperTimeRateSettings > transmitterProperTimeRateSettings = nullptr, - const std::shared_ptr< DopplerProperTimeRateSettings > receiverProperTimeRateSettings = nullptr, + const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > + lightTimeCorrectionsList = + std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + const std::shared_ptr< DopplerProperTimeRateSettings > + transmitterProperTimeRateSettings = nullptr, + const std::shared_ptr< DopplerProperTimeRateSettings > receiverProperTimeRateSettings = + nullptr, const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr, - const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria - = std::make_shared< LightTimeConvergenceCriteria >( ), - const bool normalizeWithSpeedOfLight = false ): - ObservationModelSettings( one_way_doppler, linkEnds, lightTimeCorrectionsList, biasSettings, lightTimeConvergenceCriteria ), + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ), + const bool normalizeWithSpeedOfLight = false ) : + ObservationModelSettings( one_way_doppler, + linkEnds, + lightTimeCorrectionsList, + biasSettings, + lightTimeConvergenceCriteria ), transmitterProperTimeRateSettings_( transmitterProperTimeRateSettings ), receiverProperTimeRateSettings_( receiverProperTimeRateSettings ), - normalizeWithSpeedOfLight_( normalizeWithSpeedOfLight ){ } + normalizeWithSpeedOfLight_( normalizeWithSpeedOfLight ) + { } //! Destructor - ~OneWayDopplerObservationSettings( ){ } + ~OneWayDopplerObservationSettings( ) { } //! Settings for proper time rate at transmitter std::shared_ptr< DopplerProperTimeRateSettings > transmitterProperTimeRateSettings_; @@ -693,33 +694,37 @@ class OneWayDopplerObservationSettings: public ObservationModelSettings bool normalizeWithSpeedOfLight_; }; - - //! Class to define the settings for one-way Doppler observable -class TwoWayDopplerObservationSettings: public ObservationModelSettings +class TwoWayDopplerObservationSettings : public ObservationModelSettings { -public: - + public: //! Constructor /*! * Constructor * \param uplinkOneWayDopplerSettings Settings for the one-way Doppler model of the uplink * \param downlinkOneWayDopplerSettings Settings for the one-way Doppler model of the downlink - * \param biasSettings Settings for the observation bias model that is to be used (default none: NUL + * \param biasSettings Settings for the observation bias model that is to be used (default none: + * NUL */ TwoWayDopplerObservationSettings( const std::shared_ptr< OneWayDopplerObservationSettings > uplinkOneWayDopplerSettings, const std::shared_ptr< OneWayDopplerObservationSettings > downlinkOneWayDopplerSettings, - const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr ): - ObservationModelSettings( two_way_doppler, mergeUpDownLink( - uplinkOneWayDopplerSettings->linkEnds_, downlinkOneWayDopplerSettings->linkEnds_ ), - std::shared_ptr< LightTimeCorrectionSettings >( ), biasSettings, nullptr ), + const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr ) : + ObservationModelSettings( two_way_doppler, + mergeUpDownLink( uplinkOneWayDopplerSettings->linkEnds_, + downlinkOneWayDopplerSettings->linkEnds_ ), + std::shared_ptr< LightTimeCorrectionSettings >( ), + biasSettings, + nullptr ), uplinkOneWayDopplerSettings_( uplinkOneWayDopplerSettings ), downlinkOneWayDopplerSettings_( downlinkOneWayDopplerSettings ) { - if( uplinkOneWayDopplerSettings->normalizeWithSpeedOfLight_ != downlinkOneWayDopplerSettings->normalizeWithSpeedOfLight_ ) + if( uplinkOneWayDopplerSettings->normalizeWithSpeedOfLight_ != + downlinkOneWayDopplerSettings->normalizeWithSpeedOfLight_ ) { - throw std::runtime_error( "Error when making two-way Doppler observable; up and downlink normalization not consistent." ); + throw std::runtime_error( + "Error when making two-way Doppler observable; up and downlink normalization " + "not consistent." ); } else { @@ -729,26 +734,39 @@ class TwoWayDopplerObservationSettings: public ObservationModelSettings TwoWayDopplerObservationSettings( const LinkDefinition& linkEnds, - const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList = - std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + 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 >( ), - const bool normalizeWithSpeedOfLight = false ): - ObservationModelSettings( two_way_doppler, linkEnds, - lightTimeCorrectionsList, biasSettings ), normalizeWithSpeedOfLight_( normalizeWithSpeedOfLight ) + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ), + const bool normalizeWithSpeedOfLight = false ) : + ObservationModelSettings( two_way_doppler, + linkEnds, + lightTimeCorrectionsList, + biasSettings ), + normalizeWithSpeedOfLight_( normalizeWithSpeedOfLight ) { uplinkOneWayDopplerSettings_ = std::make_shared< OneWayDopplerObservationSettings >( - getUplinkFromTwoWayLinkEnds( linkEnds ), lightTimeCorrectionsList, - nullptr, nullptr, nullptr, lightTimeConvergenceCriteria, normalizeWithSpeedOfLight ); + getUplinkFromTwoWayLinkEnds( linkEnds ), + lightTimeCorrectionsList, + nullptr, + nullptr, + nullptr, + lightTimeConvergenceCriteria, + normalizeWithSpeedOfLight ); downlinkOneWayDopplerSettings_ = std::make_shared< OneWayDopplerObservationSettings >( - getDownlinkFromTwoWayLinkEnds( linkEnds ), lightTimeCorrectionsList, - nullptr, nullptr, nullptr, lightTimeConvergenceCriteria, normalizeWithSpeedOfLight ); + getDownlinkFromTwoWayLinkEnds( linkEnds ), + lightTimeCorrectionsList, + nullptr, + nullptr, + nullptr, + lightTimeConvergenceCriteria, + normalizeWithSpeedOfLight ); } - //! Destructor - ~TwoWayDopplerObservationSettings( ){ } + ~TwoWayDopplerObservationSettings( ) { } //! Settings for the one-way Doppler model of the uplink std::shared_ptr< OneWayDopplerObservationSettings > uplinkOneWayDopplerSettings_; @@ -759,46 +777,50 @@ class TwoWayDopplerObservationSettings: public ObservationModelSettings bool normalizeWithSpeedOfLight_; }; - - - -////! Class to define the settings for one-way differenced range-rate (e.g. closed-loop Doppler) observable -//class OneWayDifferencedRangeRateObservationSettings: public ObservationModelSettings +////! Class to define the settings for one-way differenced range-rate (e.g. closed-loop Doppler) +/// observable +// class OneWayDifferencedRangeRateObservationSettings: public ObservationModelSettings //{ -//public: +// public: // //! Constructor // /*! // * Constructor -// * \param integrationTimeFunction Function that returns the integration time of observable as a function of time -// * \param lightTimeCorrections Settings for a single light-time correction that is to be used for the observation model +// * \param integrationTimeFunction Function that returns the integration time of observable as +// a function of time +// * \param lightTimeCorrections Settings for a single light-time correction that is to be used +// for the observation model // * (nullptr if none) -// * \param biasSettings Settings for the observation bias model that is to be used (default none: nullptr) +// * \param biasSettings Settings for the observation bias model that is to be used (default +// none: nullptr) // */ // OneWayDifferencedRangeRateObservationSettings( // const LinkDefinition& linkEnds, // const std::function< double( const double ) > integrationTimeFunction, // const std::shared_ptr< LightTimeCorrectionSettings > lightTimeCorrections, // const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr ): -// ObservationModelSettings( one_way_differenced_range, linkEnds, lightTimeCorrections, biasSettings ), -// integrationTimeFunction_( integrationTimeFunction ){ } +// ObservationModelSettings( one_way_differenced_range, linkEnds, lightTimeCorrections, +// biasSettings ), integrationTimeFunction_( integrationTimeFunction ){ } // //! Constructor // /*! // * Constructor -// * \param integrationTimeFunction Function that returns the integration time of observable as a function of time -// * \param lightTimeCorrectionsList List of ettings for a single light-time correction that is to be used for the observation model +// * \param integrationTimeFunction Function that returns the integration time of observable as +// a function of time +// * \param lightTimeCorrectionsList List of ettings for a single light-time correction that is +// to be used for the observation model // * (empty if none) -// * \param biasSettings Settings for the observation bias model that is to be used (default none: nullptr) +// * \param biasSettings Settings for the observation bias model that is to be used (default +// none: nullptr) // */ // OneWayDifferencedRangeRateObservationSettings( // const LinkDefinition& linkEnds, // const std::function< double( const double ) > integrationTimeFunction, -// const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList = -// std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), -// const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr ): -// ObservationModelSettings( one_way_differenced_range, linkEnds, lightTimeCorrectionsList, biasSettings ), -// integrationTimeFunction_( integrationTimeFunction ){ } +// const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > +// lightTimeCorrectionsList = std::vector< std::shared_ptr< LightTimeCorrectionSettings > +// >( ), const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr ): +// ObservationModelSettings( one_way_differenced_range, linkEnds, lightTimeCorrectionsList, +// biasSettings ), integrationTimeFunction_( integrationTimeFunction ){ } // //! Destructor // ~OneWayDifferencedRangeRateObservationSettings( ){ } @@ -808,153 +830,178 @@ class TwoWayDopplerObservationSettings: public ObservationModelSettings //}; - //! Class to define the settings for one-way differenced range-rate (e.g. closed-loop Doppler) observable -class NWayRangeObservationSettings: public ObservationModelSettings +class NWayRangeObservationSettings : public ObservationModelSettings { -public: - + public: //! Constructor /*! * Constructor - * \param oneWayRangeObsevationSettings List of settings for one-way observables that make up n-way link (each must be for - * one_way_range_ - * \param biasSettings Settings for the observation bias model that is to be used (default none: nullptr) + * \param oneWayRangeObservationSettings List of settings for one-way observables that make up + * n-way link (each must be for one_way_range_ \param biasSettings Settings for the observation + * bias model that is to be used (default none: nullptr) */ NWayRangeObservationSettings( - const std::vector< std::shared_ptr< ObservationModelSettings > > oneWayRangeObsevationSettings, + const std::vector< std::shared_ptr< ObservationModelSettings > > + oneWayRangeObservationSettings, const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr, - const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria - = std::make_shared< LightTimeConvergenceCriteria >( ) ): - ObservationModelSettings( n_way_range, mergeOneWayLinkEnds( getObservationModelListLinkEnds( oneWayRangeObsevationSettings ) ), - std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), biasSettings, nullptr ), - oneWayRangeObsevationSettings_( oneWayRangeObsevationSettings ), - multiLegLightTimeConvergenceCriteria_( lightTimeConvergenceCriteria ){ } + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ) ) : + ObservationModelSettings( n_way_range, + mergeOneWayLinkEnds( getObservationModelListLinkEnds( + oneWayRangeObservationSettings ) ), + std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + biasSettings, + nullptr ), + oneWayRangeObservationSettings_( oneWayRangeObservationSettings ), + multiLegLightTimeConvergenceCriteria_( lightTimeConvergenceCriteria ) + { } //! Constructor /*! * Constructor for same light-time corrections per link - * \param lightTimeCorrections Settings for a single light-time correction that is to be used for the observation model - * (nullptr if none) - * \param biasSettings Settings for the observation bias model that is to be used (default none: nullptr) + * \param lightTimeCorrections Settings for a single light-time correction that is to be used + * for the observation model (nullptr if none) \param biasSettings Settings for the observation + * bias model that is to be used (default none: nullptr) */ NWayRangeObservationSettings( const LinkDefinition& linkEnds, - const std::vector< std::shared_ptr< LightTimeCorrectionSettings > >& lightTimeCorrectionsList, + const std::vector< std::shared_ptr< LightTimeCorrectionSettings > >& + lightTimeCorrectionsList, const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr, - const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria - = std::make_shared< LightTimeConvergenceCriteria >( ) ): - ObservationModelSettings( n_way_range, linkEnds, std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), biasSettings ), + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ) ) : + ObservationModelSettings( n_way_range, + linkEnds, + std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + biasSettings ), multiLegLightTimeConvergenceCriteria_( lightTimeConvergenceCriteria ) { for( unsigned int i = 0; i < linkEnds.size( ) - 1; i++ ) { - oneWayRangeObsevationSettings_.push_back( - std::make_shared< ObservationModelSettings >( - one_way_range, getSingleLegLinkEnds( linkEnds.linkEnds_, i ), lightTimeCorrectionsList, - nullptr, lightTimeConvergenceCriteria ) ); + oneWayRangeObservationSettings_.push_back( std::make_shared< ObservationModelSettings >( + one_way_range, + getSingleLegLinkEnds( linkEnds.linkEnds_, i ), + lightTimeCorrectionsList, + nullptr, + lightTimeConvergenceCriteria ) ); } } //! Destructor - ~NWayRangeObservationSettings( ){ } + ~NWayRangeObservationSettings( ) { } - std::vector< std::shared_ptr< ObservationModelSettings > > oneWayRangeObsevationSettings_; + std::vector< std::shared_ptr< ObservationModelSettings > > oneWayRangeObservationSettings_; std::shared_ptr< LightTimeConvergenceCriteria > multiLegLightTimeConvergenceCriteria_; - }; -class NWayDifferencedRangeObservationSettings: public ObservationModelSettings +class NWayDifferencedRangeObservationSettings : public ObservationModelSettings { -public: - + public: NWayDifferencedRangeObservationSettings( const LinkDefinition& linkEnds, - const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList = - std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + 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 >( ) ): - ObservationModelSettings( n_way_differenced_range, linkEnds, lightTimeCorrectionsList, biasSettings ), + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ) ) : + ObservationModelSettings( n_way_differenced_range, + linkEnds, + lightTimeCorrectionsList, + biasSettings ), multiLegLightTimeConvergenceCriteria_( lightTimeConvergenceCriteria ) { for( unsigned int i = 0; i < linkEnds.size( ) - 1; i++ ) { - oneWayRangeObsevationSettings_.push_back( - std::make_shared< ObservationModelSettings >( - one_way_range, getSingleLegLinkEnds( linkEnds.linkEnds_, i ), lightTimeCorrectionsList, nullptr, - lightTimeConvergenceCriteria ) ); + oneWayRangeObservationSettings_.push_back( std::make_shared< ObservationModelSettings >( + one_way_range, + getSingleLegLinkEnds( linkEnds.linkEnds_, i ), + lightTimeCorrectionsList, + nullptr, + lightTimeConvergenceCriteria ) ); } } NWayDifferencedRangeObservationSettings( - const std::vector< std::shared_ptr< ObservationModelSettings > > oneWayRangeObsevationSettings, + const std::vector< std::shared_ptr< ObservationModelSettings > > + oneWayRangeObservationSettings, const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr, - const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria - = std::make_shared< LightTimeConvergenceCriteria >( ) ): + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ) ) : ObservationModelSettings( n_way_differenced_range, - mergeOneWayLinkEnds( getObservationModelListLinkEnds( oneWayRangeObsevationSettings ) ), - std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), biasSettings ), - oneWayRangeObsevationSettings_( oneWayRangeObsevationSettings ), - multiLegLightTimeConvergenceCriteria_( lightTimeConvergenceCriteria ) { } + mergeOneWayLinkEnds( getObservationModelListLinkEnds( + oneWayRangeObservationSettings ) ), + std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + biasSettings ), + oneWayRangeObservationSettings_( oneWayRangeObservationSettings ), + multiLegLightTimeConvergenceCriteria_( lightTimeConvergenceCriteria ) + { } std::shared_ptr< ObservationModelSettings > getUndifferencedObservationSettings( ) { return std::make_shared< NWayRangeObservationSettings >( - oneWayRangeObsevationSettings_, nullptr, multiLegLightTimeConvergenceCriteria_ ); + oneWayRangeObservationSettings_, nullptr, multiLegLightTimeConvergenceCriteria_ ); } -private: - - std::vector< std::shared_ptr< ObservationModelSettings > > oneWayRangeObsevationSettings_; + private: + std::vector< std::shared_ptr< ObservationModelSettings > > oneWayRangeObservationSettings_; std::shared_ptr< LightTimeConvergenceCriteria > multiLegLightTimeConvergenceCriteria_; - }; -class DsnNWayAveragedDopplerObservationSettings: public ObservationModelSettings +class DsnNWayAveragedDopplerObservationSettings : public ObservationModelSettings { -public: - + public: DsnNWayAveragedDopplerObservationSettings( const LinkDefinition& linkEnds, - const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList = - std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + 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 >( ), - const bool subtractDopplerSignature = true ): - ObservationModelSettings( dsn_n_way_averaged_doppler, linkEnds, lightTimeCorrectionsList, biasSettings ), + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ), + const bool subtractDopplerSignature = true ) : + ObservationModelSettings( dsn_n_way_averaged_doppler, + linkEnds, + lightTimeCorrectionsList, + biasSettings ), multiLegLightTimeConvergenceCriteria_( lightTimeConvergenceCriteria ), subtractDopplerSignature_( subtractDopplerSignature ) { for( unsigned int i = 0; i < linkEnds.size( ) - 1; i++ ) { - oneWayRangeObsevationSettings_.push_back( - std::make_shared< ObservationModelSettings >( - one_way_range, getSingleLegLinkEnds( linkEnds.linkEnds_, i ), lightTimeCorrectionsList, nullptr, - lightTimeConvergenceCriteria ) ); + oneWayRangeObservationSettings_.push_back( std::make_shared< ObservationModelSettings >( + one_way_range, + getSingleLegLinkEnds( linkEnds.linkEnds_, i ), + lightTimeCorrectionsList, + nullptr, + lightTimeConvergenceCriteria ) ); } } DsnNWayAveragedDopplerObservationSettings( - const std::vector< std::shared_ptr< ObservationModelSettings > > oneWayRangeObsevationSettings, + const std::vector< std::shared_ptr< ObservationModelSettings > > + oneWayRangeObservationSettings, const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr, - const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria - = std::make_shared< LightTimeConvergenceCriteria >( ), - const bool subtractDopplerSignature = true ): + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ), + const bool subtractDopplerSignature = true ) : ObservationModelSettings( n_way_differenced_range, - mergeOneWayLinkEnds( getObservationModelListLinkEnds( oneWayRangeObsevationSettings ) ), - std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), biasSettings ), - oneWayRangeObsevationSettings_( oneWayRangeObsevationSettings ), + mergeOneWayLinkEnds( getObservationModelListLinkEnds( + oneWayRangeObservationSettings ) ), + std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + biasSettings ), + oneWayRangeObservationSettings_( oneWayRangeObservationSettings ), multiLegLightTimeConvergenceCriteria_( lightTimeConvergenceCriteria ), - subtractDopplerSignature_( subtractDopplerSignature ){ } + subtractDopplerSignature_( subtractDopplerSignature ) + { } std::shared_ptr< ObservationModelSettings > getNWayRangeObservationSettings( ) { return std::make_shared< NWayRangeObservationSettings >( - oneWayRangeObsevationSettings_, nullptr, multiLegLightTimeConvergenceCriteria_ ); + oneWayRangeObservationSettings_, nullptr, multiLegLightTimeConvergenceCriteria_ ); } bool getSubtractDopplerSignature( ) @@ -962,63 +1009,99 @@ class DsnNWayAveragedDopplerObservationSettings: public ObservationModelSettings return subtractDopplerSignature_; } - -private: - std::vector< std::shared_ptr< ObservationModelSettings > > oneWayRangeObsevationSettings_; + private: + std::vector< std::shared_ptr< ObservationModelSettings > > oneWayRangeObservationSettings_; std::shared_ptr< LightTimeConvergenceCriteria > multiLegLightTimeConvergenceCriteria_; bool subtractDopplerSignature_; +}; +class DsnNWayRangeObservationSettings : public NWayRangeObservationSettings +{ + public: + DsnNWayRangeObservationSettings( + const LinkDefinition& linkEnds, + 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 >( ) ) : + NWayRangeObservationSettings( linkEnds, + lightTimeCorrectionsList, + biasSettings, + lightTimeConvergenceCriteria ) + { } + + DsnNWayRangeObservationSettings( + const std::vector< std::shared_ptr< ObservationModelSettings > > + oneWayRangeObservationSettings, + const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr, + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ) ) : + NWayRangeObservationSettings( oneWayRangeObservationSettings, + biasSettings, + lightTimeConvergenceCriteria ) + { } }; inline std::shared_ptr< ObservationModelSettings > oneWayRangeSettings( const LinkDefinition& linkEnds, - const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList = - std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + 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 >( ) ) + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ) ) { - return std::make_shared< ObservationModelSettings >( - one_way_range, linkEnds, lightTimeCorrectionsList, biasSettings, lightTimeConvergenceCriteria ); + return std::make_shared< ObservationModelSettings >( one_way_range, + linkEnds, + lightTimeCorrectionsList, + biasSettings, + lightTimeConvergenceCriteria ); } inline std::shared_ptr< ObservationModelSettings > angularPositionSettings( const LinkDefinition& linkEnds, - const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList = - std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + 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 >( ) ) + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ) ) { - return std::make_shared< ObservationModelSettings >( - angular_position, linkEnds, lightTimeCorrectionsList, biasSettings, lightTimeConvergenceCriteria ); + return std::make_shared< ObservationModelSettings >( angular_position, + linkEnds, + lightTimeCorrectionsList, + biasSettings, + lightTimeConvergenceCriteria ); } inline std::shared_ptr< ObservationModelSettings > relativeAngularPositionSettings( const LinkDefinition& linkEnds, - const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList = - std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + 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 >( ) ) + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ) ) { return std::make_shared< ObservationModelSettings >( - relative_angular_position, linkEnds, lightTimeCorrectionsList, biasSettings ); + relative_angular_position, linkEnds, lightTimeCorrectionsList, biasSettings ); } inline std::shared_ptr< ObservationModelSettings > positionObservableSettings( const LinkDefinition& linkEnds, - const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr) + const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr ) { return std::make_shared< ObservationModelSettings >( - position_observable, linkEnds, nullptr, biasSettings ); + position_observable, linkEnds, nullptr, biasSettings ); } inline std::shared_ptr< ObservationModelSettings > relativePositionObservableSettings( const LinkDefinition& linkEnds, - const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr) + const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr ) { return std::make_shared< ObservationModelSettings >( relative_position_observable, linkEnds, nullptr, biasSettings ); @@ -1026,34 +1109,41 @@ inline std::shared_ptr< ObservationModelSettings > relativePositionObservableSet inline std::shared_ptr< ObservationModelSettings > velocityObservableSettings( const LinkDefinition& linkEnds, - const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr) + const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr ) { return std::make_shared< ObservationModelSettings >( - velocity_observable, linkEnds, nullptr, biasSettings ); + velocity_observable, linkEnds, nullptr, biasSettings ); } inline std::shared_ptr< ObservationModelSettings > eulerAngle313ObservableSettings( const LinkDefinition& linkEnds, - const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr) + const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr ) { return std::make_shared< ObservationModelSettings >( - euler_angle_313_observable, linkEnds, nullptr, biasSettings ); + euler_angle_313_observable, linkEnds, nullptr, biasSettings ); } inline std::shared_ptr< ObservationModelSettings > oneWayOpenLoopDoppler( const LinkDefinition& linkEnds, - const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList = - std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + 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< DopplerProperTimeRateSettings > transmitterProperTimeRateSettings = nullptr, - const std::shared_ptr< DopplerProperTimeRateSettings > receiverProperTimeRateSettings = nullptr, - const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria - = std::make_shared< LightTimeConvergenceCriteria >( ), + const std::shared_ptr< DopplerProperTimeRateSettings > transmitterProperTimeRateSettings = + nullptr, + const std::shared_ptr< DopplerProperTimeRateSettings > receiverProperTimeRateSettings = + nullptr, + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ), const bool normalizeWithSpeedOfLight = false ) { - return std::make_shared< OneWayDopplerObservationSettings >( - linkEnds, lightTimeCorrectionsList, transmitterProperTimeRateSettings, receiverProperTimeRateSettings, - biasSettings, lightTimeConvergenceCriteria, normalizeWithSpeedOfLight ); + return std::make_shared< OneWayDopplerObservationSettings >( linkEnds, + lightTimeCorrectionsList, + transmitterProperTimeRateSettings, + receiverProperTimeRateSettings, + biasSettings, + lightTimeConvergenceCriteria, + normalizeWithSpeedOfLight ); } inline std::shared_ptr< ObservationModelSettings > twoWayOpenLoopDoppler( @@ -1062,174 +1152,218 @@ inline std::shared_ptr< ObservationModelSettings > twoWayOpenLoopDoppler( const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr ) { return std::make_shared< TwoWayDopplerObservationSettings >( - uplinkOneWayDopplerSettings, downlinkOneWayDopplerSettings, biasSettings ); + uplinkOneWayDopplerSettings, downlinkOneWayDopplerSettings, biasSettings ); } - inline std::shared_ptr< ObservationModelSettings > twoWayOpenLoopDoppler( const LinkDefinition& linkEnds, - const std::vector< std::shared_ptr< LightTimeCorrectionSettings > >& lightTimeCorrectionsList = std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + 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 >( ), + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ), const bool normalizeWithSpeedOfLight = false ) { - return std::make_shared< TwoWayDopplerObservationSettings >( - linkEnds, lightTimeCorrectionsList, biasSettings, lightTimeConvergenceCriteria, normalizeWithSpeedOfLight ); + return std::make_shared< TwoWayDopplerObservationSettings >( linkEnds, + lightTimeCorrectionsList, + biasSettings, + lightTimeConvergenceCriteria, + normalizeWithSpeedOfLight ); } inline std::shared_ptr< ObservationModelSettings > dopplerMeasuredFrequencyObservationSettings( const LinkDefinition& linkEnds, - const std::vector< std::shared_ptr< LightTimeCorrectionSettings > >& lightTimeCorrectionsList = std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + 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 >( )) + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ) ) { - return std::make_shared< ObservationModelSettings >( - doppler_measured_frequency, - linkEnds, lightTimeCorrectionsList, biasSettings, lightTimeConvergenceCriteria); + return std::make_shared< ObservationModelSettings >( doppler_measured_frequency, + linkEnds, + lightTimeCorrectionsList, + biasSettings, + lightTimeConvergenceCriteria ); } inline std::shared_ptr< ObservationModelSettings > oneWayClosedLoopDoppler( const LinkDefinition& linkEnds, - const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList = - std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + 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 >( ) ) + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ) ) { - return std::make_shared< ObservationModelSettings >( - one_way_differenced_range, linkEnds, lightTimeCorrectionsList, biasSettings, lightTimeConvergenceCriteria ); + return std::make_shared< ObservationModelSettings >( one_way_differenced_range, + linkEnds, + lightTimeCorrectionsList, + biasSettings, + lightTimeConvergenceCriteria ); } inline std::shared_ptr< ObservationModelSettings > twoWayDifferencedRangeObservationSettings( const LinkDefinition& linkEnds, - const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList = - std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + 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 >( ) ) + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ) ) { if( linkEnds.linkEnds_.size( ) != 3 ) { - throw std::runtime_error( "Error when creating two-way averaged Doppler settings, number of input link ends (" + - std::to_string( linkEnds.linkEnds_.size( ) ) + ") is incompatible." ); + throw std::runtime_error( + "Error when creating two-way averaged Doppler settings, number of input link ends " + "(" + + std::to_string( linkEnds.linkEnds_.size( ) ) + ") is incompatible." ); } return std::make_shared< NWayDifferencedRangeObservationSettings >( - linkEnds, lightTimeCorrectionsList, biasSettings, lightTimeConvergenceCriteria ); + linkEnds, lightTimeCorrectionsList, biasSettings, lightTimeConvergenceCriteria ); } inline std::shared_ptr< ObservationModelSettings > twoWayDifferencedRangeObservationSettings( - const std::vector< std::shared_ptr< ObservationModelSettings > > oneWayRangeObsevationSettings, + const std::vector< std::shared_ptr< ObservationModelSettings > > + oneWayRangeObservationSettings, const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr ) { - if( oneWayRangeObsevationSettings.size( ) != 2 ) + if( oneWayRangeObservationSettings.size( ) != 2 ) { - throw std::runtime_error( "Error when creating two-way averaged Doppler settings, number of input one-way links (" + - std::to_string( oneWayRangeObsevationSettings.size( ) ) + ") is incompatible." ); + throw std::runtime_error( + "Error when creating two-way averaged Doppler settings, number of input one-way " + "links (" + + std::to_string( oneWayRangeObservationSettings.size( ) ) + ") is incompatible." ); } return std::make_shared< NWayDifferencedRangeObservationSettings >( - oneWayRangeObsevationSettings, biasSettings ); + oneWayRangeObservationSettings, biasSettings ); } inline std::shared_ptr< ObservationModelSettings > nWayDifferencedRangeObservationSettings( const LinkDefinition& linkEnds, - const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList = - std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + 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 >( ) ) + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ) ) { return std::make_shared< NWayDifferencedRangeObservationSettings >( - linkEnds, lightTimeCorrectionsList, biasSettings, lightTimeConvergenceCriteria ); + linkEnds, lightTimeCorrectionsList, biasSettings, lightTimeConvergenceCriteria ); } inline std::shared_ptr< ObservationModelSettings > nWayDifferencedRangeObservationSettings( - const std::vector< std::shared_ptr< ObservationModelSettings > > oneWayRangeObsevationSettings, + const std::vector< std::shared_ptr< ObservationModelSettings > > + oneWayRangeObservationSettings, const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr, - const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria - = std::make_shared< LightTimeConvergenceCriteria >( ) ) + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ) ) { return std::make_shared< NWayDifferencedRangeObservationSettings >( - oneWayRangeObsevationSettings, biasSettings, lightTimeConvergenceCriteria ); + oneWayRangeObservationSettings, biasSettings, lightTimeConvergenceCriteria ); } inline std::shared_ptr< ObservationModelSettings > dsnNWayAveragedDopplerObservationSettings( const LinkDefinition& linkEnds, - const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList = - std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + 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 >( ), const bool subtractDopplerSignature = true ) { return std::make_shared< DsnNWayAveragedDopplerObservationSettings >( - linkEnds, lightTimeCorrectionsList, biasSettings, lightTimeConvergenceCriteria, subtractDopplerSignature ); + linkEnds, + lightTimeCorrectionsList, + biasSettings, + lightTimeConvergenceCriteria, + subtractDopplerSignature ); } inline std::shared_ptr< ObservationModelSettings > dsnNWayAveragedDopplerObservationSettings( - const std::vector< std::shared_ptr< ObservationModelSettings > > oneWayRangeObsevationSettings, + const std::vector< std::shared_ptr< ObservationModelSettings > > + oneWayRangeObservationSettings, const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr, - const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria - = std::make_shared< LightTimeConvergenceCriteria >( ), + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ), const bool subtractDopplerSignature = true ) { return std::make_shared< DsnNWayAveragedDopplerObservationSettings >( - oneWayRangeObsevationSettings, biasSettings, lightTimeConvergenceCriteria, subtractDopplerSignature ); + oneWayRangeObservationSettings, + biasSettings, + lightTimeConvergenceCriteria, + subtractDopplerSignature ); } +inline std::shared_ptr< DsnNWayRangeObservationSettings > dsnNWayRangeObservationSettings( + const LinkDefinition& linkEnds, + 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 >( ) ) +{ + return std::make_shared< DsnNWayRangeObservationSettings >( + linkEnds, lightTimeCorrectionsList, biasSettings, lightTimeConvergenceCriteria ); +} inline std::shared_ptr< ObservationModelSettings > twoWayRange( - const std::vector< std::shared_ptr< ObservationModelSettings > > oneWayRangeObsevationSettings, + const std::vector< std::shared_ptr< ObservationModelSettings > > + oneWayRangeObservationSettings, const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr ) { - if( oneWayRangeObsevationSettings.size( ) != 2 ) + if( oneWayRangeObservationSettings.size( ) != 2 ) { - throw std::runtime_error( "Error when creating two-way range settings, number of input one-way links (" + - std::to_string( oneWayRangeObsevationSettings.size( ) ) + ") is incompatible." ); + throw std::runtime_error( + "Error when creating two-way range settings, number of input one-way links (" + + std::to_string( oneWayRangeObservationSettings.size( ) ) + ") is incompatible." ); } - return std::make_shared< NWayRangeObservationSettings >( - oneWayRangeObsevationSettings, biasSettings ); + return std::make_shared< NWayRangeObservationSettings >( oneWayRangeObservationSettings, + biasSettings ); } - inline std::shared_ptr< ObservationModelSettings > twoWayRangeSimple( const LinkDefinition& linkEnds, - const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList = - std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + 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 >( ) ) + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ) ) { if( linkEnds.linkEnds_.size( ) != 3 ) { - throw std::runtime_error( "Error when creating two-way range settings, number of input link ends (" + - std::to_string( linkEnds.linkEnds_.size( ) ) + ") is incompatible." ); + throw std::runtime_error( + "Error when creating two-way range settings, number of input link ends (" + + std::to_string( linkEnds.linkEnds_.size( ) ) + ") is incompatible." ); } return std::make_shared< NWayRangeObservationSettings >( - linkEnds, lightTimeCorrectionsList, biasSettings, lightTimeConvergenceCriteria ); + linkEnds, lightTimeCorrectionsList, biasSettings, lightTimeConvergenceCriteria ); } - inline std::shared_ptr< ObservationModelSettings > nWayRange( - const std::vector< std::shared_ptr< ObservationModelSettings > > oneWayRangeObsevationSettings, + const std::vector< std::shared_ptr< ObservationModelSettings > > + oneWayRangeObservationSettings, const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr ) { - return std::make_shared< NWayRangeObservationSettings >( - oneWayRangeObsevationSettings, biasSettings ); + return std::make_shared< NWayRangeObservationSettings >( oneWayRangeObservationSettings, + biasSettings ); } - inline std::shared_ptr< ObservationModelSettings > nWayRangeSimple( const LinkDefinition& linkEnds, - const std::vector< std::shared_ptr< LightTimeCorrectionSettings > >& lightTimeCorrectionsList = - std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + 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 >( ) ) + const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = + std::make_shared< LightTimeConvergenceCriteria >( ) ) { return std::make_shared< NWayRangeObservationSettings >( - linkEnds, lightTimeCorrectionsList, biasSettings, lightTimeConvergenceCriteria ); + linkEnds, lightTimeCorrectionsList, biasSettings, lightTimeConvergenceCriteria ); } inline std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria( @@ -1238,11 +1372,12 @@ inline std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCrite const double fractionOfLightTimeTolerance = TUDAT_NAN, const LightTimeFailureHandling failureHandling = accept_without_warning ) { - return std::make_shared< LightTimeConvergenceCriteria >( - iterateCorrections, maximumNumberOfIterations, fractionOfLightTimeTolerance, failureHandling ); + return std::make_shared< LightTimeConvergenceCriteria >( iterateCorrections, + maximumNumberOfIterations, + fractionOfLightTimeTolerance, + failureHandling ); } - //! Function to create the proper time rate calculator for use in one-way Doppler /*! * Function to create the proper time rate calculator for use in one-way Doppler @@ -1256,7 +1391,7 @@ template< typename ObservationScalarType = double, typename TimeType = double > std::shared_ptr< DopplerProperTimeRateInterface > createOneWayDopplerProperTimeCalculator( std::shared_ptr< DopplerProperTimeRateSettings > properTimeRateSettings, const LinkDefinition& linkEndsDefinitions, - const simulation_setup::SystemOfBodies &bodies, + const simulation_setup::SystemOfBodies& bodies, const LinkEndType linkEndForCalculator ) { LinkEnds linkEnds = linkEndsDefinitions.linkEnds_; @@ -1265,453 +1400,570 @@ std::shared_ptr< DopplerProperTimeRateInterface > createOneWayDopplerProperTimeC // Check tyope of proper time rate model switch( properTimeRateSettings->dopplerProperTimeRateType_ ) { - case direct_first_order_doppler_proper_time_rate: - { - // Check input consistency - std::shared_ptr< DirectFirstOrderDopplerProperTimeRateSettings > directFirstOrderDopplerProperTimeRateSettings = - std::dynamic_pointer_cast< DirectFirstOrderDopplerProperTimeRateSettings >( properTimeRateSettings ); - if( directFirstOrderDopplerProperTimeRateSettings == 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 ) - { - std::string errorMessage = "Error when creating one-way Doppler proper time calculator, did not find link end " + - std::to_string( linkEndForCalculator ); - throw std::runtime_error( errorMessage ); - } - else - { - if( bodies.at( directFirstOrderDopplerProperTimeRateSettings->centralBodyName_ )->getGravityFieldModel( ) == nullptr ) + case direct_first_order_doppler_proper_time_rate: { + // Check input consistency + std::shared_ptr< DirectFirstOrderDopplerProperTimeRateSettings > + directFirstOrderDopplerProperTimeRateSettings = std::dynamic_pointer_cast< + DirectFirstOrderDopplerProperTimeRateSettings >( + properTimeRateSettings ); + if( directFirstOrderDopplerProperTimeRateSettings == 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 ) { - throw std::runtime_error( "Error when making DirectFirstOrderDopplerProperTimeRateInterface, no gravity field found for " + - directFirstOrderDopplerProperTimeRateSettings->centralBodyName_ ); + std::string errorMessage = + "Error when creating one-way Doppler proper time calculator, did not find " + "link end " + + std::to_string( linkEndForCalculator ); + throw std::runtime_error( errorMessage ); } 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 ) ); + if( bodies.at( directFirstOrderDopplerProperTimeRateSettings->centralBodyName_ ) + ->getGravityFieldModel( ) == nullptr ) + { + throw std::runtime_error( + "Error when making DirectFirstOrderDopplerProperTimeRateInterface, no " + "gravity field found for " + + directFirstOrderDopplerProperTimeRateSettings->centralBodyName_ ); } else { - throw std::runtime_error( - "Error, proper time reference point as link end not yet implemented for DopplerProperTimeRateInterface creation" ); + // 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" ); + } } } + break; } - 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 ); + 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; } -////! Typedef of list of observation models per obserable type and link ends: note that ObservableType key must be consistent -////! with contents of ObservationModelSettings pointers. The ObservationSettingsMap may be used as well, which contains the same -////! type of information. This typedef, however, has some advantages in terms of book-keeping when creating observation models. -//typedef std::map< ObservableType, std::map< LinkEnds, std::shared_ptr< ObservationModelSettings > > > SortedObservationSettingsMap; - -////! Typedef of list of observation models per link ends. Multiple observation models for a single set of link ends are allowed, +////! Typedef of list of observation models per obserable type and link ends: note that +/// ObservableType key must be consistent +////! with contents of ObservationModelSettings pointers. The ObservationSettingsMap may be used as +/// well, which contains the same +////! type of information. This typedef, however, has some advantages in terms of book-keeping when +/// creating observation models. +// typedef std::map< ObservableType, std::map< LinkEnds, std::shared_ptr< ObservationModelSettings > +// > > SortedObservationSettingsMap; + +////! Typedef of list of observation models per link ends. Multiple observation models for a single +/// set of link ends are allowed, ////! since this typedef represents a multimap. -//typedef std::multimap< LinkEnds, std::shared_ptr< ObservationModelSettings > > ObservationSettingsMap; - -//typedef std::vector< std::pair< LinkEnds, std::shared_ptr< ObservationModelSettings > > > ObservationSettingsVector; +// typedef std::multimap< LinkEnds, std::shared_ptr< ObservationModelSettings > > +// ObservationSettingsMap; -//typedef std::map< LinkEnds, std::vector< std::shared_ptr< ObservationModelSettings > > > ObservationSettingsListPerLinkEnd; +// typedef std::vector< std::pair< LinkEnds, std::shared_ptr< ObservationModelSettings > > > +// ObservationSettingsVector; +// typedef std::map< LinkEnds, std::vector< std::shared_ptr< ObservationModelSettings > > > +// ObservationSettingsListPerLinkEnd; -std::map< ObservableType, std::vector< std::shared_ptr< ObservationModelSettings > > > sortObservationModelSettingsByType( - const std::vector< std::shared_ptr< ObservationModelSettings > >& observationModelSettings ); +std::map< ObservableType, std::vector< std::shared_ptr< ObservationModelSettings > > > +sortObservationModelSettingsByType( + const std::vector< std::shared_ptr< ObservationModelSettings > >& + observationModelSettings ); //! Function to create an object that computes an observation bias /*! - * Function to create an object that computes an observation bias, which can represent any type of system-dependent influence - * on the observed value (e.g. absolute bias, relative bias, clock drift, etc.) - * \param linkEnds Observation link ends for which the bias is to be created. - * \param observableType Observable type for which bias is to be created - * \param biasSettings Settings for the observation bias that is to be created. - * \param bodies List of body objects that comprises the environment. - * \return Object that computes an observation bias according to requested settings. + * Function to create an object that computes an observation bias, which can represent any type of + * system-dependent influence on the observed value (e.g. absolute bias, relative bias, clock drift, + * etc.) \param linkEnds Observation link ends for which the bias is to be created. \param + * observableType Observable type for which bias is to be created \param biasSettings Settings for + * the observation bias that is to be created. \param bodies List of body objects that comprises the + * environment. \return Object that computes an observation bias according to requested settings. */ template< int ObservationSize = 1 > std::shared_ptr< ObservationBias< ObservationSize > > createObservationBiasCalculator( const LinkDefinition linkEnds, const ObservableType observableType, const std::shared_ptr< ObservationBiasSettings > biasSettings, - const simulation_setup::SystemOfBodies &bodies ) + const simulation_setup::SystemOfBodies& bodies ) { std::shared_ptr< ObservationBias< ObservationSize > > observationBias; switch( biasSettings->observationBiasType_ ) { - case constant_absolute_bias: - { - // Check input consistency - std::shared_ptr< ConstantObservationBiasSettings > constantBiasSettings = std::dynamic_pointer_cast< - ConstantObservationBiasSettings >( biasSettings ); - if( constantBiasSettings == nullptr ) - { - throw std::runtime_error( "Error when making constant observation bias, settings are inconsistent" ); - } + case constant_absolute_bias: { + // Check input consistency + std::shared_ptr< ConstantObservationBiasSettings > constantBiasSettings = + std::dynamic_pointer_cast< ConstantObservationBiasSettings >( biasSettings ); + if( constantBiasSettings == nullptr ) + { + throw std::runtime_error( + "Error when making constant observation bias, settings are inconsistent" ); + } - if( !constantBiasSettings->useAbsoluteBias_ ) - { - throw std::runtime_error( "Error when making constant observation bias, class settings are inconsistent" ); - } + if( !constantBiasSettings->useAbsoluteBias_ ) + { + throw std::runtime_error( + "Error when making constant observation bias, class settings are " + "inconsistent" ); + } - // Check if size of bias is consistent with requested observable size - if( constantBiasSettings->observationBias_.rows( ) != ObservationSize ) - { - throw std::runtime_error( "Error when making constant observation bias, bias size is inconsistent" ); - } - observationBias = std::make_shared< ConstantObservationBias< ObservationSize > >( + // Check if size of bias is consistent with requested observable size + if( constantBiasSettings->observationBias_.rows( ) != ObservationSize ) + { + throw std::runtime_error( + "Error when making constant observation bias, bias size is inconsistent" ); + } + observationBias = std::make_shared< ConstantObservationBias< ObservationSize > >( constantBiasSettings->observationBias_ ); - break; - } - case arc_wise_constant_absolute_bias: - { - // Check input consistency - std::shared_ptr< ArcWiseConstantObservationBiasSettings > arcwiseBiasSettings = std::dynamic_pointer_cast< - ArcWiseConstantObservationBiasSettings >( biasSettings ); - if( arcwiseBiasSettings == nullptr ) - { - throw std::runtime_error( "Error when making arc-wise observation bias, settings are inconsistent" ); - } - else if( !arcwiseBiasSettings->useAbsoluteBias_ ) - { - throw std::runtime_error( "Error when making arc-wise observation bias, class contents are inconsistent" ); + break; } - - std::vector< Eigen::Matrix< double, ObservationSize, 1 > > observationBiases; - for( unsigned int i = 0; i < arcwiseBiasSettings->observationBiases_.size( ); i++ ) - { - // Check if size of bias is consistent with requested observable size - if( arcwiseBiasSettings->observationBiases_.at( i ).rows( ) != ObservationSize ) + case arc_wise_constant_absolute_bias: { + // Check input consistency + std::shared_ptr< ArcWiseConstantObservationBiasSettings > arcwiseBiasSettings = + std::dynamic_pointer_cast< ArcWiseConstantObservationBiasSettings >( + biasSettings ); + if( arcwiseBiasSettings == nullptr ) { - throw std::runtime_error( "Error when making arc-wise observation bias, bias size is inconsistent" ); + throw std::runtime_error( + "Error when making arc-wise observation bias, settings are inconsistent" ); } - else + else if( !arcwiseBiasSettings->useAbsoluteBias_ ) { - observationBiases.push_back( arcwiseBiasSettings->observationBiases_.at( i ) ); + throw std::runtime_error( + "Error when making arc-wise observation bias, class contents are " + "inconsistent" ); } - } - observationBias = std::make_shared< ConstantArcWiseObservationBias< ObservationSize > >( - arcwiseBiasSettings->arcStartTimes_, observationBiases, - observation_models::getLinkEndIndicesForLinkEndTypeAtObservable( - observableType, arcwiseBiasSettings->linkEndForTime_, linkEnds.size( ) ).at( 0 ) ); - break; - } - case constant_relative_bias: - { - // Check input consistency - std::shared_ptr< ConstantObservationBiasSettings > constantBiasSettings = std::dynamic_pointer_cast< - ConstantObservationBiasSettings >( biasSettings ); - if( constantBiasSettings == nullptr ) - { - throw std::runtime_error( "Error when making constant relative observation bias, settings are inconsistent" ); - } - if( constantBiasSettings->useAbsoluteBias_ ) - { - throw std::runtime_error( "Error when making constant relative observation bias, class settings are inconsistent" ); + std::vector< Eigen::Matrix< double, ObservationSize, 1 > > observationBiases; + for( unsigned int i = 0; i < arcwiseBiasSettings->observationBiases_.size( ); i++ ) + { + // Check if size of bias is consistent with requested observable size + if( arcwiseBiasSettings->observationBiases_.at( i ).rows( ) != ObservationSize ) + { + throw std::runtime_error( + "Error when making arc-wise observation bias, bias size is " + "inconsistent" ); + } + else + { + observationBiases.push_back( arcwiseBiasSettings->observationBiases_.at( i ) ); + } + } + observationBias = std::make_shared< ConstantArcWiseObservationBias< ObservationSize > >( + arcwiseBiasSettings->arcStartTimes_, + observationBiases, + observation_models::getLinkEndIndicesForLinkEndTypeAtObservable( + observableType, arcwiseBiasSettings->linkEndForTime_, linkEnds.size( ) ) + .at( 0 ) ); + break; } + case constant_relative_bias: { + // Check input consistency + std::shared_ptr< ConstantObservationBiasSettings > constantBiasSettings = + std::dynamic_pointer_cast< ConstantObservationBiasSettings >( biasSettings ); + if( constantBiasSettings == nullptr ) + { + throw std::runtime_error( + "Error when making constant relative observation bias, settings are " + "inconsistent" ); + } - // Check if size of bias is consistent with requested observable size - if( constantBiasSettings->observationBias_.rows( ) != ObservationSize ) - { - throw std::runtime_error( "Error when making constant relative observation bias, bias size is inconsistent" ); - } - observationBias = std::make_shared< ConstantRelativeObservationBias< ObservationSize > >( - constantBiasSettings->observationBias_ ); - break; - } - case arc_wise_constant_relative_bias: - { - // Check input consistency - std::shared_ptr< ArcWiseConstantObservationBiasSettings > arcwiseBiasSettings = std::dynamic_pointer_cast< - ArcWiseConstantObservationBiasSettings >( biasSettings ); - if( arcwiseBiasSettings == nullptr ) - { - throw std::runtime_error( "Error when making arc-wise relative observation bias, settings are inconsistent" ); - } - else if( arcwiseBiasSettings->useAbsoluteBias_ ) - { - throw std::runtime_error( "Error when making arc-wise relative observation bias, class contents are inconsistent" ); - } + if( constantBiasSettings->useAbsoluteBias_ ) + { + throw std::runtime_error( + "Error when making constant relative observation bias, class settings are " + "inconsistent" ); + } - std::vector< Eigen::Matrix< double, ObservationSize, 1 > > observationBiases; - for( unsigned int i = 0; i < arcwiseBiasSettings->observationBiases_.size( ); i++ ) - { // Check if size of bias is consistent with requested observable size - if( arcwiseBiasSettings->observationBiases_.at( i ).rows( ) != ObservationSize ) + if( constantBiasSettings->observationBias_.rows( ) != ObservationSize ) { - throw std::runtime_error( "Error when making arc-wise observation bias, bias size is inconsistent" ); + throw std::runtime_error( + "Error when making constant relative observation bias, bias size is " + "inconsistent" ); } - else + observationBias = + std::make_shared< ConstantRelativeObservationBias< ObservationSize > >( + constantBiasSettings->observationBias_ ); + break; + } + case arc_wise_constant_relative_bias: { + // Check input consistency + std::shared_ptr< ArcWiseConstantObservationBiasSettings > arcwiseBiasSettings = + std::dynamic_pointer_cast< ArcWiseConstantObservationBiasSettings >( + biasSettings ); + if( arcwiseBiasSettings == nullptr ) { - observationBiases.push_back( arcwiseBiasSettings->observationBiases_.at( i ) ); + throw std::runtime_error( + "Error when making arc-wise relative observation bias, settings are " + "inconsistent" ); + } + else if( arcwiseBiasSettings->useAbsoluteBias_ ) + { + throw std::runtime_error( + "Error when making arc-wise relative observation bias, class contents are " + "inconsistent" ); } - } - observationBias = std::make_shared< ConstantRelativeArcWiseObservationBias< ObservationSize > >( - arcwiseBiasSettings->arcStartTimes_, observationBiases, - observation_models::getLinkEndIndicesForLinkEndTypeAtObservable( - observableType, arcwiseBiasSettings->linkEndForTime_, linkEnds.size( ) ).at( 0 ) ); - break; - } - case constant_time_drift_bias: - { - // Check input consistency - std::shared_ptr< ConstantTimeDriftBiasSettings > constantTimeDriftBiasSettings = std::dynamic_pointer_cast< - ConstantTimeDriftBiasSettings >( biasSettings ); - if( constantTimeDriftBiasSettings == nullptr ) - { - throw std::runtime_error( "Error when making constant time drift observation bias, settings are inconsistent" ); - } - // Check if size of bias is consistent with requested observable size - if( constantTimeDriftBiasSettings->timeDriftBias_.rows( ) != ObservationSize ) - { - throw std::runtime_error( "Error when making constant observation time drift bias, bias size is inconsistent" ); - } - observationBias = std::make_shared< ConstantTimeDriftBias< ObservationSize > >( - constantTimeDriftBiasSettings->timeDriftBias_, observation_models::getLinkEndIndicesForLinkEndTypeAtObservable( - observableType, constantTimeDriftBiasSettings->linkEndForTime_, linkEnds.size( ) ).at( 0 ), constantTimeDriftBiasSettings->referenceEpoch_ ); - break; - } - case arc_wise_time_drift_bias: - { - // Check input consistency - std::shared_ptr< ArcWiseTimeDriftBiasSettings > arcwiseBiasSettings = std::dynamic_pointer_cast< - ArcWiseTimeDriftBiasSettings >( biasSettings ); - if( arcwiseBiasSettings == nullptr ) - { - throw std::runtime_error( "Error when making arc-wise time drift bias, settings are inconsistent" ); + std::vector< Eigen::Matrix< double, ObservationSize, 1 > > observationBiases; + for( unsigned int i = 0; i < arcwiseBiasSettings->observationBiases_.size( ); i++ ) + { + // Check if size of bias is consistent with requested observable size + if( arcwiseBiasSettings->observationBiases_.at( i ).rows( ) != ObservationSize ) + { + throw std::runtime_error( + "Error when making arc-wise observation bias, bias size is " + "inconsistent" ); + } + else + { + observationBiases.push_back( arcwiseBiasSettings->observationBiases_.at( i ) ); + } + } + observationBias = + std::make_shared< ConstantRelativeArcWiseObservationBias< ObservationSize > >( + arcwiseBiasSettings->arcStartTimes_, + observationBiases, + observation_models::getLinkEndIndicesForLinkEndTypeAtObservable( + observableType, + arcwiseBiasSettings->linkEndForTime_, + linkEnds.size( ) ) + .at( 0 ) ); + break; } + case constant_time_drift_bias: { + // Check input consistency + std::shared_ptr< ConstantTimeDriftBiasSettings > constantTimeDriftBiasSettings = + std::dynamic_pointer_cast< ConstantTimeDriftBiasSettings >( biasSettings ); + if( constantTimeDriftBiasSettings == nullptr ) + { + throw std::runtime_error( + "Error when making constant time drift observation bias, settings are " + "inconsistent" ); + } - std::vector< Eigen::Matrix< double, ObservationSize, 1 > > observationBiases; - for( unsigned int i = 0; i < arcwiseBiasSettings->timeDriftBiases_.size( ); i++ ) - { // Check if size of bias is consistent with requested observable size - if( arcwiseBiasSettings->timeDriftBiases_.at( i ).rows( ) != ObservationSize ) + if( constantTimeDriftBiasSettings->timeDriftBias_.rows( ) != ObservationSize ) { - throw std::runtime_error( "Error when making arc-wise time drift bias, bias size is inconsistent" ); + throw std::runtime_error( + "Error when making constant observation time drift bias, bias size is " + "inconsistent" ); } - else + observationBias = std::make_shared< ConstantTimeDriftBias< ObservationSize > >( + constantTimeDriftBiasSettings->timeDriftBias_, + observation_models::getLinkEndIndicesForLinkEndTypeAtObservable( + observableType, + constantTimeDriftBiasSettings->linkEndForTime_, + linkEnds.size( ) ) + .at( 0 ), + constantTimeDriftBiasSettings->referenceEpoch_ ); + break; + } + case arc_wise_time_drift_bias: { + // Check input consistency + std::shared_ptr< ArcWiseTimeDriftBiasSettings > arcwiseBiasSettings = + std::dynamic_pointer_cast< ArcWiseTimeDriftBiasSettings >( biasSettings ); + if( arcwiseBiasSettings == nullptr ) { - observationBiases.push_back( arcwiseBiasSettings->timeDriftBiases_.at( i ) ); + throw std::runtime_error( + "Error when making arc-wise time drift bias, settings are inconsistent" ); } - } - observationBias = std::make_shared< ArcWiseTimeDriftBias< ObservationSize > >( - arcwiseBiasSettings->arcStartTimes_, observationBiases, - observation_models::getLinkEndIndicesForLinkEndTypeAtObservable( - observableType, arcwiseBiasSettings->linkEndForTime_, linkEnds.size( ) ).at( 0 ), arcwiseBiasSettings->referenceEpochs_ ); - break; - } - case constant_time_bias: - { - // Check input consistency - std::shared_ptr< ConstantTimeBiasSettings > constantTimeBiasSettings = std::dynamic_pointer_cast< ConstantTimeBiasSettings >( biasSettings ); - if( constantTimeBiasSettings == nullptr ) - { - throw std::runtime_error( "Error when making constant time observation bias, settings are inconsistent" ); - } - observationBias = std::make_shared< ConstantTimeBias< ObservationSize > >( - constantTimeBiasSettings->timeBias_, observation_models::getLinkEndIndicesForLinkEndTypeAtObservable( - observableType, constantTimeBiasSettings->linkEndForTime_, linkEnds.size( ) ).at( 0 ) ); - break; - } - case arc_wise_time_bias: - { - // Check input consistency - std::shared_ptr< ArcWiseTimeBiasSettings > arcwiseBiasSettings = std::dynamic_pointer_cast< - ArcWiseTimeBiasSettings >( biasSettings ); - if( arcwiseBiasSettings == nullptr ) - { - throw std::runtime_error( "Error when making arc-wise time bias, settings are inconsistent" ); + std::vector< Eigen::Matrix< double, ObservationSize, 1 > > observationBiases; + for( unsigned int i = 0; i < arcwiseBiasSettings->timeDriftBiases_.size( ); i++ ) + { + // Check if size of bias is consistent with requested observable size + if( arcwiseBiasSettings->timeDriftBiases_.at( i ).rows( ) != ObservationSize ) + { + throw std::runtime_error( + "Error when making arc-wise time drift bias, bias size is " + "inconsistent" ); + } + else + { + observationBiases.push_back( arcwiseBiasSettings->timeDriftBiases_.at( i ) ); + } + } + observationBias = std::make_shared< ArcWiseTimeDriftBias< ObservationSize > >( + arcwiseBiasSettings->arcStartTimes_, + observationBiases, + observation_models::getLinkEndIndicesForLinkEndTypeAtObservable( + observableType, arcwiseBiasSettings->linkEndForTime_, linkEnds.size( ) ) + .at( 0 ), + arcwiseBiasSettings->referenceEpochs_ ); + break; } + case constant_time_bias: { + // Check input consistency + std::shared_ptr< ConstantTimeBiasSettings > constantTimeBiasSettings = + std::dynamic_pointer_cast< ConstantTimeBiasSettings >( biasSettings ); + if( constantTimeBiasSettings == nullptr ) + { + throw std::runtime_error( + "Error when making constant time observation bias, settings are " + "inconsistent" ); + } - observationBias = std::make_shared< ArcWiseTimeBias< ObservationSize > >( - arcwiseBiasSettings->arcStartTimes_, arcwiseBiasSettings->timeBiases_, - observation_models::getLinkEndIndicesForLinkEndTypeAtObservable( - observableType, arcwiseBiasSettings->linkEndForTime_, linkEnds.size( ) ).at( 0 ) ); - break; - } - case two_way_range_time_scale_bias: - { - if( observableType != n_way_range ) - { - throw std::runtime_error( "Error when making two-way range time scale bias, bias is to be applied to wrong observable: " + - getObservableName( observableType, linkEnds.size( ) ) ); - } - else if( linkEnds.size( ) != 3 ) - { - throw std::runtime_error( "Error when making two-way range time scale bias, bias is to be applied to observable with wrong number of link ends: " + - std::to_string( linkEnds.size( ) ) ); + observationBias = std::make_shared< ConstantTimeBias< ObservationSize > >( + constantTimeBiasSettings->timeBias_, + observation_models::getLinkEndIndicesForLinkEndTypeAtObservable( + observableType, + constantTimeBiasSettings->linkEndForTime_, + linkEnds.size( ) ) + .at( 0 ) ); + break; } - else - { - if( linkEnds.at( transmitter ).bodyName_ != "Earth" ) + case arc_wise_time_bias: { + // Check input consistency + std::shared_ptr< ArcWiseTimeBiasSettings > arcwiseBiasSettings = + std::dynamic_pointer_cast< ArcWiseTimeBiasSettings >( biasSettings ); + if( arcwiseBiasSettings == nullptr ) { - std::cerr<<" Warning when making two-way range time scale bias, bias is to be applied observable with transmitter not on Earth: "< >( + arcwiseBiasSettings->arcStartTimes_, + arcwiseBiasSettings->timeBiases_, + observation_models::getLinkEndIndicesForLinkEndTypeAtObservable( + observableType, arcwiseBiasSettings->linkEndForTime_, linkEnds.size( ) ) + .at( 0 ) ); + break; + } + case two_way_range_time_scale_bias: { + if( observableType != n_way_range ) { - std::cerr<<" Warning when making two-way range time scale bias, bias is to be applied observable with receiver not on Earth: "< >( - earth_orientation::createDefaultTimeConverter( ), - bodies.at( linkEnds.at( transmitter ).bodyName_ )->getGroundStation( linkEnds.at( transmitter ).stationName_ )->getNominalStationState( ), - bodies.at( linkEnds.at( transmitter ).bodyName_ )->getGroundStation( linkEnds.at( receiver ).stationName_ )->getNominalStationState( ) ); + else if( linkEnds.size( ) != 3 ) + { + throw std::runtime_error( + "Error when making two-way range time scale bias, bias is to be applied to " + "observable with wrong number of link ends: " + + std::to_string( linkEnds.size( ) ) ); + } + else + { + if( linkEnds.at( transmitter ).bodyName_ != "Earth" ) + { + std::cerr << " Warning when making two-way range time scale bias, bias is to " + "be applied observable with transmitter not on Earth: " + << linkEnds.at( transmitter ).bodyName_ << std::endl; + } - break; - } - case multiple_observation_biases: - { - // Check input consistency - std::shared_ptr< MultipleObservationBiasSettings > multiBiasSettings = std::dynamic_pointer_cast< - MultipleObservationBiasSettings >( biasSettings ); - if( multiBiasSettings == nullptr ) - { - throw std::runtime_error( "Error when making multiple observation biases, settings are inconsistent" ); - } + if( linkEnds.at( receiver ).bodyName_ != "Earth" ) + { + std::cerr << " Warning when making two-way range time scale bias, bias is to " + "be applied observable with receiver not on Earth: " + << linkEnds.at( receiver ).bodyName_ << std::endl; + } + } - // Create list of biases - std::vector< std::shared_ptr< ObservationBias< ObservationSize > > > observationBiasList; - for( unsigned int i = 0; i < multiBiasSettings->biasSettingsList_.size( ); i++ ) - { - observationBiasList.push_back( createObservationBiasCalculator< ObservationSize >( - linkEnds, observableType, multiBiasSettings->biasSettingsList_.at( i ) , bodies ) ); - } + observationBias = std::make_shared< TwoWayTimeScaleRangeBias< ObservationSize > >( + earth_orientation::createDefaultTimeConverter( ), + bodies.at( linkEnds.at( transmitter ).bodyName_ ) + ->getGroundStation( linkEnds.at( transmitter ).stationName_ ) + ->getNominalStationState( ), + bodies.at( linkEnds.at( transmitter ).bodyName_ ) + ->getGroundStation( linkEnds.at( receiver ).stationName_ ) + ->getNominalStationState( ) ); - // Create combined bias object - observationBias = std::make_shared< MultiTypeObservationBias< ObservationSize > >( - observationBiasList ); - break; - } - case clock_induced_bias: - { - if( observableType != one_way_range && observableType != n_way_range && observableType != dsn_n_way_range ) - { - throw std::runtime_error( "Error, clock-induced observation bias currently only supported for one- and n-way range" ); + break; } - else - { + case multiple_observation_biases: { // Check input consistency - std::shared_ptr< TiminigSystemBiasSettings > clockInducedBiasSettings = std::dynamic_pointer_cast< - TiminigSystemBiasSettings >( biasSettings ); - - std::vector< int > relevantLinkEndIndices = getLinkEndIndicesForLinkEndIdAtObservable( - observableType, linkEnds.linkEnds_, LinkEndId( clockInducedBiasSettings->bodyName_, clockInducedBiasSettings->stationName_ ) ); + std::shared_ptr< MultipleObservationBiasSettings > multiBiasSettings = + std::dynamic_pointer_cast< MultipleObservationBiasSettings >( biasSettings ); + if( multiBiasSettings == nullptr ) + { + throw std::runtime_error( + "Error when making multiple observation biases, settings are " + "inconsistent" ); + } - if( clockInducedBiasSettings == nullptr ) + // Create list of biases + std::vector< std::shared_ptr< ObservationBias< ObservationSize > > > + observationBiasList; + for( unsigned int i = 0; i < multiBiasSettings->biasSettingsList_.size( ); i++ ) { - throw std::runtime_error( "Error when making clock-induced bias, settings are inconsistent" ); + observationBiasList.push_back( createObservationBiasCalculator< ObservationSize >( + linkEnds, + observableType, + multiBiasSettings->biasSettingsList_.at( i ), + bodies ) ); } - std::shared_ptr< system_models::TimingSystem > timingSystem; - if( bodies.count( clockInducedBiasSettings->bodyName_ ) == 0 ) + // Create combined bias object + observationBias = std::make_shared< MultiTypeObservationBias< ObservationSize > >( + observationBiasList ); + break; + } + case clock_induced_bias: { + if( observableType != one_way_range && observableType != n_way_range && + observableType != dsn_n_way_range ) { - throw std::runtime_error( "Error when getting timing system of body " + clockInducedBiasSettings->bodyName_ + - " for observation bias, no such body exists." ); + throw std::runtime_error( + "Error, clock-induced observation bias currently only supported for one- " + "and n-way range" ); } else { - if( clockInducedBiasSettings->stationName_ == "" ) + // Check input consistency + std::shared_ptr< TiminigSystemBiasSettings > clockInducedBiasSettings = + std::dynamic_pointer_cast< TiminigSystemBiasSettings >( biasSettings ); + + std::vector< int > relevantLinkEndIndices = + getLinkEndIndicesForLinkEndIdAtObservable( + observableType, + linkEnds.linkEnds_, + LinkEndId( clockInducedBiasSettings->bodyName_, + clockInducedBiasSettings->stationName_ ) ); + + if( clockInducedBiasSettings == nullptr ) { - if( bodies.at( clockInducedBiasSettings->bodyName_ )->getVehicleSystems( ) == nullptr ) - { - throw std::runtime_error( "Error when getting timing system of body " + clockInducedBiasSettings->bodyName_ + - " for observation bias, body has no vehicle systems." ); - } - else - { - timingSystem = bodies.at( clockInducedBiasSettings->bodyName_ )->getVehicleSystems( )->getTimingSystem( ); - if( timingSystem == nullptr ) - { - throw std::runtime_error( "Error when getting timing system of body " + clockInducedBiasSettings->bodyName_ + - " for observation bias, body has no timing system." ); - } - } + throw std::runtime_error( + "Error when making clock-induced bias, settings are inconsistent" ); + } + + std::shared_ptr< system_models::TimingSystem > timingSystem; + if( bodies.count( clockInducedBiasSettings->bodyName_ ) == 0 ) + { + throw std::runtime_error( "Error when getting timing system of body " + + clockInducedBiasSettings->bodyName_ + + " for observation bias, no such body exists." ); } else { - if( bodies.at( clockInducedBiasSettings->bodyName_ )->getGroundStationMap( ).count( clockInducedBiasSettings->stationName_ ) == 0 ) + if( clockInducedBiasSettings->stationName_ == "" ) { - throw std::runtime_error( "Error when getting timing system of body " + clockInducedBiasSettings->bodyName_ + - " and station " + clockInducedBiasSettings->stationName_ + - " no such station exists" ); + if( bodies.at( clockInducedBiasSettings->bodyName_ ) + ->getVehicleSystems( ) == nullptr ) + { + throw std::runtime_error( + "Error when getting timing system of body " + + clockInducedBiasSettings->bodyName_ + + " for observation bias, body has no vehicle systems." ); + } + else + { + timingSystem = bodies.at( clockInducedBiasSettings->bodyName_ ) + ->getVehicleSystems( ) + ->getTimingSystem( ); + if( timingSystem == nullptr ) + { + throw std::runtime_error( + "Error when getting timing system of body " + + clockInducedBiasSettings->bodyName_ + + " for observation bias, body has no timing system." ); + } + } } else { - timingSystem = bodies.at( clockInducedBiasSettings->bodyName_ )->getGroundStation( clockInducedBiasSettings->stationName_ )->getTimingSystem( ); - if( timingSystem == nullptr ) + if( bodies.at( clockInducedBiasSettings->bodyName_ ) + ->getGroundStationMap( ) + .count( clockInducedBiasSettings->stationName_ ) == 0 ) + { + throw std::runtime_error( "Error when getting timing system of body " + + clockInducedBiasSettings->bodyName_ + + " and station " + + clockInducedBiasSettings->stationName_ + + " no such station exists" ); + } + else { - throw std::runtime_error( "Error when getting timing system of body " + clockInducedBiasSettings->bodyName_ + - " and station " + clockInducedBiasSettings->stationName_ + - " station has no timing system" ); + timingSystem = bodies.at( clockInducedBiasSettings->bodyName_ ) + ->getGroundStation( + clockInducedBiasSettings->stationName_ ) + ->getTimingSystem( ); + if( timingSystem == nullptr ) + { + throw std::runtime_error( + "Error when getting timing system of body " + + clockInducedBiasSettings->bodyName_ + " and station " + + clockInducedBiasSettings->stationName_ + + " station has no timing system" ); + } } } - } - // Create combined bias object - observationBias = std::make_shared< ClockInducedRangeBias< ObservationSize > >( - timingSystem, relevantLinkEndIndices, - LinkEndId( clockInducedBiasSettings->bodyName_, clockInducedBiasSettings->stationName_ ) ); + // Create combined bias object + observationBias = std::make_shared< ClockInducedRangeBias< ObservationSize > >( + timingSystem, + relevantLinkEndIndices, + LinkEndId( clockInducedBiasSettings->bodyName_, + clockInducedBiasSettings->stationName_ ) ); + } } + break; + } + default: { + std::string errorMessage = "Error when making observation bias, bias type " + + std::to_string( biasSettings->observationBiasType_ ) + " not recognized"; + throw std::runtime_error( errorMessage ); } - break; - } - default: - { - std::string errorMessage = "Error when making observation bias, bias type " + - std::to_string( biasSettings->observationBiasType_ ) + " not recognized"; - throw std::runtime_error( errorMessage ); - } } return observationBias; } -std::function< double ( observation_models::FrequencyBands, observation_models::FrequencyBands ) > getTurnaroundFunction( - const simulation_setup::SystemOfBodies &bodies, - const LinkEnds& linkEnds ); +std::function< double( observation_models::FrequencyBands, observation_models::FrequencyBands ) > +getTurnaroundFunction( const simulation_setup::SystemOfBodies& bodies, const LinkEnds& linkEnds ); -std::shared_ptr< ground_stations::StationFrequencyInterpolator > getTransmittingFrequencyInterpolator( - const simulation_setup::SystemOfBodies &bodies, - const LinkEnds& linkEnds ); +std::shared_ptr< ground_stations::StationFrequencyInterpolator > +getTransmittingFrequencyInterpolator( const simulation_setup::SystemOfBodies& bodies, + const LinkEnds& linkEnds ); //! Interface class for creating observation models /*! - * Interface class for creating observation models. This class is used instead of a single templated free function to - * allow ObservationModel deroved classed with different ObservationSize template arguments to be created using the same - * interface. This class has template specializations for each value of ObservationSize, and contains a single - * createObservationModel function that performs the required operation. + * Interface class for creating observation models. This class is used instead of a single + * templated free function to allow ObservationModel deroved classed with different ObservationSize + * template arguments to be created using the same interface. This class has template + * specializations for each value of ObservationSize, and contains a single createObservationModel + * function that performs the required operation. */ -template< int ObservationSize = 1, typename ObservationScalarType = double, typename TimeType = double > +template< int ObservationSize = 1, + typename ObservationScalarType = double, + typename TimeType = double > class ObservationModelCreator { -public: - + public: //! Function to create an observation model. /*! * Function to create an observation model. @@ -1720,974 +1972,1260 @@ class ObservationModelCreator * \param bodies List of body objects that comprises the environment * \return Observation model of required settings. */ - static std::shared_ptr< observation_models::ObservationModel< - ObservationSize, ObservationScalarType, TimeType > > createObservationModel( - const std::shared_ptr< ObservationModelSettings > observationSettings, - const simulation_setup::SystemOfBodies &bodies, - ObservableType topLevelObservableType = undefined_observation_model ); + static std::shared_ptr< observation_models::ObservationModel< ObservationSize, + ObservationScalarType, + TimeType > > + createObservationModel( const std::shared_ptr< ObservationModelSettings > observationSettings, + const simulation_setup::SystemOfBodies& bodies, + ObservableType topLevelObservableType = undefined_observation_model ); }; //! Interface class for creating observation models of size 1. template< typename ObservationScalarType, typename TimeType > class ObservationModelCreator< 1, ObservationScalarType, TimeType > { -public: - + public: //! Function to create an observation model of size 1. /*! * Function to create an observation model of size 1. * \param linkEnds Link ends for observation model that is to be created - * \param observationSettings Settings for observation model that is to be created (must be for observation model if size 1). - * \param bodies List of body objects that comprises the environment - * \return Observation model of required settings. + * \param observationSettings Settings for observation model that is to be created (must be for + * observation model if size 1). \param bodies List of body objects that comprises the + * environment \return Observation model of required settings. */ - static std::shared_ptr< observation_models::ObservationModel< - 1, ObservationScalarType, TimeType > > createObservationModel( - const std::shared_ptr< ObservationModelSettings > observationSettings, - const simulation_setup::SystemOfBodies &bodies, - ObservableType topLevelObservableType = undefined_observation_model ) + static std::shared_ptr< + observation_models::ObservationModel< 1, ObservationScalarType, TimeType > > + createObservationModel( const std::shared_ptr< ObservationModelSettings > observationSettings, + const simulation_setup::SystemOfBodies& bodies, + ObservableType topLevelObservableType = undefined_observation_model ) { using namespace observation_models; - - std::shared_ptr< observation_models::ObservationModel< - 1, ObservationScalarType, TimeType > > observationModel; + std::shared_ptr< + observation_models::ObservationModel< 1, ObservationScalarType, TimeType > > + observationModel; LinkEnds linkEnds = observationSettings->linkEnds_.linkEnds_; - - if ( topLevelObservableType == undefined_observation_model ) + if( topLevelObservableType == undefined_observation_model ) { topLevelObservableType = observationSettings->observableType_; } - // 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_ ), + // 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" ); + 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 ); - } + 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< OneWayDopplerObservationModel< - ObservationScalarType, TimeType > >( + if( std::dynamic_pointer_cast< OneWayDopplerObservationSettings >( + observationSettings ) == nullptr ) + { + // Create observation model + observationModel = std::make_shared< + OneWayDopplerObservationModel< ObservationScalarType, TimeType > >( linkEnds, createLightTimeCalculator< ObservationScalarType, TimeType >( - linkEnds, transmitter, receiver, - bodies, topLevelObservableType, observationSettings->lightTimeCorrectionsList_, - observationSettings->lightTimeConvergenceCriteria_ ), + linkEnds, + transmitter, + receiver, + bodies, + topLevelObservableType, + observationSettings->lightTimeCorrectionsList_, + observationSettings->lightTimeConvergenceCriteria_ ), observationBias, std::function< ObservationScalarType( const TimeType ) >( ), std::function< ObservationScalarType( const TimeType ) >( ), false ); - } - 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 ) + else { - receiverProperTimeRate = - createOneWayDopplerProperTimeCalculator< ObservationScalarType, TimeType >( - oneWayDopplerSettings->receiverProperTimeRateSettings_, linkEnds, bodies, receiver ); - } + 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 ); + } - // Create observation model - observationModel = std::make_shared< OneWayDopplerObservationModel< - ObservationScalarType, TimeType > >( + 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_ ), + linkEnds, + transmitter, + receiver, + bodies, + topLevelObservableType, + observationSettings->lightTimeCorrectionsList_, + observationSettings->lightTimeConvergenceCriteria_ ), transmitterProperTimeRate, receiverProperTimeRate, observationBias, oneWayDopplerSettings->normalizeWithSpeedOfLight_ ); - } - - 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" ); + break; } - if( linkEnds.count( reflector1 ) == 0 ) - { - throw std::runtime_error( "Error when making 2 way Doppler model, no retransmitter 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( transmitter ) == 0 ) - { - throw std::runtime_error( "Error when making 2 way Doppler model, no transmitter found" ); - } + if( linkEnds.count( reflector1 ) == 0 ) + { + throw std::runtime_error( + "Error when making 2 way Doppler model, no retransmitter found" ); + } - std::shared_ptr< ObservationBias< 1 > > observationBias; - if( observationSettings->biasSettings_ != nullptr ) - { - observationBias = - createObservationBiasCalculator( - linkEnds, observationSettings->observableType_, observationSettings->biasSettings_,bodies ); - } + if( linkEnds.count( transmitter ) == 0 ) + { + throw std::runtime_error( + "Error when making 2 way Doppler model, no transmitter found" ); + } - // Create observation model - LinkDefinition uplinkLinkEnds; - uplinkLinkEnds[ transmitter ] = linkEnds.at( transmitter ); - uplinkLinkEnds[ receiver ] = linkEnds.at( reflector1 ); + std::shared_ptr< ObservationBias< 1 > > observationBias; + if( observationSettings->biasSettings_ != nullptr ) + { + observationBias = + createObservationBiasCalculator( linkEnds, + observationSettings->observableType_, + observationSettings->biasSettings_, + bodies ); + } - LinkDefinition downlinkLinkEnds; - downlinkLinkEnds[ transmitter ] = linkEnds.at( reflector1 ); - downlinkLinkEnds[ receiver ] = linkEnds.at( receiver ); + // Create observation model + LinkDefinition uplinkLinkEnds; + uplinkLinkEnds[ transmitter ] = linkEnds.at( transmitter ); + uplinkLinkEnds[ receiver ] = linkEnds.at( reflector1 ); - std::shared_ptr< TwoWayDopplerObservationSettings > twoWayDopplerSettings = - std::dynamic_pointer_cast< TwoWayDopplerObservationSettings >( observationSettings ); + LinkDefinition downlinkLinkEnds; + downlinkLinkEnds[ transmitter ] = linkEnds.at( reflector1 ); + downlinkLinkEnds[ receiver ] = linkEnds.at( receiver ); + std::shared_ptr< TwoWayDopplerObservationSettings > twoWayDopplerSettings = + std::dynamic_pointer_cast< TwoWayDopplerObservationSettings >( + observationSettings ); - if( twoWayDopplerSettings == nullptr ) - { - observationModel = std::make_shared< TwoWayDopplerObservationModel< - ObservationScalarType, TimeType > >( + 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 ) ), + 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 ); - } - else - { - observationModel = std::make_shared< TwoWayDopplerObservationModel< - ObservationScalarType, TimeType > >( + } + 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_ ); - } - - break; - } + 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_ ); + } - 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 ) - { - 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" ); - } + break; + } + + 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 ) + { + 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< OneWayDifferencedRangeObservationModel< - ObservationScalarType, TimeType > >( + // Create observation model + observationModel = std::make_shared< + OneWayDifferencedRangeObservationModel< ObservationScalarType, TimeType > >( linkEnds, createLightTimeCalculator< ObservationScalarType, TimeType >( - linkEnds, transmitter, receiver, - bodies, topLevelObservableType, observationSettings->lightTimeCorrectionsList_, - observationSettings->lightTimeConvergenceCriteria_ ), + linkEnds, + transmitter, + receiver, + bodies, + topLevelObservableType, + observationSettings->lightTimeCorrectionsList_, + observationSettings->lightTimeConvergenceCriteria_ ), createLightTimeCalculator< ObservationScalarType, TimeType >( - linkEnds, transmitter, receiver, - bodies, topLevelObservableType, observationSettings->lightTimeCorrectionsList_, - observationSettings->lightTimeConvergenceCriteria_ ), + linkEnds, + transmitter, + receiver, + bodies, + topLevelObservableType, + observationSettings->lightTimeCorrectionsList_, + observationSettings->lightTimeConvergenceCriteria_ ), observationBias ); - 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 ); + 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 ); + } - // Create observation bias object - std::shared_ptr< ObservationBias< 1 > > observationBias; - if( observationSettings->biasSettings_ != nullptr ) - { - observationBias = - createObservationBiasCalculator( - linkEnds, observationSettings->observableType_, observationSettings->biasSettings_, bodies ); - } + // Create observation bias object + std::shared_ptr< ObservationBias< 1 > > observationBias; + if( observationSettings->biasSettings_ != nullptr ) + { + observationBias = + createObservationBiasCalculator( linkEnds, + observationSettings->observableType_, + observationSettings->biasSettings_, + bodies ); + } - std::shared_ptr< NWayRangeObservationSettings > nWayRangeObservationSettings = - std::dynamic_pointer_cast< NWayRangeObservationSettings >( observationSettings ); - if( nWayRangeObservationSettings != nullptr && nWayRangeObservationSettings->oneWayRangeObsevationSettings_.size( ) - != linkEnds.size( ) - 1 ) - { - throw std::runtime_error( "Error when making n-way range, input data is inconsistent" ); - } + 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" ); + } - // 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++ ) + // 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 ) { - if( nWayRangeObservationSettings->oneWayRangeObsevationSettings_.at( i )->observableType_ != one_way_range ) + multiLegLightTimeConvergenceCriteria = + nWayRangeObservationSettings->multiLegLightTimeConvergenceCriteria_; + for( unsigned int i = 0; i < linkEnds.size( ) - 1; i++ ) { - throw std::runtime_error( "Error in n-way observable creation, constituent link is not of type 1-way." ); - } - lightTimeCorrectionsList.push_back( - nWayRangeObservationSettings->oneWayRangeObsevationSettings_.at( i )->lightTimeCorrectionsList_ ); - singleLegsLightTimeConvergenceCriteriaList.push_back( - nWayRangeObservationSettings->oneWayRangeObsevationSettings_.at( i )->lightTimeConvergenceCriteria_ ); - if( nWayRangeObservationSettings->oneWayRangeObsevationSettings_.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( 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" ); + } } } - } - else - { - multiLegLightTimeConvergenceCriteria = observationSettings->lightTimeConvergenceCriteria_; - for( unsigned int i = 0; i < linkEnds.size( ) - 1; i++ ) + else { - lightTimeCorrectionsList.push_back( - observationSettings->lightTimeCorrectionsList_ ); - singleLegsLightTimeConvergenceCriteriaList.push_back( - observationSettings->lightTimeConvergenceCriteria_ ); + multiLegLightTimeConvergenceCriteria = + observationSettings->lightTimeConvergenceCriteria_; + for( unsigned int i = 0; i < linkEnds.size( ) - 1; 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, - singleLegsLightTimeConvergenceCriteriaList, - multiLegLightTimeConvergenceCriteria ); + // 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 > >( + // 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 ); - } - - std::shared_ptr< ObservationBias< 1 > > observationBias; - if( observationSettings->biasSettings_ != nullptr ) - { - observationBias = - createObservationBiasCalculator( - linkEnds, observationSettings->observableType_, observationSettings->biasSettings_, bodies ); + 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 ); + } - 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< ObservationBias< 1 > > observationBias; + if( observationSettings->biasSettings_ != nullptr ) + { + observationBias = + createObservationBiasCalculator( linkEnds, + observationSettings->observableType_, + observationSettings->biasSettings_, + bodies ); + } - 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 ); + 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< ObservationBias< 1 > > observationBias; - if( observationSettings->biasSettings_ != nullptr ) - { - observationBias = createObservationBiasCalculator( - linkEnds, observationSettings->observableType_, observationSettings->biasSettings_, bodies ); - } + 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::function< double ( observation_models::FrequencyBands, observation_models::FrequencyBands ) > turnaroundRatioFunction = - getTurnaroundFunction( bodies, linkEnds ); + std::shared_ptr< ObservationBias< 1 > > observationBias; + if( observationSettings->biasSettings_ != nullptr ) + { + observationBias = + createObservationBiasCalculator( linkEnds, + observationSettings->observableType_, + observationSettings->biasSettings_, + bodies ); + } - std::shared_ptr< ground_stations::StationFrequencyInterpolator > transmittingFrequencyInterpolator = - getTransmittingFrequencyInterpolator( bodies, linkEnds ); + 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 ) + std::map< LinkEndType, std::shared_ptr< ground_stations::GroundStationState > > + stationStates; + for( auto it: linkEnds ) { - stationStates[ it.first ] = - bodies.at( linkEnds.at( it.first ).bodyName_ )->getGroundStation( linkEnds.at( it.first ).stationName_ )->getNominalStationState( ); + 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< 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< observation_models::MultiLegLightTimeCalculator< ObservationScalarType, TimeType > > multiLegLightTimeCalculator; - try - { - std::vector< std::shared_ptr< LightTimeConvergenceCriteria > > singleLegsLightTimeConvergenceCriteriaList; - std::shared_ptr< LightTimeConvergenceCriteria > multiLegLightTimeConvergenceCriteria; - std::vector< std::vector< std::shared_ptr< LightTimeCorrectionSettings > > > lightTimeCorrectionsList; - multiLegLightTimeConvergenceCriteria = observationSettings->lightTimeConvergenceCriteria_; - for( unsigned int i = 0; i < linkEnds.size( ) - 1; i++ ) + 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 ) { - lightTimeCorrectionsList.push_back( - observationSettings->lightTimeCorrectionsList_ ); - singleLegsLightTimeConvergenceCriteriaList.push_back( - observationSettings->lightTimeConvergenceCriteria_ ); + throw std::runtime_error( + "Error when making DSN N-way range, input data is inconsistent" ); } - // Create multi-leg light time calculator - multiLegLightTimeCalculator = createMultiLegLightTimeCalculator< ObservationScalarType, TimeType >( - linkEnds, bodies, topLevelObservableType, lightTimeCorrectionsList, - singleLegsLightTimeConvergenceCriteriaList, - multiLegLightTimeConvergenceCriteria ); - - } - 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 ); - } + 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< ObservationBias< 1 > > observationBias; - if( observationSettings->biasSettings_ != nullptr ) - { - observationBias = createObservationBiasCalculator( - linkEnds, observationSettings->observableType_, observationSettings->biasSettings_, bodies ); - } + 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 ); + } - std::shared_ptr< ground_stations::StationFrequencyInterpolator > transmittingFrequencyInterpolator = - getTransmittingFrequencyInterpolator( 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 ) + { + stationStates[ it.first ] = + bodies.at( linkEnds.at( it.first ).bodyName_ ) + ->getGroundStation( + linkEnds.at( it.first ).stationName_ ) + ->getNominalStationState( ); + } + } - 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 ) + observationModel = std::make_shared< + DsnNWayRangeObservationModel< ObservationScalarType, TimeType > >( + linkEnds, + multiLegLightTimeCalculator, + transmittingFrequencyInterpolator, + observationBias, + stationStates ); + } + catch( const std::exception& caughtException ) { - stationStates[ it.first ] = - bodies.at( linkEnds.at( it.first ).bodyName_ )->getGroundStation( linkEnds.at( it.first ).stationName_ )->getNominalStationState( ); + std::string exceptionText = std::string( caughtException.what( ) ); + throw std::runtime_error( + "Error when creating DSN N-way range observation model, error: " + + exceptionText ); } + break; } - - 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 >( "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 ) + 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." ); + } + } + catch( const std::exception& caughtException ) { - throw std::runtime_error( "Error when getting two-way Doppler model for measured frequency observable, model in null." ); + std::string exceptionText = std::string( caughtException.what( ) ); + throw std::runtime_error( + "Error when creating Doppler Measured Frequency observation model, " + "error: " + + exceptionText ); } - } - 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 bias settings - std::shared_ptr< ObservationBias< 1 > > observationBias; - if( observationSettings->biasSettings_ != nullptr ) - { - observationBias = createObservationBiasCalculator( - linkEnds, observationSettings->observableType_, observationSettings->biasSettings_, bodies ); - } + // Determine the bias settings + std::shared_ptr< ObservationBias< 1 > > observationBias; + if( observationSettings->biasSettings_ != nullptr ) + { + observationBias = + createObservationBiasCalculator( linkEnds, + observationSettings->observableType_, + observationSettings->biasSettings_, + bodies ); + } - // Determine the turnaround ratio function - std::function< double ( observation_models::FrequencyBands, observation_models::FrequencyBands ) > turnaroundRatioFunction = - getTurnaroundFunction( bodies, linkEnds ); + // Determine the turnaround ratio function + std::function< double( observation_models::FrequencyBands, + observation_models::FrequencyBands ) > + turnaroundRatioFunction = getTurnaroundFunction( bodies, linkEnds ); - // Check if transmitter has frequency calculator - std::shared_ptr< ground_stations::StationFrequencyInterpolator > transmittingFrequencyInterpolator = - getTransmittingFrequencyInterpolator( bodies, 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 ) + std::map< LinkEndType, std::shared_ptr< ground_stations::GroundStationState > > + stationStates; + for( auto it: linkEnds ) { - stationStates[ it.first ] = - bodies.at( linkEnds.at( it.first ).bodyName_ )->getGroundStation( linkEnds.at( it.first ).stationName_ )->getNominalStationState( ); + 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( ); + } } - } - observationModel = std::make_shared< - DopplerMeasuredFrequencyObservationModel< ObservationScalarType, TimeType > >( - linkEnds, twoWayDopplerModel, + 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; } - }; //! Interface class for creating observation models of size 2. template< typename ObservationScalarType, typename TimeType > class ObservationModelCreator< 2, ObservationScalarType, TimeType > { -public: - + public: //! Function to create an observation model of size 2. /*! * Function to create an observation model of size 2. * \param linkEnds Link ends for observation model that is to be created - * \param observationSettings Settings for observation model that is to be created (must be for observation model if size 1). - * \param bodies List of body objects that comprises the environment - * \return Observation model of required settings. + * \param observationSettings Settings for observation model that is to be created (must be for + * observation model if size 1). \param bodies List of body objects that comprises the + * environment \return Observation model of required settings. */ - static std::shared_ptr< observation_models::ObservationModel< - 2, ObservationScalarType, TimeType > > createObservationModel( - const std::shared_ptr< ObservationModelSettings > observationSettings, - const simulation_setup::SystemOfBodies &bodies, - ObservableType topLevelObservableType = undefined_observation_model ) + static std::shared_ptr< + observation_models::ObservationModel< 2, ObservationScalarType, TimeType > > + createObservationModel( const std::shared_ptr< ObservationModelSettings > observationSettings, + const simulation_setup::SystemOfBodies& bodies, + ObservableType topLevelObservableType = undefined_observation_model ) { using namespace observation_models; - std::shared_ptr< observation_models::ObservationModel< - 2, ObservationScalarType, TimeType > > observationModel; + std::shared_ptr< + observation_models::ObservationModel< 2, ObservationScalarType, TimeType > > + observationModel; LinkEnds linkEnds = observationSettings->linkEnds_.linkEnds_; // Check type of observation model. switch( observationSettings->observableType_ ) { - case angular_position: - { - // Check consistency input. - if( linkEnds.size( ) != 2 ) - { - std::string errorMessage = - "Error when making angular position 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 angular position model, no receiver found" ); - } - if( linkEnds.count( transmitter ) == 0 ) - { - throw std::runtime_error( "Error when making angular position model, no transmitter found" ); - } - + case angular_position: { + // Check consistency input. + if( linkEnds.size( ) != 2 ) + { + std::string errorMessage = "Error when making angular position 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 angular position model, no receiver found" ); + } + if( linkEnds.count( transmitter ) == 0 ) + { + throw std::runtime_error( + "Error when making angular position model, no transmitter found" ); + } - std::shared_ptr< ObservationBias< 2 > > observationBias; - if( observationSettings->biasSettings_ != nullptr ) - { - observationBias = - createObservationBiasCalculator< 2 >( - linkEnds, observationSettings->observableType_, observationSettings->biasSettings_,bodies ); - } + std::shared_ptr< ObservationBias< 2 > > observationBias; + if( observationSettings->biasSettings_ != nullptr ) + { + observationBias = createObservationBiasCalculator< 2 >( + linkEnds, + observationSettings->observableType_, + observationSettings->biasSettings_, + bodies ); + } - // Create observation model - observationModel = std::make_shared< AngularPositionObservationModel< - ObservationScalarType, TimeType > >( + // Create observation model + observationModel = std::make_shared< + AngularPositionObservationModel< ObservationScalarType, TimeType > >( linkEnds, createLightTimeCalculator< ObservationScalarType, TimeType >( - linkEnds, transmitter, receiver, - bodies, topLevelObservableType, observationSettings->lightTimeCorrectionsList_, - observationSettings->lightTimeConvergenceCriteria_ ), + linkEnds, + transmitter, + receiver, + bodies, + topLevelObservableType, + observationSettings->lightTimeCorrectionsList_, + observationSettings->lightTimeConvergenceCriteria_ ), observationBias ); - break; - } - case relative_angular_position: - { - // Check consistency input. - if( linkEnds.size( ) != 3 ) - { - std::string errorMessage = - "Error when making angular position 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 angular position model, no receiver found" ); + break; } - if( linkEnds.count( transmitter ) == 0 ) - { - throw std::runtime_error( "Error when making angular position model, no transmitter found" ); - } - if( linkEnds.count( transmitter2 ) == 0 ) - { - throw std::runtime_error( "Error when making angular position model, no second transmitter found" ); - } - + case relative_angular_position: { + // Check consistency input. + if( linkEnds.size( ) != 3 ) + { + std::string errorMessage = "Error when making angular position 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 angular position model, no receiver found" ); + } + if( linkEnds.count( transmitter ) == 0 ) + { + throw std::runtime_error( + "Error when making angular position model, no transmitter found" ); + } + if( linkEnds.count( transmitter2 ) == 0 ) + { + throw std::runtime_error( + "Error when making angular position model, no second transmitter " + "found" ); + } - std::shared_ptr< ObservationBias< 2 > > observationBias; - if( observationSettings->biasSettings_ != nullptr ) - { - observationBias = - createObservationBiasCalculator< 2 >( - linkEnds, observationSettings->observableType_, observationSettings->biasSettings_,bodies ); - } + std::shared_ptr< ObservationBias< 2 > > observationBias; + if( observationSettings->biasSettings_ != nullptr ) + { + observationBias = createObservationBiasCalculator< 2 >( + linkEnds, + observationSettings->observableType_, + observationSettings->biasSettings_, + bodies ); + } - // Create observation model - observationModel = std::make_shared< RelativeAngularPositionObservationModel< - ObservationScalarType, TimeType > >( - linkEnds, createLightTimeCalculator< ObservationScalarType, TimeType >( - linkEnds, transmitter, receiver, bodies, - topLevelObservableType, observationSettings->lightTimeCorrectionsList_, - observationSettings->lightTimeConvergenceCriteria_ ), + // Create observation model + observationModel = std::make_shared< + RelativeAngularPositionObservationModel< ObservationScalarType, + TimeType > >( + linkEnds, createLightTimeCalculator< ObservationScalarType, TimeType >( - linkEnds, transmitter2, receiver, bodies, - topLevelObservableType, observationSettings->lightTimeCorrectionsList_, - observationSettings->lightTimeConvergenceCriteria_ ), observationBias ); - - break; - } - default: - std::string errorMessage = "Error, observable " + std::to_string( - observationSettings->observableType_ ) + - " not recognized when making size 2 observation model."; - throw std::runtime_error( errorMessage ); - break; + linkEnds, + transmitter, + receiver, + bodies, + topLevelObservableType, + observationSettings->lightTimeCorrectionsList_, + observationSettings->lightTimeConvergenceCriteria_ ), + createLightTimeCalculator< ObservationScalarType, TimeType >( + linkEnds, + transmitter2, + receiver, + bodies, + topLevelObservableType, + observationSettings->lightTimeCorrectionsList_, + observationSettings->lightTimeConvergenceCriteria_ ), + observationBias ); + + break; + } + default: + std::string errorMessage = "Error, observable " + + std::to_string( observationSettings->observableType_ ) + + " not recognized when making size 2 observation model."; + throw std::runtime_error( errorMessage ); + break; } return observationModel; } - }; //! Interface class for creating observation models of size 3. template< typename ObservationScalarType, typename TimeType > class ObservationModelCreator< 3, ObservationScalarType, TimeType > { -public: - + public: //! Function to create an observation model of size 3. /*! * Function to create an observation model of size 3. * \param linkEnds Link ends for observation model that is to be created - * \param observationSettings Settings for observation model that is to be created (must be for observation model if size 1). - * \param bodies List of body objects that comprises the environment - * \return Observation model of required settings. + * \param observationSettings Settings for observation model that is to be created (must be for + * observation model if size 1). \param bodies List of body objects that comprises the + * environment \return Observation model of required settings. */ - static std::shared_ptr< observation_models::ObservationModel< 3, ObservationScalarType, TimeType > > createObservationModel( - const std::shared_ptr< ObservationModelSettings > observationSettings, - const simulation_setup::SystemOfBodies &bodies, - ObservableType topLevelObservableType = undefined_observation_model ) + static std::shared_ptr< + observation_models::ObservationModel< 3, ObservationScalarType, TimeType > > + createObservationModel( const std::shared_ptr< ObservationModelSettings > observationSettings, + const simulation_setup::SystemOfBodies& bodies, + ObservableType topLevelObservableType = undefined_observation_model ) { using namespace observation_models; - std::shared_ptr< observation_models::ObservationModel< - 3, ObservationScalarType, TimeType > > observationModel; + std::shared_ptr< + observation_models::ObservationModel< 3, ObservationScalarType, TimeType > > + observationModel; LinkEnds linkEnds = observationSettings->linkEnds_.linkEnds_; // Check type of observation model. switch( observationSettings->observableType_ ) { - case position_observable: - { - // Check consistency input. - if( linkEnds.size( ) != 1 ) - { - std::string errorMessage = - "Error when making position observable model, " + - std::to_string( linkEnds.size( ) ) + " link ends found"; - throw std::runtime_error( errorMessage ); - } - - if( linkEnds.count( observed_body ) == 0 ) - { - throw std::runtime_error( "Error when making position observable model, no observed_body found" ); - } + case position_observable: { + // Check consistency input. + if( linkEnds.size( ) != 1 ) + { + std::string errorMessage = "Error when making position observable model, " + + std::to_string( linkEnds.size( ) ) + " link ends found"; + throw std::runtime_error( errorMessage ); + } - if( observationSettings->lightTimeCorrectionsList_.size( ) > 0 ) - { - throw std::runtime_error( "Error when making position observable model, found light time corrections" ); - } - if( linkEnds.at( observed_body ).stationName_ != "" ) - { - throw std::runtime_error( "Error, cannot yet create position function for reference point" ); - } + if( linkEnds.count( observed_body ) == 0 ) + { + throw std::runtime_error( + "Error when making position observable model, no observed_body found" ); + } - std::shared_ptr< ObservationBias< 3 > > observationBias; - if( observationSettings->biasSettings_ != nullptr ) - { - observationBias = - createObservationBiasCalculator< 3 >( - linkEnds, observationSettings->observableType_, observationSettings->biasSettings_,bodies ); - } + if( observationSettings->lightTimeCorrectionsList_.size( ) > 0 ) + { + throw std::runtime_error( + "Error when making position observable model, found light time " + "corrections" ); + } + if( linkEnds.at( observed_body ).stationName_ != "" ) + { + throw std::runtime_error( + "Error, cannot yet create position function for reference point" ); + } + std::shared_ptr< ObservationBias< 3 > > observationBias; + if( observationSettings->biasSettings_ != nullptr ) + { + observationBias = createObservationBiasCalculator< 3 >( + linkEnds, + observationSettings->observableType_, + observationSettings->biasSettings_, + bodies ); + } - // Create observation model - observationModel = std::make_shared< PositionObservationModel< - ObservationScalarType, TimeType > >( + // Create observation model + observationModel = std::make_shared< + PositionObservationModel< ObservationScalarType, TimeType > >( linkEnds, std::bind( &simulation_setup::Body::getStateInBaseFrameFromEphemeris< - ObservationScalarType, TimeType >, - bodies.at( linkEnds.at( observed_body ).bodyName_ ), std::placeholders::_1 ), + ObservationScalarType, + TimeType >, + bodies.at( linkEnds.at( observed_body ).bodyName_ ), + std::placeholders::_1 ), observationBias ); - break; - } - case relative_position_observable: - { - // Check consistency input. - if( linkEnds.size( ) != 2 ) - { - std::string errorMessage = "Error when making relative position observable model, " + std::to_string( linkEnds.size( ) ) + " link ends found"; - throw std::runtime_error( errorMessage ); + break; } + case relative_position_observable: { + // Check consistency input. + if( linkEnds.size( ) != 2 ) + { + std::string errorMessage = + "Error when making relative position observable model, " + + std::to_string( linkEnds.size( ) ) + " link ends found"; + throw std::runtime_error( errorMessage ); + } - if( linkEnds.count( observed_body ) == 0 ) - { - throw std::runtime_error( "Error when making relative position observable model, no observed_body found" ); - } - if( linkEnds.count( observer ) == 0 ) - { - throw std::runtime_error( "Error when making relative position observable model, no observer found" ); - } + if( linkEnds.count( observed_body ) == 0 ) + { + throw std::runtime_error( + "Error when making relative position observable model, no " + "observed_body found" ); + } + if( linkEnds.count( observer ) == 0 ) + { + throw std::runtime_error( + "Error when making relative position observable model, no observer " + "found" ); + } - if( observationSettings->lightTimeCorrectionsList_.size( ) > 0 ) - { - throw std::runtime_error( "Error when making relative position observable model, found light time corrections" ); - } - if( linkEnds.at( observed_body ).stationName_ != "" ) - { - throw std::runtime_error( "Error, cannot yet create position function for reference point" ); - } + if( observationSettings->lightTimeCorrectionsList_.size( ) > 0 ) + { + throw std::runtime_error( + "Error when making relative position observable model, found light " + "time corrections" ); + } + if( linkEnds.at( observed_body ).stationName_ != "" ) + { + throw std::runtime_error( + "Error, cannot yet create position function for reference point" ); + } - std::shared_ptr< ObservationBias< 3 > > observationBias; - if( observationSettings->biasSettings_ != nullptr ) - { - observationBias = createObservationBiasCalculator< 3 >( - linkEnds, observationSettings->observableType_, observationSettings->biasSettings_,bodies ); - } + std::shared_ptr< ObservationBias< 3 > > observationBias; + if( observationSettings->biasSettings_ != nullptr ) + { + observationBias = createObservationBiasCalculator< 3 >( + linkEnds, + observationSettings->observableType_, + observationSettings->biasSettings_, + bodies ); + } - // Create observation model - observationModel = std::make_shared< RelativePositionObservationModel< - ObservationScalarType, TimeType > >( - linkEnds, - std::bind( &simulation_setup::Body::getStateInBaseFrameFromEphemeris< ObservationScalarType, TimeType >, - bodies.at( linkEnds.at( observed_body ).bodyName_ ), std::placeholders::_1 ), - std::bind( &simulation_setup::Body::getStateInBaseFrameFromEphemeris< ObservationScalarType, TimeType >, - bodies.at( linkEnds.at( observer ).bodyName_ ), std::placeholders::_1 ), - observationBias ); - break; - } - case euler_angle_313_observable: - { - // Check consistency input. - if( linkEnds.size( ) != 1 ) - { - std::string errorMessage = - "Error when making euler angle observable model, " + - std::to_string( linkEnds.size( ) ) + " link ends found"; - throw std::runtime_error( errorMessage ); + // Create observation model + observationModel = std::make_shared< + RelativePositionObservationModel< ObservationScalarType, TimeType > >( + linkEnds, + std::bind( &simulation_setup::Body::getStateInBaseFrameFromEphemeris< + ObservationScalarType, + TimeType >, + bodies.at( linkEnds.at( observed_body ).bodyName_ ), + std::placeholders::_1 ), + std::bind( &simulation_setup::Body::getStateInBaseFrameFromEphemeris< + ObservationScalarType, + TimeType >, + bodies.at( linkEnds.at( observer ).bodyName_ ), + std::placeholders::_1 ), + observationBias ); + break; } + case euler_angle_313_observable: { + // Check consistency input. + if( linkEnds.size( ) != 1 ) + { + std::string errorMessage = "Error when making euler angle observable model, " + + std::to_string( linkEnds.size( ) ) + " link ends found"; + throw std::runtime_error( errorMessage ); + } - if( linkEnds.count( observed_body ) == 0 ) - { - throw std::runtime_error( "Error when making euler angle observable model, no observed_body found" ); - } + if( linkEnds.count( observed_body ) == 0 ) + { + throw std::runtime_error( + "Error when making euler angle observable model, no observed_body " + "found" ); + } - if( observationSettings->lightTimeCorrectionsList_.size( ) > 0 ) - { - throw std::runtime_error( "Error when making euler angle observable model, found light time corrections" ); - } - if( linkEnds.at( observed_body ).stationName_ != "" ) - { - throw std::runtime_error( "Error, cannot yet create euler angle function for reference point" ); - } + if( observationSettings->lightTimeCorrectionsList_.size( ) > 0 ) + { + throw std::runtime_error( + "Error when making euler angle observable model, found light time " + "corrections" ); + } + if( linkEnds.at( observed_body ).stationName_ != "" ) + { + throw std::runtime_error( + "Error, cannot yet create euler angle function for reference point" ); + } - std::shared_ptr< ObservationBias< 3 > > observationBias; - if( observationSettings->biasSettings_ != nullptr ) - { - observationBias = - createObservationBiasCalculator< 3 >( - linkEnds, observationSettings->observableType_, observationSettings->biasSettings_, bodies ); - } + std::shared_ptr< ObservationBias< 3 > > observationBias; + if( observationSettings->biasSettings_ != nullptr ) + { + observationBias = createObservationBiasCalculator< 3 >( + linkEnds, + observationSettings->observableType_, + observationSettings->biasSettings_, + bodies ); + } - std::function< Eigen::Quaterniond( const TimeType ) > toBodyFixedFrameFunction; - if( bodies.at( linkEnds.at( observed_body ).bodyName_ )->getRotationalEphemeris( ) == nullptr ) - { - throw std::runtime_error( "Error, cannot euler angle observable; no rotation model found" ); - } - else - { - toBodyFixedFrameFunction = std::bind( - &ephemerides::RotationalEphemeris::getRotationToTargetFrameTemplated< TimeType >, - bodies.at( linkEnds.at( observed_body ).bodyName_ )->getRotationalEphemeris( ), + std::function< Eigen::Quaterniond( const TimeType ) > toBodyFixedFrameFunction; + if( bodies.at( linkEnds.at( observed_body ).bodyName_ ) + ->getRotationalEphemeris( ) == nullptr ) + { + throw std::runtime_error( + "Error, cannot euler angle observable; no rotation model found" ); + } + else + { + toBodyFixedFrameFunction = std::bind( + &ephemerides::RotationalEphemeris::getRotationToTargetFrameTemplated< + TimeType >, + bodies.at( linkEnds.at( observed_body ).bodyName_ ) + ->getRotationalEphemeris( ), std::placeholders::_1 ); - } - - - // Create observation model - observationModel = std::make_shared< EulerAngle313ObservationModel< - ObservationScalarType, TimeType > >( - linkEnds, - toBodyFixedFrameFunction, observationBias ); + } - break; - } - case velocity_observable: - { - // Check consistency input. - if( linkEnds.size( ) != 1 ) - { - std::string errorMessage = - "Error when making velocity observable model, " + std::to_string( linkEnds.size( ) ) + " link ends found"; - throw std::runtime_error( errorMessage ); - } + // Create observation model + observationModel = std::make_shared< + EulerAngle313ObservationModel< ObservationScalarType, TimeType > >( + linkEnds, toBodyFixedFrameFunction, observationBias ); - if( linkEnds.count( observed_body ) == 0 ) - { - throw std::runtime_error( "Error when making velocity observable model, no observed_body found" ); + break; } + case velocity_observable: { + // Check consistency input. + if( linkEnds.size( ) != 1 ) + { + std::string errorMessage = "Error when making velocity observable model, " + + std::to_string( linkEnds.size( ) ) + " link ends found"; + throw std::runtime_error( errorMessage ); + } - if( observationSettings->lightTimeCorrectionsList_.size( ) > 0 ) - { - - throw std::runtime_error( "Error when making velocity observable model, found light time corrections" ); - } - if( linkEnds.at( observed_body ).stationName_ != "" ) - { - throw std::runtime_error( "Error, cannot yet create velocity function for reference point" ); - } + if( linkEnds.count( observed_body ) == 0 ) + { + throw std::runtime_error( + "Error when making velocity observable model, no observed_body found" ); + } - std::shared_ptr< ObservationBias< 3 > > observationBias; - if( observationSettings->biasSettings_ != NULL ) - { - observationBias = - createObservationBiasCalculator< 3 >( - linkEnds, observationSettings->observableType_, observationSettings->biasSettings_,bodies ); - } + if( observationSettings->lightTimeCorrectionsList_.size( ) > 0 ) + { + throw std::runtime_error( + "Error when making velocity observable model, found light time " + "corrections" ); + } + if( linkEnds.at( observed_body ).stationName_ != "" ) + { + throw std::runtime_error( + "Error, cannot yet create velocity function for reference point" ); + } + std::shared_ptr< ObservationBias< 3 > > observationBias; + if( observationSettings->biasSettings_ != NULL ) + { + observationBias = createObservationBiasCalculator< 3 >( + linkEnds, + observationSettings->observableType_, + observationSettings->biasSettings_, + bodies ); + } - // Create observation model - observationModel = std::make_shared< VelocityObservationModel< - ObservationScalarType, TimeType > >( + // Create observation model + observationModel = std::make_shared< + VelocityObservationModel< ObservationScalarType, TimeType > >( linkEnds, std::bind( &simulation_setup::Body::getStateInBaseFrameFromEphemeris< - ObservationScalarType, TimeType >, - bodies.at( linkEnds.at( observed_body ).bodyName_ ), std::placeholders::_1 ), + ObservationScalarType, + TimeType >, + bodies.at( linkEnds.at( observed_body ).bodyName_ ), + std::placeholders::_1 ), observationBias ); - break; - } - default: - std::string errorMessage = "Error, observable " + std::to_string( - observationSettings->observableType_ ) + - " not recognized when making size 3 observation model."; - throw std::runtime_error( errorMessage ); - break; + break; + } + default: + std::string errorMessage = "Error, observable " + + std::to_string( observationSettings->observableType_ ) + + " not recognized when making size 3 observation model."; + throw std::runtime_error( errorMessage ); + break; } return observationModel; } @@ -2697,238 +3235,276 @@ class ObservationModelCreator< 3, ObservationScalarType, TimeType > /*! * Function to create an object to simulate observations of a given type * \param observableType Type of observable for which object is to simulate ObservationSimulator - * \param settingsPerLinkEnds Map of settings for the observation models that are to be created in the simulator object: one - * for each required set of link ends (each settings object must be consistent with observableType). - * \param bodies Map of Body objects that comprise the environment + * \param settingsPerLinkEnds Map of settings for the observation models that are to be created in + * the simulator object: one for each required set of link ends (each settings object must be + * consistent with observableType). \param bodies Map of Body objects that comprise the environment * \return Object that simulates the observables according to the provided settings. */ -template< int ObservationSize = 1, typename ObservationScalarType = double, typename TimeType = double > -std::shared_ptr< ObservationSimulator< ObservationSize, ObservationScalarType, TimeType > > createObservationSimulator( +template< int ObservationSize = 1, + typename ObservationScalarType = double, + typename TimeType = double > +std::shared_ptr< ObservationSimulator< ObservationSize, ObservationScalarType, TimeType > > +createObservationSimulator( const ObservableType observableType, - const std::vector< std::shared_ptr< ObservationModelSettings > > settingsList, - const simulation_setup::SystemOfBodies &bodies ) + const std::vector< std::shared_ptr< ObservationModelSettings > > settingsList, + const simulation_setup::SystemOfBodies& bodies ) { - std::map< LinkEnds, std::shared_ptr< ObservationModel< ObservationSize, ObservationScalarType, TimeType > > > + std::map< LinkEnds, + std::shared_ptr< + ObservationModel< ObservationSize, ObservationScalarType, TimeType > > > observationModels; // Iterate over all link ends for( unsigned int i = 0; i < settingsList.size( ); i++ ) { - observationModels[ settingsList.at( i )->linkEnds_.linkEnds_ ] = ObservationModelCreator< - ObservationSize, ObservationScalarType, TimeType >::createObservationModel( - settingsList.at( i ), bodies ); + observationModels[ settingsList.at( i )->linkEnds_.linkEnds_ ] = + ObservationModelCreator< ObservationSize, ObservationScalarType, TimeType >:: + createObservationModel( settingsList.at( i ), bodies ); } - return std::make_shared< ObservationSimulator< ObservationSize, ObservationScalarType, TimeType > >( - observableType, observationModels ); + return std::make_shared< + ObservationSimulator< ObservationSize, ObservationScalarType, TimeType > >( + observableType, observationModels ); } //! Function to create a map of object to simulate observations (one object for each type of observable). /*! - * Function to create a map of object to simulate observations (one object for each type of observable). - * \param observationSettingsList List of settings for the observation models that are to be created in the simulator object - * \param bodies Map of Body objects that comprise the environment - * \return List of objects that simulate the observables according to the provided settings. + * Function to create a map of object to simulate observations (one object for each type of + * observable). \param observationSettingsList List of settings for the observation models that are + * to be created in the simulator object \param bodies Map of Body objects that comprise the + * environment \return List of objects that simulate the observables according to the provided + * settings. */ template< typename ObservationScalarType = double, typename TimeType = double > std::vector< std::shared_ptr< ObservationSimulatorBase< ObservationScalarType, TimeType > > > - createObservationSimulators( +createObservationSimulators( const std::vector< std::shared_ptr< ObservationModelSettings > >& observationSettingsList, const simulation_setup::SystemOfBodies& bodies ) { - std::vector< std::shared_ptr< ObservationSimulatorBase< ObservationScalarType, TimeType > > > observationSimulators; - std::map< ObservableType, std::vector< std::shared_ptr< ObservationModelSettings > > > sortedObservationSettingsList = - sortObservationModelSettingsByType( observationSettingsList ); + std::vector< std::shared_ptr< ObservationSimulatorBase< ObservationScalarType, TimeType > > > + observationSimulators; + std::map< ObservableType, std::vector< std::shared_ptr< ObservationModelSettings > > > + sortedObservationSettingsList = + sortObservationModelSettingsByType( observationSettingsList ); // Iterate over all observables - for( auto it : sortedObservationSettingsList ) + for( auto it: sortedObservationSettingsList ) { // Call createObservationSimulator of required observation size ObservableType observableType = it.first; int observableSize = getObservableSize( observableType ); switch( observableSize ) { - case 1: - { - observationSimulators.push_back( createObservationSimulator< 1, ObservationScalarType, TimeType >( - observableType, it.second, bodies ) ); - break; - } - case 2: - { - observationSimulators.push_back( createObservationSimulator< 2, ObservationScalarType, TimeType >( - observableType, it.second, bodies ) ); - break; - } - case 3: - { - observationSimulators.push_back( createObservationSimulator< 3, ObservationScalarType, TimeType >( - observableType, it.second, bodies ) ); - break; - } - default: - throw std::runtime_error( "Error, cannot create observation simulator for size other than 1,2 and 3 "); + case 1: { + observationSimulators.push_back( + createObservationSimulator< 1, ObservationScalarType, TimeType >( + observableType, it.second, bodies ) ); + break; + } + case 2: { + observationSimulators.push_back( + createObservationSimulator< 2, ObservationScalarType, TimeType >( + observableType, it.second, bodies ) ); + break; + } + case 3: { + observationSimulators.push_back( + createObservationSimulator< 3, ObservationScalarType, TimeType >( + observableType, it.second, bodies ) ); + break; + } + default: + throw std::runtime_error( + "Error, cannot create observation simulator for size other than 1,2 and " + "3 " ); } } return observationSimulators; } template< typename ObservationScalarType = double, typename TimeType = double > -std::map< ObservableType, std::shared_ptr< ObservationSimulatorBase< ObservationScalarType, TimeType > > > createSortedObservationSimulators( - const std::vector< std::shared_ptr< ObservationModelSettings > >& observationSettingsList, - const simulation_setup::SystemOfBodies& bodies ) +std::map< ObservableType, + std::shared_ptr< ObservationSimulatorBase< ObservationScalarType, TimeType > > > +createSortedObservationSimulators( + const std::vector< std::shared_ptr< ObservationModelSettings > >& observationSettingsList, + const simulation_setup::SystemOfBodies& bodies ) { - std::map< ObservableType, std::shared_ptr< ObservationSimulatorBase< ObservationScalarType, TimeType > > > sortedObservationSimulators; + std::map< ObservableType, + std::shared_ptr< ObservationSimulatorBase< ObservationScalarType, TimeType > > > + sortedObservationSimulators; - std::vector< std::shared_ptr< ObservationSimulatorBase< ObservationScalarType, TimeType > > > observationSimulators = - createObservationSimulators< ObservationScalarType, TimeType >( observationSettingsList, bodies ); + std::vector< std::shared_ptr< ObservationSimulatorBase< ObservationScalarType, TimeType > > > + observationSimulators = createObservationSimulators< ObservationScalarType, TimeType >( + observationSettingsList, bodies ); for( unsigned int i = 0; i < observationSimulators.size( ); i++ ) { - sortedObservationSimulators[ observationSimulators.at( i )->getObservableType( ) ] = observationSimulators.at( i ); + sortedObservationSimulators[ observationSimulators.at( i )->getObservableType( ) ] = + observationSimulators.at( i ); } return sortedObservationSimulators; - } - //! Typedef for list of light time corrections for a list of link ends -typedef std::map< observation_models::LinkEnds, -std::vector< std::vector< std::shared_ptr< observation_models::LightTimeCorrection > > > > -PerLinkEndPerLightTimeSolutionCorrections; - -template< typename ObservationScalarType, typename TimeType, int ObservationSize > -std::vector< std::vector< std::shared_ptr< observation_models::LightTimeCorrection > > > getLightTimeCorrections( - const std::shared_ptr< observation_models::ObservationModel< - ObservationSize, ObservationScalarType, TimeType> > observationModel ) +typedef std::map< + observation_models::LinkEnds, + std::vector< std::vector< std::shared_ptr< observation_models::LightTimeCorrection > > > > + PerLinkEndPerLightTimeSolutionCorrections; + +template< typename ObservationScalarType, typename TimeType, int ObservationSize > +std::vector< std::vector< std::shared_ptr< observation_models::LightTimeCorrection > > > +getLightTimeCorrections( + const std::shared_ptr< observation_models::ObservationModel< ObservationSize, + ObservationScalarType, + TimeType > > observationModel ) { // Clear list, for current link ends. - std::vector< std::vector< std::shared_ptr< observation_models::LightTimeCorrection > > > currentLightTimeCorrections; + std::vector< std::vector< std::shared_ptr< observation_models::LightTimeCorrection > > > + currentLightTimeCorrections; - std::vector< std::shared_ptr< observation_models::LightTimeCorrection > > singleObservableCorrectionList; + std::vector< std::shared_ptr< observation_models::LightTimeCorrection > > + singleObservableCorrectionList; // Check type of observable switch( observationModel->getObservableType( ) ) { - case observation_models::one_way_range: - { - std::shared_ptr< observation_models::OneWayRangeObservationModel - < ObservationScalarType, TimeType> > oneWayRangeModel = - std::dynamic_pointer_cast< observation_models::OneWayRangeObservationModel - < ObservationScalarType, TimeType> > - ( observationModel ); - singleObservableCorrectionList = ( - oneWayRangeModel->getLightTimeCalculator( )->getLightTimeCorrection( ) ); - break; - } - case observation_models::one_way_doppler: - { - std::shared_ptr< observation_models::OneWayDopplerObservationModel - < ObservationScalarType, TimeType> > oneWayRangeModel = - std::dynamic_pointer_cast< observation_models::OneWayDopplerObservationModel - < ObservationScalarType, TimeType> > - ( observationModel ); - singleObservableCorrectionList = ( - oneWayRangeModel->getLightTimeCalculator( )->getLightTimeCorrection( ) ); - break; - } - case observation_models::two_way_doppler: - { - std::shared_ptr< observation_models::TwoWayDopplerObservationModel - < ObservationScalarType, TimeType> > twoWaDopplerModel = - std::dynamic_pointer_cast< observation_models::TwoWayDopplerObservationModel - < ObservationScalarType, TimeType> > - ( observationModel ); - currentLightTimeCorrections.push_back( - twoWaDopplerModel->getUplinkDopplerCalculator( )->getLightTimeCalculator( )->getLightTimeCorrection( ) ); - currentLightTimeCorrections.push_back( - twoWaDopplerModel->getDownlinkDopplerCalculator( )->getLightTimeCalculator( )->getLightTimeCorrection( ) ); - break; - } - case observation_models::angular_position: - { - std::shared_ptr< observation_models::AngularPositionObservationModel - < ObservationScalarType, TimeType> > angularPositionModel = - std::dynamic_pointer_cast< observation_models::AngularPositionObservationModel - < ObservationScalarType, TimeType> > - ( observationModel ); - singleObservableCorrectionList = ( - angularPositionModel->getLightTimeCalculator( )->getLightTimeCorrection( ) ); - break; - } - case observation_models::one_way_differenced_range: - { - std::shared_ptr< observation_models::OneWayDifferencedRangeObservationModel - < ObservationScalarType, TimeType> > oneWayDifferencedRangeObservationModel = - std::dynamic_pointer_cast< observation_models::OneWayDifferencedRangeObservationModel - < ObservationScalarType, TimeType> > - ( observationModel ); - currentLightTimeCorrections.push_back( - oneWayDifferencedRangeObservationModel->getArcStartLightTimeCalculator( )-> - getLightTimeCorrection( ) ); - currentLightTimeCorrections.push_back( - oneWayDifferencedRangeObservationModel->getArcEndLightTimeCalculator( )-> - getLightTimeCorrection( ) ); - - break; - } - case observation_models::n_way_range: - { - std::shared_ptr< observation_models::NWayRangeObservationModel< ObservationScalarType, TimeType > > nWayRangeObservationModel = - std::dynamic_pointer_cast< observation_models::NWayRangeObservationModel< ObservationScalarType, TimeType > > - ( observationModel ); - std::vector< std::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > > lightTimeCalculatorList = - nWayRangeObservationModel->getLightTimeCalculators( ); - for( unsigned int i = 0; i < lightTimeCalculatorList.size( ); i++ ) - { - currentLightTimeCorrections.push_back( lightTimeCalculatorList.at( i )->getLightTimeCorrection( ) ); + case observation_models::one_way_range: { + std::shared_ptr< observation_models::OneWayRangeObservationModel< ObservationScalarType, + TimeType > > + oneWayRangeModel = std::dynamic_pointer_cast< + observation_models::OneWayRangeObservationModel< ObservationScalarType, + TimeType > >( + observationModel ); + singleObservableCorrectionList = + ( oneWayRangeModel->getLightTimeCalculator( )->getLightTimeCorrection( ) ); + break; } - break; - } - case observation_models::dsn_n_way_range: - { - std::shared_ptr< observation_models::DsnNWayRangeObservationModel< ObservationScalarType, TimeType > > nWayRangeObservationModel = - std::dynamic_pointer_cast< observation_models::DsnNWayRangeObservationModel< ObservationScalarType, TimeType > > - ( observationModel ); - std::vector< std::shared_ptr< observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > > lightTimeCalculatorList = - nWayRangeObservationModel->getLightTimeCalculator( )->getLightTimeCalculators( ); - for( unsigned int i = 0; i < lightTimeCalculatorList.size( ); i++ ) - { - currentLightTimeCorrections.push_back( lightTimeCalculatorList.at( i )->getLightTimeCorrection( ) ); + case observation_models::one_way_doppler: { + std::shared_ptr< + observation_models::OneWayDopplerObservationModel< ObservationScalarType, + TimeType > > + oneWayRangeModel = std::dynamic_pointer_cast< + observation_models::OneWayDopplerObservationModel< + ObservationScalarType, + TimeType > >( observationModel ); + singleObservableCorrectionList = + ( oneWayRangeModel->getLightTimeCalculator( )->getLightTimeCorrection( ) ); + break; } - break; - } - case observation_models::position_observable: - { - break; - } - case observation_models::euler_angle_313_observable: - { - break; - } - case observation_models::velocity_observable: - { - break; - } - case observation_models::relative_angular_position: - { - std::shared_ptr< observation_models::RelativeAngularPositionObservationModel< ObservationScalarType, TimeType> > relativeAngularPositionModel = - std::dynamic_pointer_cast< observation_models::RelativeAngularPositionObservationModel - < ObservationScalarType, TimeType> >( observationModel ); - currentLightTimeCorrections.push_back( - relativeAngularPositionModel->getLightTimeCalculatorFirstTransmitter( )->getLightTimeCorrection( ) ); - currentLightTimeCorrections.push_back( - relativeAngularPositionModel->getLightTimeCalculatorSecondTransmitter( )->getLightTimeCorrection( ) ); - break; - } - case observation_models::relative_position_observable: - { - break; - } - default: - std::string errorMessage = - "Error in light time correction list creation, observable type " + - std::to_string( observationModel->getObservableType( ) ) + " not recognized."; - throw std::runtime_error( errorMessage ); + case observation_models::two_way_doppler: { + std::shared_ptr< + observation_models::TwoWayDopplerObservationModel< ObservationScalarType, + TimeType > > + twoWaDopplerModel = std::dynamic_pointer_cast< + observation_models::TwoWayDopplerObservationModel< + ObservationScalarType, + TimeType > >( observationModel ); + currentLightTimeCorrections.push_back( twoWaDopplerModel->getUplinkDopplerCalculator( ) + ->getLightTimeCalculator( ) + ->getLightTimeCorrection( ) ); + currentLightTimeCorrections.push_back( + twoWaDopplerModel->getDownlinkDopplerCalculator( ) + ->getLightTimeCalculator( ) + ->getLightTimeCorrection( ) ); + break; + } + case observation_models::angular_position: { + std::shared_ptr< + observation_models::AngularPositionObservationModel< ObservationScalarType, + TimeType > > + angularPositionModel = std::dynamic_pointer_cast< + observation_models::AngularPositionObservationModel< + ObservationScalarType, + TimeType > >( observationModel ); + singleObservableCorrectionList = + ( angularPositionModel->getLightTimeCalculator( )->getLightTimeCorrection( ) ); + break; + } + case observation_models::one_way_differenced_range: { + std::shared_ptr< observation_models::OneWayDifferencedRangeObservationModel< + ObservationScalarType, + TimeType > > + oneWayDifferencedRangeObservationModel = std::dynamic_pointer_cast< + observation_models::OneWayDifferencedRangeObservationModel< + ObservationScalarType, + TimeType > >( observationModel ); + currentLightTimeCorrections.push_back( + oneWayDifferencedRangeObservationModel->getArcStartLightTimeCalculator( ) + ->getLightTimeCorrection( ) ); + currentLightTimeCorrections.push_back( + oneWayDifferencedRangeObservationModel->getArcEndLightTimeCalculator( ) + ->getLightTimeCorrection( ) ); + + break; + } + case observation_models::n_way_range: { + std::shared_ptr< observation_models::NWayRangeObservationModel< ObservationScalarType, + TimeType > > + nWayRangeObservationModel = std::dynamic_pointer_cast< + observation_models::NWayRangeObservationModel< ObservationScalarType, + TimeType > >( + observationModel ); + std::vector< std::shared_ptr< + observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > > + lightTimeCalculatorList = nWayRangeObservationModel->getLightTimeCalculators( ); + for( unsigned int i = 0; i < lightTimeCalculatorList.size( ); i++ ) + { + currentLightTimeCorrections.push_back( + lightTimeCalculatorList.at( i )->getLightTimeCorrection( ) ); + } + break; + } + case observation_models::dsn_n_way_range: { + std::shared_ptr< + observation_models::DsnNWayRangeObservationModel< ObservationScalarType, + TimeType > > + nWayRangeObservationModel = std::dynamic_pointer_cast< + observation_models::DsnNWayRangeObservationModel< ObservationScalarType, + TimeType > >( + observationModel ); + std::vector< std::shared_ptr< + observation_models::LightTimeCalculator< ObservationScalarType, TimeType > > > + lightTimeCalculatorList = nWayRangeObservationModel->getLightTimeCalculator( ) + ->getLightTimeCalculators( ); + for( unsigned int i = 0; i < lightTimeCalculatorList.size( ); i++ ) + { + currentLightTimeCorrections.push_back( + lightTimeCalculatorList.at( i )->getLightTimeCorrection( ) ); + } + break; + } + case observation_models::position_observable: { + break; + } + case observation_models::euler_angle_313_observable: { + break; + } + case observation_models::velocity_observable: { + break; + } + case observation_models::relative_angular_position: { + std::shared_ptr< observation_models::RelativeAngularPositionObservationModel< + ObservationScalarType, + TimeType > > + relativeAngularPositionModel = std::dynamic_pointer_cast< + observation_models::RelativeAngularPositionObservationModel< + ObservationScalarType, + TimeType > >( observationModel ); + currentLightTimeCorrections.push_back( + relativeAngularPositionModel->getLightTimeCalculatorFirstTransmitter( ) + ->getLightTimeCorrection( ) ); + currentLightTimeCorrections.push_back( + relativeAngularPositionModel->getLightTimeCalculatorSecondTransmitter( ) + ->getLightTimeCorrection( ) ); + break; + } + case observation_models::relative_position_observable: { + break; + } + default: + std::string errorMessage = + "Error in light time correction list creation, observable type " + + std::to_string( observationModel->getObservableType( ) ) + " not recognized."; + throw std::runtime_error( errorMessage ); } if( singleObservableCorrectionList.size( ) > 0 ) @@ -2937,40 +3513,50 @@ std::vector< std::vector< std::shared_ptr< observation_models::LightTimeCorrecti } return currentLightTimeCorrections; - } //! Function to retrieve a list of light-time corrections per link end from a list of observation models. /*! - * Function to retrieve a list of light-time corrections per link end from a list of observation models. - * \param observationModels Map of observation models (may not be of mixed type) with LinkEnds of observable as map key - * \return Map of light-time corrections, with associated link ends as key. + * Function to retrieve a list of light-time corrections per link end from a list of observation + * models. \param observationModels Map of observation models (may not be of mixed type) with + * LinkEnds of observable as map key \return Map of light-time corrections, with associated link + * ends as key. */ -template< typename ObservationScalarType, typename TimeType, int ObservationSize > +template< typename ObservationScalarType, typename TimeType, int ObservationSize > PerLinkEndPerLightTimeSolutionCorrections getLightTimeCorrectionsList( - const std::map< observation_models::LinkEnds, std::shared_ptr< observation_models::ObservationModel< - ObservationSize, ObservationScalarType, TimeType> > > observationModels ) + const std::map< + observation_models::LinkEnds, + std::shared_ptr< observation_models::ObservationModel< ObservationSize, + ObservationScalarType, + TimeType > > > + observationModels ) { PerLinkEndPerLightTimeSolutionCorrections lightTimeCorrectionsList; // Retrieve type of observable - observation_models::ObservableType observableType = observationModels.begin( )->second->getObservableType( ); + observation_models::ObservableType observableType = + observationModels.begin( )->second->getObservableType( ); // Iterate over link ends - for( typename std::map< observation_models::LinkEnds, std::shared_ptr< observation_models::ObservationModel< - ObservationSize, ObservationScalarType, TimeType> > >::const_iterator - observationModelIterator = observationModels.begin( ); - observationModelIterator != observationModels.end( ); observationModelIterator++ ) + for( typename std::map< observation_models::LinkEnds, + std::shared_ptr< observation_models::ObservationModel< + ObservationSize, + ObservationScalarType, + TimeType > > >::const_iterator observationModelIterator = + observationModels.begin( ); + observationModelIterator != observationModels.end( ); + observationModelIterator++ ) { - std::vector< std::vector< std::shared_ptr< observation_models::LightTimeCorrection > > > currentLightTimeCorrections = - getLightTimeCorrections( observationModelIterator->second ); + std::vector< std::vector< std::shared_ptr< observation_models::LightTimeCorrection > > > + currentLightTimeCorrections = + getLightTimeCorrections( observationModelIterator->second ); // Add light-time corrections for current link ends. if( currentLightTimeCorrections.size( ) > 0 ) { - lightTimeCorrectionsList[ observationModelIterator->first ] = currentLightTimeCorrections; + lightTimeCorrectionsList[ observationModelIterator->first ] = + currentLightTimeCorrections; } - } return lightTimeCorrectionsList; } @@ -2978,120 +3564,167 @@ PerLinkEndPerLightTimeSolutionCorrections getLightTimeCorrectionsList( template< int ObservationSize > class UndifferencedObservationModelExtractor { -public: + public: template< typename ObservationScalarType, typename TimeType > - static std::pair< std::shared_ptr< observation_models::ObservationModel< ObservationSize, ObservationScalarType, TimeType > >, - std::shared_ptr< observation_models::ObservationModel< ObservationSize, ObservationScalarType, TimeType > > > - extract( const std::shared_ptr< observation_models::ObservationModel< ObservationSize, ObservationScalarType, TimeType > > differencedObservationModel ); + static std::pair< std::shared_ptr< observation_models::ObservationModel< ObservationSize, + ObservationScalarType, + TimeType > >, + std::shared_ptr< observation_models::ObservationModel< ObservationSize, + ObservationScalarType, + TimeType > > > + extract( const std::shared_ptr< observation_models::ObservationModel< ObservationSize, + ObservationScalarType, + TimeType > > + differencedObservationModel ); }; -template< > +template<> class UndifferencedObservationModelExtractor< 1 > { -public: + public: template< typename ObservationScalarType, typename TimeType > - static std::pair< std::shared_ptr< observation_models::ObservationModel< 1, ObservationScalarType, TimeType > >, - std::shared_ptr< observation_models::ObservationModel< 1, ObservationScalarType, TimeType > > > - extract( const std::shared_ptr< observation_models::ObservationModel< 1, ObservationScalarType, TimeType > > differencedObservationModel ) + static std::pair< + std::shared_ptr< + observation_models::ObservationModel< 1, ObservationScalarType, TimeType > >, + std::shared_ptr< + observation_models::ObservationModel< 1, ObservationScalarType, TimeType > > > + extract( const std::shared_ptr< + observation_models::ObservationModel< 1, ObservationScalarType, TimeType > > + differencedObservationModel ) { - std::shared_ptr< observation_models::ObservationModel< 1, ObservationScalarType, TimeType > > firstObservationModel; - std::shared_ptr< observation_models::ObservationModel< 1, ObservationScalarType, TimeType > > secondObservationModel; + std::shared_ptr< + observation_models::ObservationModel< 1, ObservationScalarType, TimeType > > + firstObservationModel; + std::shared_ptr< + observation_models::ObservationModel< 1, ObservationScalarType, TimeType > > + secondObservationModel; // Check type of observable switch( differencedObservationModel->getObservableType( ) ) { - case observation_models::one_way_differenced_range: - { - std::shared_ptr< observation_models::OneWayDifferencedRangeObservationModel - < ObservationScalarType, TimeType> > oneWayDifferencedRangeObservationModel = - std::dynamic_pointer_cast< observation_models::OneWayDifferencedRangeObservationModel - < ObservationScalarType, TimeType > >( differencedObservationModel ); - firstObservationModel = std::make_shared< observation_models::OneWayRangeObservationModel< ObservationScalarType, TimeType > >( + case observation_models::one_way_differenced_range: { + std::shared_ptr< observation_models::OneWayDifferencedRangeObservationModel< + ObservationScalarType, + TimeType > > + oneWayDifferencedRangeObservationModel = std::dynamic_pointer_cast< + observation_models::OneWayDifferencedRangeObservationModel< + ObservationScalarType, + TimeType > >( differencedObservationModel ); + firstObservationModel = std::make_shared< + observation_models::OneWayRangeObservationModel< ObservationScalarType, + TimeType > >( oneWayDifferencedRangeObservationModel->getLinkEnds( ), oneWayDifferencedRangeObservationModel->getArcStartLightTimeCalculator( ) ); - secondObservationModel = std::make_shared< observation_models::OneWayRangeObservationModel< ObservationScalarType, TimeType > >( + secondObservationModel = std::make_shared< + observation_models::OneWayRangeObservationModel< ObservationScalarType, + TimeType > >( oneWayDifferencedRangeObservationModel->getLinkEnds( ), oneWayDifferencedRangeObservationModel->getArcEndLightTimeCalculator( ) ); - break; - } - case observation_models::n_way_differenced_range: - { - std::shared_ptr< observation_models::NWayDifferencedRangeObservationModel - < ObservationScalarType, TimeType> > nWayDifferencedRangeObservationModel = - std::dynamic_pointer_cast< observation_models::NWayDifferencedRangeObservationModel - < ObservationScalarType, TimeType > >( differencedObservationModel ); - firstObservationModel = nWayDifferencedRangeObservationModel->getArcStartObservationModel( ); - secondObservationModel = nWayDifferencedRangeObservationModel->getArcEndObservationModel( ); - break; - } - case observation_models::dsn_n_way_averaged_doppler: - { - std::shared_ptr< observation_models::DsnNWayAveragedDopplerObservationModel< ObservationScalarType, TimeType > > - dsnNWayAveragedDopplerObservationModel = - std::dynamic_pointer_cast< observation_models::DsnNWayAveragedDopplerObservationModel< ObservationScalarType, TimeType > >( - differencedObservationModel ); - firstObservationModel = dsnNWayAveragedDopplerObservationModel->getArcStartObservationModel( ); - secondObservationModel = dsnNWayAveragedDopplerObservationModel->getArcEndObservationModel( ); - break; - } - case observation_models::doppler_measured_frequency: - { - throw std::runtime_error( "Error when extract undifferenced observation model. Doppler Measured Frequency model not implemented." ); - } - default: - std::string errorMessage = - "Error when getting size 1 undifferenced observation models: observable type " + - std::to_string( differencedObservationModel->getObservableType( ) ) + " not recognized."; - throw std::runtime_error( errorMessage ); + break; + } + case observation_models::n_way_differenced_range: { + std::shared_ptr< observation_models::NWayDifferencedRangeObservationModel< + ObservationScalarType, + TimeType > > + nWayDifferencedRangeObservationModel = std::dynamic_pointer_cast< + observation_models::NWayDifferencedRangeObservationModel< + ObservationScalarType, + TimeType > >( differencedObservationModel ); + firstObservationModel = + nWayDifferencedRangeObservationModel->getArcStartObservationModel( ); + secondObservationModel = + nWayDifferencedRangeObservationModel->getArcEndObservationModel( ); + break; + } + case observation_models::dsn_n_way_averaged_doppler: { + std::shared_ptr< observation_models::DsnNWayAveragedDopplerObservationModel< + ObservationScalarType, + TimeType > > + dsnNWayAveragedDopplerObservationModel = std::dynamic_pointer_cast< + observation_models::DsnNWayAveragedDopplerObservationModel< + ObservationScalarType, + TimeType > >( differencedObservationModel ); + firstObservationModel = + dsnNWayAveragedDopplerObservationModel->getArcStartObservationModel( ); + secondObservationModel = + dsnNWayAveragedDopplerObservationModel->getArcEndObservationModel( ); + break; + } + case observation_models::doppler_measured_frequency: { + throw std::runtime_error( + "Error when extract undifferenced observation model. Doppler Measured " + "Frequency model not implemented." ); + } + default: + std::string errorMessage = + "Error when getting size 1 undifferenced observation models: observable " + "type " + + std::to_string( differencedObservationModel->getObservableType( ) ) + + " not recognized."; + throw std::runtime_error( errorMessage ); } return std::make_pair( firstObservationModel, secondObservationModel ); } }; -template< > +template<> class UndifferencedObservationModelExtractor< 2 > { -public: + public: template< typename ObservationScalarType, typename TimeType > - static std::pair< std::shared_ptr< observation_models::ObservationModel< 2, ObservationScalarType, TimeType > >, - std::shared_ptr< observation_models::ObservationModel< 2, ObservationScalarType, TimeType > > > - extract( const std::shared_ptr< observation_models::ObservationModel< 2, ObservationScalarType, TimeType > > differencedObservationModel ) + static std::pair< + std::shared_ptr< + observation_models::ObservationModel< 2, ObservationScalarType, TimeType > >, + std::shared_ptr< + observation_models::ObservationModel< 2, ObservationScalarType, TimeType > > > + extract( const std::shared_ptr< + observation_models::ObservationModel< 2, ObservationScalarType, TimeType > > + differencedObservationModel ) { - std::shared_ptr< observation_models::ObservationModel< 2, ObservationScalarType, TimeType > > firstObservationModel; - std::shared_ptr< observation_models::ObservationModel< 2, ObservationScalarType, TimeType > > secondObservationModel; + std::shared_ptr< + observation_models::ObservationModel< 2, ObservationScalarType, TimeType > > + firstObservationModel; + std::shared_ptr< + observation_models::ObservationModel< 2, ObservationScalarType, TimeType > > + secondObservationModel; // Check type of observable switch( differencedObservationModel->getObservableType( ) ) { - case observation_models::relative_angular_position: - { - std::shared_ptr< observation_models::RelativeAngularPositionObservationModel - < ObservationScalarType, TimeType> > relativeAngularPositionModel = - std::dynamic_pointer_cast< observation_models::RelativeAngularPositionObservationModel - < ObservationScalarType, TimeType > >( differencedObservationModel ); - firstObservationModel = std::make_shared< observation_models::AngularPositionObservationModel< ObservationScalarType, TimeType > >( + case observation_models::relative_angular_position: { + std::shared_ptr< observation_models::RelativeAngularPositionObservationModel< + ObservationScalarType, + TimeType > > + relativeAngularPositionModel = std::dynamic_pointer_cast< + observation_models::RelativeAngularPositionObservationModel< + ObservationScalarType, + TimeType > >( differencedObservationModel ); + firstObservationModel = std::make_shared< + observation_models::AngularPositionObservationModel< ObservationScalarType, + TimeType > >( relativeAngularPositionModel->getFirstLinkEnds( ), relativeAngularPositionModel->getLightTimeCalculatorFirstTransmitter( ) ); - secondObservationModel = std::make_shared< observation_models::AngularPositionObservationModel< ObservationScalarType, TimeType > >( + secondObservationModel = std::make_shared< + observation_models::AngularPositionObservationModel< ObservationScalarType, + TimeType > >( relativeAngularPositionModel->getSecondLinkEnds( ), - relativeAngularPositionModel->getLightTimeCalculatorSecondTransmitter() ); - break; - } - default: - std::string errorMessage = - "Error when getting size 2 undifferenced observation models: observable type " + - std::to_string( differencedObservationModel->getObservableType( ) ) + " not recognized."; - throw std::runtime_error( errorMessage ); + relativeAngularPositionModel->getLightTimeCalculatorSecondTransmitter( ) ); + break; + } + default: + std::string errorMessage = + "Error when getting size 2 undifferenced observation models: observable " + "type " + + std::to_string( differencedObservationModel->getObservableType( ) ) + + " not recognized."; + throw std::runtime_error( errorMessage ); } return std::make_pair( firstObservationModel, secondObservationModel ); } }; +} // namespace observation_models +} // namespace tudat - -} // namespace observation_models - -} // namespace tudat - -#endif // TUDAT_CREATEOBSERVATIONMODEL_H +#endif // TUDAT_CREATEOBSERVATIONMODEL_H diff --git a/include/tudat/simulation/estimation_setup/createObservationPartials.h b/include/tudat/simulation/estimation_setup/createObservationPartials.h index bd98956a4e..fcc814b26a 100644 --- a/include/tudat/simulation/estimation_setup/createObservationPartials.h +++ b/include/tudat/simulation/estimation_setup/createObservationPartials.h @@ -13,13 +13,12 @@ #include - +#include "tudat/simulation/estimation_setup/createDirectObservationPartials.h" #include "tudat/simulation/estimation_setup/createDopplerPartials.h" -#include "tudat/simulation/estimation_setup/createNWayRangePartials.h" #include "tudat/simulation/estimation_setup/createEulerAngleObservationPartials.h" -#include "tudat/simulation/estimation_setup/createDirectObservationPartials.h" +#include "tudat/simulation/estimation_setup/createNWayRangePartials.h" +#include "tudat/simulation/estimation_setup/createObservationBiasPartial.h" #include "tudat/simulation/propagation_setup/dependentVariablesInterface.h" -#include "tudat/simulation/estimation_setup/createObsevationBiasPartial.h" namespace tudat { @@ -27,172 +26,234 @@ namespace tudat namespace observation_partials { - - //! Function to split observation partials and scaling object (produced by observationPartialsAndScaler function) into separate //! containers /*! - * Function to split observation partials and scaling object (produced by observationPartialsAndScaler function) into separate - * containers - * \param observationPartialsAndScalers Combined list of observation partials and scaling objects - * \param observationPartials List of observation partials, per link ends, and per parameter indices (returned by reference) - * \param observationPartialScalers List of position partial scaling objects, per link ends (returned by reference) + * Function to split observation partials and scaling object (produced by + * observationPartialsAndScaler function) into separate containers \param + * observationPartialsAndScalers Combined list of observation partials and scaling objects \param + * observationPartials List of observation partials, per link ends, and per parameter indices + * (returned by reference) \param observationPartialScalers List of position partial scaling + * objects, per link ends (returned by reference) */ template< int ObservationSize > void splitObservationPartialsAndScalers( - const std::map< observation_models::LinkEnds, - std::pair< std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< ObservationSize > > >, - std::shared_ptr< PositionPartialScaling > > >& observationPartialsAndScalers, - std::map< observation_models::LinkEnds, std::map< std::pair< int, int >, - std::shared_ptr< observation_partials::ObservationPartial< ObservationSize > > > >& observationPartials, - std::map< observation_models::LinkEnds, std::shared_ptr< observation_partials::PositionPartialScaling > >& - observationPartialScalers ) + const std::map< + observation_models::LinkEnds, + std::pair< std::map< std::pair< int, int >, + std::shared_ptr< ObservationPartial< ObservationSize > > >, + std::shared_ptr< PositionPartialScaling > > >& + observationPartialsAndScalers, + std::map< observation_models::LinkEnds, + std::map< std::pair< int, int >, + std::shared_ptr< observation_partials::ObservationPartial< + ObservationSize > > > >& observationPartials, + std::map< observation_models::LinkEnds, + std::shared_ptr< observation_partials::PositionPartialScaling > >& + observationPartialScalers ) { observationPartials.clear( ); observationPartialScalers.clear( ); // Put one-way range partials and scalers in member variables. - for( typename std::map< observation_models::LinkEnds, std::pair< std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< ObservationSize > > >, - std::shared_ptr< PositionPartialScaling > > >::const_iterator - rangePartialPairIterator = observationPartialsAndScalers.begin( ); rangePartialPairIterator != observationPartialsAndScalers.end( ); + for( typename std::map< + observation_models::LinkEnds, + std::pair< std::map< std::pair< int, int >, + std::shared_ptr< ObservationPartial< ObservationSize > > >, + std::shared_ptr< PositionPartialScaling > > >::const_iterator + rangePartialPairIterator = observationPartialsAndScalers.begin( ); + rangePartialPairIterator != observationPartialsAndScalers.end( ); rangePartialPairIterator++ ) { - observationPartials[ rangePartialPairIterator->first ] = rangePartialPairIterator->second.first; - observationPartialScalers[ rangePartialPairIterator->first ] = rangePartialPairIterator->second.second; + observationPartials[ rangePartialPairIterator->first ] = + rangePartialPairIterator->second.first; + observationPartialScalers[ rangePartialPairIterator->first ] = + rangePartialPairIterator->second.second; } } - template< typename ParameterType, typename TimeType, int ObservationSize > -std::pair< std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< ObservationSize > > > , -std::shared_ptr< PositionPartialScaling > > createDifferencedObservablePartials( - const std::shared_ptr< observation_models::ObservationModel< ObservationSize, ParameterType, TimeType > > observationModel, +std::pair< + std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< ObservationSize > > >, + std::shared_ptr< PositionPartialScaling > > +createDifferencedObservablePartials( + const std::shared_ptr< + observation_models::ObservationModel< ObservationSize, ParameterType, TimeType > > + observationModel, const simulation_setup::SystemOfBodies& bodies, - const std::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > parametersToEstimate, + const std::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > + parametersToEstimate, const bool isPartialForDifferencedObservable = false, const bool isPartialForConcatenatedObservable = false ); //! Interface class for creating observation partials /*! - * Interface class for creating observation partials. This class is used instead of a single templated free function to - * allow ObservationPartial derived classed with different ObservationSize template arguments to be created using the same - * interface. This class has template specializations for each value of ObservationSize, and contains a single + * Interface class for creating observation partials. This class is used instead of a single + * templated free function to allow ObservationPartial derived classed with different + * ObservationSize template arguments to be created using the same interface. This class has + * template specializations for each value of ObservationSize, and contains a single * createObservationModel function that performs the required operation. */ template< int ObservationSize, typename ObservationScalarType, typename TimeType > class ObservationPartialCreator { -public: - + public: //! Function to create a list of observation partial objects, and associated scaling objects /*! * Function to create a list of observation partial objects, and associated scaling objects * \param observableType Type of observable for which partials are to be created - * \param observationModelList List of observation models, with the link ends of map key, for which partials are to be created - * \param bodies Map of body objects that comprises the environment - * \param parametersToEstimate Parameters for which partial derivatives are to be computed - * \return Map with list of observation partials. Key is associated link ends. Value is a list of observation partial - * objects, one for each parameter w.r.t. which the observation partial is non-zero (in general). The format is a pair - * with: - * first: map with pair (parameter start entry in parameter vector and parameter size) and associated observation partial - * object as value of map - * second: PositionPartialScaling object associated with all partials of single LinkEnds. + * \param observationModelList List of observation models, with the link ends of map key, for + * which partials are to be created \param bodies Map of body objects that comprises the + * environment \param parametersToEstimate Parameters for which partial derivatives are to be + * computed \return Map with list of observation partials. Key is associated link ends. Value is + * a list of observation partial objects, one for each parameter w.r.t. which the observation + * partial is non-zero (in general). The format is a pair with: first: map with pair (parameter + * start entry in parameter vector and parameter size) and associated observation partial object + * as value of map second: PositionPartialScaling object associated with all partials of single + * LinkEnds. */ static std::pair< std::map< std::pair< int, int >, - std::shared_ptr< ObservationPartial< ObservationSize > > >, std::shared_ptr< PositionPartialScaling > > createObservationPartials( - const std::shared_ptr< observation_models::ObservationModel< ObservationSize, ObservationScalarType, TimeType > > observationModel, + std::shared_ptr< ObservationPartial< ObservationSize > > >, + std::shared_ptr< PositionPartialScaling > > + createObservationPartials( + const std::shared_ptr< observation_models::ObservationModel< ObservationSize, + ObservationScalarType, + TimeType > > + observationModel, const simulation_setup::SystemOfBodies& bodies, - const std::shared_ptr< estimatable_parameters::EstimatableParameterSet< ObservationScalarType > > parametersToEstimate, + const std::shared_ptr< + estimatable_parameters::EstimatableParameterSet< ObservationScalarType > > + parametersToEstimate, const bool isPartialForDifferencedObservable = false, const bool isPartialForConcatenatedObservable = false, - const std::shared_ptr< propagators::DependentVariablesInterface< TimeType > > dependentVariablesInterface = - std::shared_ptr< propagators::DependentVariablesInterface< TimeType > >( ) ); + const std::shared_ptr< propagators::DependentVariablesInterface< TimeType > > + dependentVariablesInterface = std::shared_ptr< + propagators::DependentVariablesInterface< TimeType > >( ) ); }; //! Interface class for creating observation partials for observables of size 1. template< typename ObservationScalarType, typename TimeType > class ObservationPartialCreator< 1, ObservationScalarType, TimeType > { -public: - + public: //! Function to create a list of observation partial objects, and associated scaling objects /*! * Function to create a list of observation partial objects, and associated scaling objects * \param observableType Type of observable for which partials are to be created - * \param observationModelList List of observation models, with the link ends of map key, for which partials are to be created - * \param bodies Map of body objects that comprises the environment - * \param parametersToEstimate Parameters for which partial derivatives are to be computed - * \return Map with list of observation partials. Key is associated link ends. Value is a list of observation partial - * objects, one for each parameter w.r.t. which the observation partial is non-zero (in general). The format is a pair - * with: - * first: map with pair (parameter start entry in parameter vector and parameter size) and associated observation partial - * object as value of map - * second: PositionPartialScaling object associated with all partials of single LinkEnds. + * \param observationModelList List of observation models, with the link ends of map key, for + * which partials are to be created \param bodies Map of body objects that comprises the + * environment \param parametersToEstimate Parameters for which partial derivatives are to be + * computed \return Map with list of observation partials. Key is associated link ends. Value is + * a list of observation partial objects, one for each parameter w.r.t. which the observation + * partial is non-zero (in general). The format is a pair with: first: map with pair (parameter + * start entry in parameter vector and parameter size) and associated observation partial object + * as value of map second: PositionPartialScaling object associated with all partials of single + * LinkEnds. */ - static std::pair< std::map< std::pair< int, int >, - std::shared_ptr< ObservationPartial< 1 > > >, std::shared_ptr< PositionPartialScaling > > createObservationPartials( - const std::shared_ptr< observation_models::ObservationModel< 1, ObservationScalarType, TimeType > > observationModel, + static std::pair< std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< 1 > > >, + std::shared_ptr< PositionPartialScaling > > + createObservationPartials( + const std::shared_ptr< + observation_models::ObservationModel< 1, ObservationScalarType, TimeType > > + observationModel, const simulation_setup::SystemOfBodies& bodies, - const std::shared_ptr< estimatable_parameters::EstimatableParameterSet< ObservationScalarType > > parametersToEstimate, + const std::shared_ptr< + estimatable_parameters::EstimatableParameterSet< ObservationScalarType > > + parametersToEstimate, const bool isPartialForDifferencedObservable = false, const bool isPartialForConcatenatedObservable = false, - const std::shared_ptr< propagators::DependentVariablesInterface< TimeType > > dependentVariablesInterface = - std::shared_ptr< propagators::DependentVariablesInterface< TimeType > >( ) ) + const std::shared_ptr< propagators::DependentVariablesInterface< TimeType > > + dependentVariablesInterface = std::shared_ptr< + propagators::DependentVariablesInterface< TimeType > >( ) ) { - std::pair< std::map< std::pair< int, int >, - std::shared_ptr< ObservationPartial< 1 > > >, std::shared_ptr< PositionPartialScaling > > observationPartials; + std::pair< std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< 1 > > >, + std::shared_ptr< PositionPartialScaling > > + observationPartials; switch( observationModel->getObservableType( ) ) { - case observation_models::one_way_range: - observationPartials = createSingleLinkObservationPartials< ObservationScalarType, 1, TimeType >( - observationModel, bodies, parametersToEstimate, isPartialForDifferencedObservable, isPartialForConcatenatedObservable ); - break; - case observation_models::one_way_doppler: - observationPartials = createSingleLinkObservationPartials< ObservationScalarType, 1, TimeType >( - observationModel, bodies, parametersToEstimate, isPartialForDifferencedObservable, isPartialForConcatenatedObservable ); - break; - case observation_models::two_way_doppler: - if( isPartialForConcatenatedObservable ) - { - throw std::runtime_error( "Error when requesting partial creation for 2-way Doppler; concatenated partial not supported" ); - } - observationPartials = createTwoWayDopplerPartials< ObservationScalarType, TimeType >( - observationModel, bodies, parametersToEstimate, isPartialForDifferencedObservable ); - break; - case observation_models::n_way_range: - if( isPartialForConcatenatedObservable ) - { - throw std::runtime_error( "Error when requesting partial creation for n-way range; concatenated partial not supported" ); - } - observationPartials = createNWayRangePartials< ObservationScalarType >( - observationModel, bodies, parametersToEstimate, isPartialForDifferencedObservable ); - break; - case observation_models::one_way_differenced_range: - if( isPartialForConcatenatedObservable ) - { - throw std::runtime_error( "Error when requesting partial creation for 1-way averaged Doppler; concatenated partial not supported" ); - } - observationPartials = createDifferencedObservablePartials< ObservationScalarType, TimeType, 1 >( - observationModel, bodies, parametersToEstimate, isPartialForDifferencedObservable ); - break; - case observation_models::n_way_differenced_range: - case observation_models::dsn_n_way_averaged_doppler: - case observation_models::doppler_measured_frequency: - if( isPartialForDifferencedObservable ) - { - throw std::runtime_error( "Error when requesting partial creation for n-way averaged Doppler; differenced partial not supported" ); - } - if( isPartialForConcatenatedObservable ) - { - throw std::runtime_error( "Error when requesting partial creation for n-way averaged Doppler; concatenated partial not supported" ); - } - observationPartials = createDifferencedObservablePartials< ObservationScalarType, TimeType, 1 >( - observationModel, bodies, parametersToEstimate ); - break; - default: - std::string errorMessage = - "Error when making observation partial set, could not recognize observable " + - std::to_string( observationModel->getObservableType( ) ) + " of size 1 "; - throw std::runtime_error( errorMessage ); + case observation_models::one_way_range: + observationPartials = + createSingleLinkObservationPartials< ObservationScalarType, 1, TimeType >( + observationModel, + bodies, + parametersToEstimate, + isPartialForDifferencedObservable, + isPartialForConcatenatedObservable ); + break; + case observation_models::one_way_doppler: + observationPartials = + createSingleLinkObservationPartials< ObservationScalarType, 1, TimeType >( + observationModel, + bodies, + parametersToEstimate, + isPartialForDifferencedObservable, + isPartialForConcatenatedObservable ); + break; + case observation_models::two_way_doppler: + if( isPartialForConcatenatedObservable ) + { + throw std::runtime_error( + "Error when requesting partial creation for 2-way Doppler; " + "concatenated partial not supported" ); + } + observationPartials = + createTwoWayDopplerPartials< ObservationScalarType, TimeType >( + observationModel, + bodies, + parametersToEstimate, + isPartialForDifferencedObservable ); + break; + case observation_models::n_way_range: + if( isPartialForConcatenatedObservable ) + { + throw std::runtime_error( + "Error when requesting partial creation for n-way range; concatenated " + "partial not supported" ); + } + observationPartials = createNWayRangePartials< ObservationScalarType >( + observationModel, + bodies, + parametersToEstimate, + isPartialForDifferencedObservable ); + break; + case observation_models::one_way_differenced_range: + if( isPartialForConcatenatedObservable ) + { + throw std::runtime_error( + "Error when requesting partial creation for 1-way averaged Doppler; " + "concatenated partial not supported" ); + } + observationPartials = + createDifferencedObservablePartials< ObservationScalarType, TimeType, 1 >( + observationModel, + bodies, + parametersToEstimate, + isPartialForDifferencedObservable ); + break; + case observation_models::n_way_differenced_range: + case observation_models::dsn_n_way_averaged_doppler: + case observation_models::doppler_measured_frequency: + if( isPartialForDifferencedObservable ) + { + throw std::runtime_error( + "Error when requesting partial creation for n-way averaged Doppler; " + "differenced partial not supported" ); + } + if( isPartialForConcatenatedObservable ) + { + throw std::runtime_error( + "Error when requesting partial creation for n-way averaged Doppler; " + "concatenated partial not supported" ); + } + observationPartials = + createDifferencedObservablePartials< ObservationScalarType, TimeType, 1 >( + observationModel, bodies, parametersToEstimate ); + break; + default: + std::string errorMessage = + "Error when making observation partial set, could not recognize " + "observable " + + std::to_string( observationModel->getObservableType( ) ) + " of size 1 "; + throw std::runtime_error( errorMessage ); } return observationPartials; @@ -203,138 +264,190 @@ class ObservationPartialCreator< 1, ObservationScalarType, TimeType > template< typename ObservationScalarType, typename TimeType > class ObservationPartialCreator< 2, ObservationScalarType, TimeType > { -public: - + public: //! Function to create a list of observation partial objects, and associated scaling objects /*! * Function to create a list of observation partial objects, and associated scaling objects * \param observableType Type of observable for which partials are to be created - * \param observationModelList List of observation models, with the link ends of map key, for which partials are to be created - * \param bodies Map of body objects that comprises the environment - * \param parametersToEstimate Parameters for which partial derivatives are to be computed - * \return Map with list of observation partials. Key is associated link ends. Value is a list of observation partial - * objects, one for each parameter w.r.t. which the observation partial is non-zero (in general). The format is a pair - * with: - * first: map with pair (parameter start entry in parameter vector and parameter size) and associated observation partial - * object as value of map - * second: PositionPartialScaling object associated with all partials of single LinkEnds. + * \param observationModelList List of observation models, with the link ends of map key, for + * which partials are to be created \param bodies Map of body objects that comprises the + * environment \param parametersToEstimate Parameters for which partial derivatives are to be + * computed \return Map with list of observation partials. Key is associated link ends. Value is + * a list of observation partial objects, one for each parameter w.r.t. which the observation + * partial is non-zero (in general). The format is a pair with: first: map with pair (parameter + * start entry in parameter vector and parameter size) and associated observation partial object + * as value of map second: PositionPartialScaling object associated with all partials of single + * LinkEnds. */ - static std::pair< std::map< std::pair< int, int >, - std::shared_ptr< ObservationPartial< 2 > > >, std::shared_ptr< PositionPartialScaling > > createObservationPartials( - const std::shared_ptr< observation_models::ObservationModel< 2, ObservationScalarType, TimeType > > observationModel, + static std::pair< std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< 2 > > >, + std::shared_ptr< PositionPartialScaling > > + createObservationPartials( + const std::shared_ptr< + observation_models::ObservationModel< 2, ObservationScalarType, TimeType > > + observationModel, const simulation_setup::SystemOfBodies& bodies, - const std::shared_ptr< estimatable_parameters::EstimatableParameterSet< ObservationScalarType > > parametersToEstimate, + const std::shared_ptr< + estimatable_parameters::EstimatableParameterSet< ObservationScalarType > > + parametersToEstimate, const bool isPartialForDifferencedObservable = false, const bool isPartialForConcatenatedObservable = false, - const std::shared_ptr< propagators::DependentVariablesInterface< TimeType > > dependentVariablesInterface = - std::shared_ptr< propagators::DependentVariablesInterface< TimeType > > ( ) ) + const std::shared_ptr< propagators::DependentVariablesInterface< TimeType > > + dependentVariablesInterface = std::shared_ptr< + propagators::DependentVariablesInterface< TimeType > >( ) ) { - std::pair< std::map< std::pair< int, int >, - std::shared_ptr< ObservationPartial< 2 > > >, std::shared_ptr< PositionPartialScaling > > observationPartials; + std::pair< std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< 2 > > >, + std::shared_ptr< PositionPartialScaling > > + observationPartials; switch( observationModel->getObservableType( ) ) { - case observation_models::angular_position: - observationPartials = createSingleLinkObservationPartials< ObservationScalarType, 2, TimeType >( - observationModel, bodies, parametersToEstimate, isPartialForDifferencedObservable, isPartialForConcatenatedObservable ); - break; - case observation_models::relative_angular_position: - observationPartials = createDifferencedObservablePartials< ObservationScalarType, TimeType, 2 >( - observationModel, bodies, parametersToEstimate, isPartialForDifferencedObservable, isPartialForConcatenatedObservable ); - break; - default: - std::string errorMessage = - "Error when making observation partial set, could not recognize observable " + - std::to_string( observationModel->getObservableType( ) ) + " of size 2 "; - throw std::runtime_error( errorMessage ); + case observation_models::angular_position: + observationPartials = + createSingleLinkObservationPartials< ObservationScalarType, 2, TimeType >( + observationModel, + bodies, + parametersToEstimate, + isPartialForDifferencedObservable, + isPartialForConcatenatedObservable ); + break; + case observation_models::relative_angular_position: + observationPartials = + createDifferencedObservablePartials< ObservationScalarType, TimeType, 2 >( + observationModel, + bodies, + parametersToEstimate, + isPartialForDifferencedObservable, + isPartialForConcatenatedObservable ); + break; + default: + std::string errorMessage = + "Error when making observation partial set, could not recognize " + "observable " + + std::to_string( observationModel->getObservableType( ) ) + " of size 2 "; + throw std::runtime_error( errorMessage ); } return observationPartials; } - }; //! Interface class for creating observation partials for observables of size 3. template< typename ObservationScalarType, typename TimeType > class ObservationPartialCreator< 3, ObservationScalarType, TimeType > { -public: - + public: //! Function to create a list of observation partial objects, and associated scaling objects /*! * Function to create a list of observation partial objects, and associated scaling objects * \param observableType Type of observable for which partials are to be created - * \param observationModelList List of observation models, with the link ends of map key, for which partials are to be created - * \param bodies Map of body objects that comprises the environment - * \param parametersToEstimate Parameters for which partial derivatives are to be computed - * \return Map with list of observation partials. Key is associated link ends. Value is a list of observation partial - * objects, one for each parameter w.r.t. which the observation partial is non-zero (in general). The format is a pair - * with: - * first: map with pair (parameter start entry in parameter vector and parameter size) and associated observation partial - * object as value of map - * second: PositionPartialScaling object associated with all partials of single LinkEnds. + * \param observationModelList List of observation models, with the link ends of map key, for + * which partials are to be created \param bodies Map of body objects that comprises the + * environment \param parametersToEstimate Parameters for which partial derivatives are to be + * computed \return Map with list of observation partials. Key is associated link ends. Value is + * a list of observation partial objects, one for each parameter w.r.t. which the observation + * partial is non-zero (in general). The format is a pair with: first: map with pair (parameter + * start entry in parameter vector and parameter size) and associated observation partial object + * as value of map second: PositionPartialScaling object associated with all partials of single + * LinkEnds. */ - static std::pair< std::map< std::pair< int, int >, - std::shared_ptr< ObservationPartial< 3 > > >, std::shared_ptr< PositionPartialScaling > > createObservationPartials( - const std::shared_ptr< observation_models::ObservationModel< 3, ObservationScalarType, TimeType > > observationModel, + static std::pair< std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< 3 > > >, + std::shared_ptr< PositionPartialScaling > > + createObservationPartials( + const std::shared_ptr< + observation_models::ObservationModel< 3, ObservationScalarType, TimeType > > + observationModel, const simulation_setup::SystemOfBodies& bodies, - const std::shared_ptr< estimatable_parameters::EstimatableParameterSet< ObservationScalarType > > parametersToEstimate, + const std::shared_ptr< + estimatable_parameters::EstimatableParameterSet< ObservationScalarType > > + parametersToEstimate, const bool isPartialForDifferencedObservable = false, const bool isPartialForConcatenatedObservable = false, - const std::shared_ptr< propagators::DependentVariablesInterface< TimeType > > dependentVariablesInterface = - std::shared_ptr< propagators::DependentVariablesInterface< TimeType > >( ) ) + const std::shared_ptr< propagators::DependentVariablesInterface< TimeType > > + dependentVariablesInterface = std::shared_ptr< + propagators::DependentVariablesInterface< TimeType > >( ) ) { - std::pair< std::map< std::pair< int, int >, - std::shared_ptr< ObservationPartial< 3 > > >, std::shared_ptr< PositionPartialScaling > > observationPartials; + std::pair< std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< 3 > > >, + std::shared_ptr< PositionPartialScaling > > + observationPartials; switch( observationModel->getObservableType( ) ) { - case observation_models::position_observable: - observationPartials = createSingleLinkObservationPartials< ObservationScalarType, 3, TimeType >( - observationModel, bodies, parametersToEstimate, isPartialForDifferencedObservable, isPartialForConcatenatedObservable ); - break; - case observation_models::euler_angle_313_observable: - observationPartials = createEulerAngleObservablePartials< ObservationScalarType >( - observationModel->getLinkEnds( ), bodies, parametersToEstimate, isPartialForDifferencedObservable, isPartialForConcatenatedObservable ); - break; - case observation_models::velocity_observable: - observationPartials = createSingleLinkObservationPartials< ObservationScalarType, 3, TimeType >( - observationModel, bodies, parametersToEstimate, isPartialForDifferencedObservable, isPartialForConcatenatedObservable ); - break; - case observation_models::relative_position_observable: - observationPartials = createSingleLinkObservationPartials< ObservationScalarType, 3, TimeType >( - observationModel, bodies, parametersToEstimate, isPartialForDifferencedObservable, isPartialForConcatenatedObservable ); - break; - default: - std::string errorMessage = - "Error when making observation partial set, could not recognize observable " + - std::to_string( observationModel->getObservableType( ) ) + " of size 3 "; - throw std::runtime_error( errorMessage ); + case observation_models::position_observable: + observationPartials = + createSingleLinkObservationPartials< ObservationScalarType, 3, TimeType >( + observationModel, + bodies, + parametersToEstimate, + isPartialForDifferencedObservable, + isPartialForConcatenatedObservable ); + break; + case observation_models::euler_angle_313_observable: + observationPartials = createEulerAngleObservablePartials< ObservationScalarType >( + observationModel->getLinkEnds( ), + bodies, + parametersToEstimate, + isPartialForDifferencedObservable, + isPartialForConcatenatedObservable ); + break; + case observation_models::velocity_observable: + observationPartials = + createSingleLinkObservationPartials< ObservationScalarType, 3, TimeType >( + observationModel, + bodies, + parametersToEstimate, + isPartialForDifferencedObservable, + isPartialForConcatenatedObservable ); + break; + case observation_models::relative_position_observable: + observationPartials = + createSingleLinkObservationPartials< ObservationScalarType, 3, TimeType >( + observationModel, + bodies, + parametersToEstimate, + isPartialForDifferencedObservable, + isPartialForConcatenatedObservable ); + break; + default: + std::string errorMessage = + "Error when making observation partial set, could not recognize " + "observable " + + std::to_string( observationModel->getObservableType( ) ) + " of size 3 "; + throw std::runtime_error( errorMessage ); } return observationPartials; } - }; - template< typename ObservationScalarType, typename TimeType, int ObservationSize > std::map< observation_models::LinkEnds, -std::pair< std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< ObservationSize > > > , -std::shared_ptr< PositionPartialScaling > > > createObservablePartialsList( - const std::map< observation_models::LinkEnds, - std::shared_ptr< observation_models::ObservationModel< ObservationSize, ObservationScalarType, TimeType > > > observationModelList, + std::pair< std::map< std::pair< int, int >, + std::shared_ptr< ObservationPartial< ObservationSize > > >, + std::shared_ptr< PositionPartialScaling > > > +createObservablePartialsList( + const std::map< + observation_models::LinkEnds, + std::shared_ptr< observation_models::ObservationModel< ObservationSize, + ObservationScalarType, + TimeType > > > + observationModelList, const simulation_setup::SystemOfBodies& bodies, - const std::shared_ptr< estimatable_parameters::EstimatableParameterSet< ObservationScalarType > > parametersToEstimate, + const std::shared_ptr< + estimatable_parameters::EstimatableParameterSet< ObservationScalarType > > + parametersToEstimate, const bool isPartialForDifferencedObservable = false, const bool isPartialForConcatenatedObservable = false, - const std::shared_ptr< propagators::DependentVariablesInterface< TimeType > > dependentVariablesInterface = - std::shared_ptr< propagators::DependentVariablesInterface< TimeType > >( ) ) + const std::shared_ptr< propagators::DependentVariablesInterface< TimeType > > + dependentVariablesInterface = + std::shared_ptr< propagators::DependentVariablesInterface< TimeType > >( ) ) { std::map< observation_models::LinkEnds, - std::pair< std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< ObservationSize > > > , - std::shared_ptr< PositionPartialScaling > > > partialsList; - - observation_models::ObservableType observableType = observation_models::undefined_observation_model; - for( auto it : observationModelList ) + std::pair< std::map< std::pair< int, int >, + std::shared_ptr< ObservationPartial< ObservationSize > > >, + std::shared_ptr< PositionPartialScaling > > > + partialsList; + + observation_models::ObservableType observableType = + observation_models::undefined_observation_model; + for( auto it: observationModelList ) { if( observableType == observation_models::undefined_observation_model ) { @@ -342,21 +455,29 @@ std::shared_ptr< PositionPartialScaling > > > createObservablePartialsList( } else if( observableType != it.second->getObservableType( ) ) { - throw std::runtime_error( "Error when creating differenced observation partials, input models are inconsistent" ); + throw std::runtime_error( + "Error when creating differenced observation partials, input models are " + "inconsistent" ); } - partialsList[ it.first ] = ObservationPartialCreator< ObservationSize, ObservationScalarType, TimeType >::createObservationPartials( - it.second, bodies, parametersToEstimate, isPartialForDifferencedObservable, isPartialForConcatenatedObservable, dependentVariablesInterface ); + partialsList[ it.first ] = + ObservationPartialCreator< ObservationSize, ObservationScalarType, TimeType >:: + createObservationPartials( it.second, + bodies, + parametersToEstimate, + isPartialForDifferencedObservable, + isPartialForConcatenatedObservable, + dependentVariablesInterface ); } return partialsList; } - template< int ObservationSize > class DifferencedObservationPartialCreator { -public: - static std::shared_ptr< ObservationPartial< ObservationSize > > createDifferencedObservationPartial( + public: + static std::shared_ptr< ObservationPartial< ObservationSize > > + createDifferencedObservationPartial( const observation_models::ObservableType differencedObservableType, const std::shared_ptr< ObservationPartial< ObservationSize > > firstPartial, const std::shared_ptr< ObservationPartial< ObservationSize > > secondPartial, @@ -364,10 +485,10 @@ class DifferencedObservationPartialCreator const simulation_setup::SystemOfBodies& bodies ); }; -template< > +template<> class DifferencedObservationPartialCreator< 1 > { -public: + public: static std::shared_ptr< ObservationPartial< 1 > > createDifferencedObservationPartial( const observation_models::ObservableType differencedObservableType, const std::shared_ptr< ObservationPartial< 1 > > firstPartial, @@ -380,111 +501,152 @@ class DifferencedObservationPartialCreator< 1 > std::shared_ptr< ObservationPartial< 1 > > differencedPartial; switch( differencedObservableType ) { - case one_way_differenced_range: - { - if( !isParameterObservationLinkTimeProperty( getDifferencedPartialParameterIdentifier( firstPartial, secondPartial ).first ) ) - { - if( firstPartial != nullptr ) + case one_way_differenced_range: { + if( !isParameterObservationLinkTimeProperty( + getDifferencedPartialParameterIdentifier( firstPartial, secondPartial ) + .first ) ) { - if( std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( firstPartial ) == nullptr ) + if( firstPartial != nullptr ) { - throw std::runtime_error( "Error when creating one-way differenced range partial; first input object type is incompatible" ); + if( std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( + firstPartial ) == nullptr ) + { + throw std::runtime_error( + "Error when creating one-way differenced range partial; first " + "input object type is incompatible" ); + } + else if( std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( + firstPartial ) + ->getObservableType( ) != one_way_range ) + { + throw std::runtime_error( + "Error when creating one-way differenced range partial; first " + "input observable type is incompatible" ); + } } - else if( std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( firstPartial )->getObservableType( ) != one_way_range ) + + if( secondPartial != nullptr ) { - throw std::runtime_error( "Error when creating one-way differenced range partial; first input observable type is incompatible" ); + if( std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( + secondPartial ) == nullptr ) + { + throw std::runtime_error( + "Error when creating one-way differenced range partial; second " + "input object type is incompatible" ); + } + else if( std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( + secondPartial ) + ->getObservableType( ) != one_way_range ) + { + throw std::runtime_error( + "Error when creating one-way differenced range partial; second " + "input observable type is incompatible" ); + } } } - if( secondPartial != nullptr ) + differencedPartial = std::make_shared< DifferencedObservablePartial< 1 > >( + firstPartial, + secondPartial, + &observation_models::getDifferencedOneWayRangeScalingFactor, + getUndifferencedTimeAndStateIndices( one_way_differenced_range, + linkEnds.size( ) ) ); + break; + } + case n_way_differenced_range: { + if( firstPartial != nullptr ) { - if( std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( secondPartial ) == nullptr ) + if( std::dynamic_pointer_cast< NWayRangePartial >( firstPartial ) == nullptr ) { - throw std::runtime_error( "Error when creating one-way differenced range partial; second input object type is incompatible" ); - } - else if( std::dynamic_pointer_cast< DirectObservationPartial< 1 > >( secondPartial )->getObservableType( ) != one_way_range ) - { - throw std::runtime_error( "Error when creating one-way differenced range partial; second input observable type is incompatible" ); + throw std::runtime_error( + "Error when creating n-way differenced range partial; first input " + "object type is incompatible" ); } } - } - differencedPartial = std::make_shared< DifferencedObservablePartial< 1 > >( - firstPartial, secondPartial, &observation_models::getDifferencedOneWayRangeScalingFactor, - getUndifferencedTimeAndStateIndices( one_way_differenced_range, linkEnds.size( ) ) ); - break; - } - case n_way_differenced_range: - { - if( firstPartial != nullptr ) - { - if( std::dynamic_pointer_cast< NWayRangePartial >( firstPartial ) == nullptr ) + if( secondPartial != nullptr ) { - throw std::runtime_error( "Error when creating n-way differenced range partial; first input object type is incompatible" ); + if( std::dynamic_pointer_cast< NWayRangePartial >( secondPartial ) == nullptr ) + { + throw std::runtime_error( + "Error when creating n-way differenced range partial; second input " + "object type is incompatible" ); + } } - } - if( secondPartial != nullptr ) - { - if( std::dynamic_pointer_cast< NWayRangePartial >( secondPartial ) == nullptr ) - { - throw std::runtime_error( "Error when creating n-way differenced range partial; second input object type is incompatible" ); - } + differencedPartial = std::make_shared< DifferencedObservablePartial< 1 > >( + firstPartial, + secondPartial, + &observation_models::getDifferencedNWayRangeScalingFactor, + getUndifferencedTimeAndStateIndices( n_way_differenced_range, + linkEnds.size( ) ) ); + break; } - - differencedPartial = std::make_shared< DifferencedObservablePartial< 1 > >( - firstPartial, secondPartial, &observation_models::getDifferencedNWayRangeScalingFactor, - getUndifferencedTimeAndStateIndices( n_way_differenced_range, linkEnds.size( ) ) ); - break; - } - case dsn_n_way_averaged_doppler: - { - if( firstPartial != nullptr ) - { - if( std::dynamic_pointer_cast< NWayRangePartial >( firstPartial ) == nullptr ) + case dsn_n_way_averaged_doppler: { + if( firstPartial != nullptr ) { - throw std::runtime_error( "Error when creating DSN n-way averaged Doppler partial; first input object type is incompatible" ); + if( std::dynamic_pointer_cast< NWayRangePartial >( firstPartial ) == nullptr ) + { + throw std::runtime_error( + "Error when creating DSN n-way averaged Doppler partial; first " + "input object type is incompatible" ); + } } - } - if( secondPartial != nullptr ) - { - if( std::dynamic_pointer_cast< NWayRangePartial >( secondPartial ) == nullptr ) + if( secondPartial != nullptr ) { - throw std::runtime_error( "Error when creating DSN n-way averaged Doppler partial; second input object type is incompatible" ); + if( std::dynamic_pointer_cast< NWayRangePartial >( secondPartial ) == nullptr ) + { + throw std::runtime_error( + "Error when creating DSN n-way averaged Doppler partial; second " + "input object type is incompatible" ); + } } - } - - const std::function< double ( std::vector< FrequencyBands >, double ) > receivedFrequencyFunction = - createLinkFrequencyFunction( - bodies, linkEnds, observation_models::retransmitter, observation_models::receiver ); - - const std::function< double( - const observation_models::LinkEndType, const std::vector< Eigen::Vector6d >&, - const std::vector< double >&, const std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings >, - const bool ) > scalingFactorFunction = - std::bind( observation_models::getDsnNWayAveragedDopplerScalingFactor, receivedFrequencyFunction, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5 ); - - differencedPartial = std::make_shared< DifferencedObservablePartial< 1 > >( - firstPartial, secondPartial, scalingFactorFunction, - getUndifferencedTimeAndStateIndices( dsn_n_way_averaged_doppler, linkEnds.size( ) ) ); - break; - } - default: - throw std::runtime_error( "Error when creating differenced observable partial (size 1); observable " + getObservableName( differencedObservableType ) + - " is not differenced. " ); + const std::function< double( std::vector< FrequencyBands >, double ) > + receivedFrequencyFunction = + createLinkFrequencyFunction( bodies, + linkEnds, + observation_models::retransmitter, + observation_models::receiver ); + + const std::function< double( + const observation_models::LinkEndType, + const std::vector< Eigen::Vector6d >&, + const std::vector< double >&, + const std::shared_ptr< + observation_models::ObservationAncilliarySimulationSettings >, + const bool ) > + scalingFactorFunction = std::bind( + observation_models::getDsnNWayAveragedDopplerScalingFactor, + receivedFrequencyFunction, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3, + std::placeholders::_4, + std::placeholders::_5 ); + + differencedPartial = std::make_shared< DifferencedObservablePartial< 1 > >( + firstPartial, + secondPartial, + scalingFactorFunction, + getUndifferencedTimeAndStateIndices( dsn_n_way_averaged_doppler, + linkEnds.size( ) ) ); + break; + } + default: + throw std::runtime_error( + "Error when creating differenced observable partial (size 1); observable " + + getObservableName( differencedObservableType ) + " is not differenced. " ); } return differencedPartial; } }; -template< > +template<> class DifferencedObservationPartialCreator< 2 > { -public: + public: static std::shared_ptr< ObservationPartial< 2 > > createDifferencedObservationPartial( const observation_models::ObservableType differencedObservableType, const std::shared_ptr< ObservationPartial< 2 > > firstPartial, @@ -497,61 +659,87 @@ class DifferencedObservationPartialCreator< 2 > std::shared_ptr< ObservationPartial< 2 > > differencedPartial; switch( differencedObservableType ) { - case relative_angular_position: - { - if( !isParameterObservationLinkTimeProperty( getDifferencedPartialParameterIdentifier( firstPartial, secondPartial ).first ) ) - { - if( firstPartial != nullptr ) + case relative_angular_position: { + if( !isParameterObservationLinkTimeProperty( + getDifferencedPartialParameterIdentifier( firstPartial, secondPartial ) + .first ) ) { - if( std::dynamic_pointer_cast< DirectObservationPartial< 2 > >( firstPartial ) == nullptr ) + if( firstPartial != nullptr ) { - throw std::runtime_error( "Error when creating relative angular position partial; first input object type is incompatible" ); + if( std::dynamic_pointer_cast< DirectObservationPartial< 2 > >( + firstPartial ) == nullptr ) + { + throw std::runtime_error( + "Error when creating relative angular position partial; first " + "input object type is incompatible" ); + } + else if( std::dynamic_pointer_cast< DirectObservationPartial< 2 > >( + firstPartial ) + ->getObservableType( ) != angular_position ) + { + throw std::runtime_error( + "Error when creating relative angular position partial; first " + "input observable type is incompatible" ); + } } - else if( std::dynamic_pointer_cast< DirectObservationPartial< 2 > >( firstPartial )->getObservableType( ) != angular_position ) - { - throw std::runtime_error( "Error when creating relative angular position partial; first input observable type is incompatible" ); - } - } - if( secondPartial != nullptr ) - { - if( std::dynamic_pointer_cast< DirectObservationPartial< 2 > >( secondPartial ) == nullptr ) - { - throw std::runtime_error( "Error when creating relative angular position partial; second input object type is incompatible" ); - } - else if( std::dynamic_pointer_cast< DirectObservationPartial< 2 > >( secondPartial )->getObservableType( ) != angular_position ) + if( secondPartial != nullptr ) { - throw std::runtime_error( "Error when creating relative angular position partial; second input observable type is incompatible" ); + if( std::dynamic_pointer_cast< DirectObservationPartial< 2 > >( + secondPartial ) == nullptr ) + { + throw std::runtime_error( + "Error when creating relative angular position partial; second " + "input object type is incompatible" ); + } + else if( std::dynamic_pointer_cast< DirectObservationPartial< 2 > >( + secondPartial ) + ->getObservableType( ) != angular_position ) + { + throw std::runtime_error( + "Error when creating relative angular position partial; second " + "input observable type is incompatible" ); + } } } - } - - differencedPartial = std::make_shared< DifferencedObservablePartial< 2 > >( - firstPartial, secondPartial, &getRelativeAngularPositionScalingFactor, - getUndifferencedTimeAndStateIndices( relative_angular_position, linkEnds.size( ) ) ); - break; - } - default: - throw std::runtime_error( "Error when creating differenced observable partial (size 2); observable " + getObservableName( differencedObservableType ) + - " is not differenced. " ); + differencedPartial = std::make_shared< DifferencedObservablePartial< 2 > >( + firstPartial, + secondPartial, + &getRelativeAngularPositionScalingFactor, + getUndifferencedTimeAndStateIndices( relative_angular_position, + linkEnds.size( ) ) ); + break; + } + default: + throw std::runtime_error( + "Error when creating differenced observable partial (size 2); observable " + + getObservableName( differencedObservableType ) + " is not differenced. " ); } return differencedPartial; } }; template< int ObservationSize > -std::map< std::pair< int, int >, std::pair< std::shared_ptr< ObservationPartial< ObservationSize > >, std::shared_ptr< ObservationPartial< ObservationSize > > > > +std::map< std::pair< int, int >, + std::pair< std::shared_ptr< ObservationPartial< ObservationSize > >, + std::shared_ptr< ObservationPartial< ObservationSize > > > > mergeUndifferencedPartialContribution( - const std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< ObservationSize > > >& firstPartialList, - const std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< ObservationSize > > >& secondPartialList ) + const std::map< std::pair< int, int >, + std::shared_ptr< ObservationPartial< ObservationSize > > >& + firstPartialList, + const std::map< std::pair< int, int >, + std::shared_ptr< ObservationPartial< ObservationSize > > >& + secondPartialList ) { - std::map< std::pair< int, int >, std::pair< std::shared_ptr< ObservationPartial< ObservationSize > >, std::shared_ptr< ObservationPartial< ObservationSize > > > > + std::map< std::pair< int, int >, + std::pair< std::shared_ptr< ObservationPartial< ObservationSize > >, + std::shared_ptr< ObservationPartial< ObservationSize > > > > mergedPartials; std::shared_ptr< ObservationPartial< ObservationSize > > firstPartial; std::shared_ptr< ObservationPartial< ObservationSize > > secondPartial; - for( auto it : firstPartialList ) + for( auto it: firstPartialList ) { firstPartial = it.second; if( secondPartialList.count( it.first ) != 0 ) @@ -565,7 +753,7 @@ mergeUndifferencedPartialContribution( mergedPartials[ it.first ] = std::make_pair( firstPartial, secondPartial ); } - for( auto it : secondPartialList ) + for( auto it: secondPartialList ) { if( mergedPartials.count( it.first ) == 0 ) { @@ -575,104 +763,137 @@ mergeUndifferencedPartialContribution( return mergedPartials; } - template< typename ParameterType, typename TimeType, int ObservationSize > -std::pair< std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< ObservationSize > > > , -std::shared_ptr< PositionPartialScaling > > createDifferencedObservablePartials( - const std::shared_ptr< observation_models::ObservationModel< ObservationSize, ParameterType, TimeType > > observationModel, +std::pair< + std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< ObservationSize > > >, + std::shared_ptr< PositionPartialScaling > > +createDifferencedObservablePartials( + const std::shared_ptr< + observation_models::ObservationModel< ObservationSize, ParameterType, TimeType > > + observationModel, const simulation_setup::SystemOfBodies& bodies, - const std::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > parametersToEstimate, + const std::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterType > > + parametersToEstimate, const bool isPartialForDifferencedObservable, const bool isPartialForConcatenatedObservable ) { using namespace observation_models; - std::pair< std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< ObservationSize > > > , - std::shared_ptr< PositionPartialScaling > > differencedPartialsAndScaling; + std::pair< std::map< std::pair< int, int >, + std::shared_ptr< ObservationPartial< ObservationSize > > >, + std::shared_ptr< PositionPartialScaling > > + differencedPartialsAndScaling; LinkEnds linkEnds = observationModel->getLinkEnds( ); ObservableType differencedObservableType = observationModel->getObservableType( ); - ObservableType undifferencedObservableType = getUndifferencedObservableType( - differencedObservableType ); + ObservableType undifferencedObservableType = + getUndifferencedObservableType( differencedObservableType ); - auto undifferencedObservationModels = UndifferencedObservationModelExtractor< ObservationSize >::extract( observationModel ); + auto undifferencedObservationModels = + UndifferencedObservationModelExtractor< ObservationSize >::extract( observationModel ); - std::shared_ptr< ObservationModel< ObservationSize, ParameterType, TimeType > > undifferencedObservationModelFirst = - undifferencedObservationModels.first; - std::shared_ptr< ObservationModel< ObservationSize, ParameterType, TimeType > > undifferencedObservationModelSecond = - undifferencedObservationModels.second; + std::shared_ptr< ObservationModel< ObservationSize, ParameterType, TimeType > > + undifferencedObservationModelFirst = undifferencedObservationModels.first; + std::shared_ptr< ObservationModel< ObservationSize, ParameterType, TimeType > > + undifferencedObservationModelSecond = undifferencedObservationModels.second; - std::pair< std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< ObservationSize > > >, std::shared_ptr< PositionPartialScaling > > + std::pair< std::map< std::pair< int, int >, + std::shared_ptr< ObservationPartial< ObservationSize > > >, + std::shared_ptr< PositionPartialScaling > > firstUndifferencedObservablePartials = - ObservationPartialCreator::createObservationPartials( - undifferencedObservationModelFirst, bodies, parametersToEstimate, true, isPartialForConcatenatedObservable ); - - std::pair< std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< ObservationSize > > >, std::shared_ptr< PositionPartialScaling > > + ObservationPartialCreator< ObservationSize, ParameterType, TimeType >:: + createObservationPartials( undifferencedObservationModelFirst, + bodies, + parametersToEstimate, + true, + isPartialForConcatenatedObservable ); + + std::pair< std::map< std::pair< int, int >, + std::shared_ptr< ObservationPartial< ObservationSize > > >, + std::shared_ptr< PositionPartialScaling > > secondUndifferencedObservablePartials = - ObservationPartialCreator::createObservationPartials( - undifferencedObservationModelSecond, bodies, parametersToEstimate, true, isPartialForConcatenatedObservable ); - - std::map< std::pair< int, int >, std::pair< std::shared_ptr< ObservationPartial< ObservationSize > >, std::shared_ptr< ObservationPartial< ObservationSize > > > > + ObservationPartialCreator< ObservationSize, ParameterType, TimeType >:: + createObservationPartials( undifferencedObservationModelSecond, + bodies, + parametersToEstimate, + true, + isPartialForConcatenatedObservable ); + + std::map< std::pair< int, int >, + std::pair< std::shared_ptr< ObservationPartial< ObservationSize > >, + std::shared_ptr< ObservationPartial< ObservationSize > > > > mergedPartials = mergeUndifferencedPartialContribution( - firstUndifferencedObservablePartials.first, secondUndifferencedObservablePartials.first ); + firstUndifferencedObservablePartials.first, + secondUndifferencedObservablePartials.first ); - std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< ObservationSize > > > differencedObservationPartialList; + std::map< std::pair< int, int >, std::shared_ptr< ObservationPartial< ObservationSize > > > + differencedObservationPartialList; // Iterate over all one-way range partials and create one-way range rate partial from them. - for( auto it : mergedPartials ) + for( auto it: mergedPartials ) { // Create range rate partial. - differencedObservationPartialList[ it.first ] = - DifferencedObservationPartialCreator< ObservationSize >::createDifferencedObservationPartial( - differencedObservableType, - it.second.first, - it.second.second, - linkEnds, - bodies ); + differencedObservationPartialList[ it.first ] = DifferencedObservationPartialCreator< + ObservationSize >::createDifferencedObservationPartial( differencedObservableType, + it.second.first, + it.second.second, + linkEnds, + bodies ); } - // Creatlinke bias partials - std::map< int, std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > > - vectorParametersToEstimate = parametersToEstimate->getVectorParameters( ); - for( std::map< int, std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > >::iterator - parameterIterator = - vectorParametersToEstimate.begin( ); parameterIterator != vectorParametersToEstimate.end( ); parameterIterator++ ) + std::map< int, + std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > > + vectorParametersToEstimate = parametersToEstimate->getVectorParameters( ); + for( std::map< int, + std::shared_ptr< estimatable_parameters::EstimatableParameter< + Eigen::VectorXd > > >::iterator parameterIterator = + vectorParametersToEstimate.begin( ); + parameterIterator != vectorParametersToEstimate.end( ); + parameterIterator++ ) { - - std::shared_ptr< ObservationPartial< ObservationSize > > currentDifferencedObservationPartial; - if( !isParameterObservationLinkTimeProperty( parameterIterator->second->getParameterName( ).first ) && - isParameterObservationLinkProperty( parameterIterator->second->getParameterName( ).first ) ) + std::shared_ptr< ObservationPartial< ObservationSize > > + currentDifferencedObservationPartial; + if( !isParameterObservationLinkTimeProperty( + parameterIterator->second->getParameterName( ).first ) && + isParameterObservationLinkProperty( + parameterIterator->second->getParameterName( ).first ) ) { - currentDifferencedObservationPartial = createObservationPartialWrtLinkProperty< ObservationSize >( - linkEnds, differencedObservableType, parameterIterator->second, bodies, isPartialForDifferencedObservable, isPartialForConcatenatedObservable ); + currentDifferencedObservationPartial = + createObservationPartialWrtLinkProperty< ObservationSize >( + linkEnds, + differencedObservableType, + parameterIterator->second, + bodies, + isPartialForDifferencedObservable, + isPartialForConcatenatedObservable ); } - // Check if partial is non-nullptr (i.e. whether dependency exists between current doppler and current parameter) + // Check if partial is non-nullptr (i.e. whether dependency exists between current doppler + // and current parameter) if( currentDifferencedObservationPartial != nullptr ) { // Add partial to the list. - std::pair< double, double > currentPair = std::pair< int, int >( parameterIterator->first, - parameterIterator->second->getParameterSize( ) ); + std::pair< double, double > currentPair = std::pair< int, int >( + parameterIterator->first, parameterIterator->second->getParameterSize( ) ); differencedObservationPartialList[ currentPair ] = currentDifferencedObservationPartial; } } differencedPartialsAndScaling = std::make_pair( - differencedObservationPartialList, ObservationPartialScalingCreator< ObservationSize >:: - template createDifferencedPositionPartialScalingObject< ParameterType, TimeType >( - differencedObservableType, firstUndifferencedObservablePartials.second, - secondUndifferencedObservablePartials.second, bodies ) ); + differencedObservationPartialList, + ObservationPartialScalingCreator< ObservationSize >:: + template createDifferencedPositionPartialScalingObject< ParameterType, + TimeType >( + differencedObservableType, + firstUndifferencedObservablePartials.second, + secondUndifferencedObservablePartials.second, + bodies ) ); return differencedPartialsAndScaling; - - } +} // namespace observation_partials +} // namespace tudat -} - -} - - -#endif // TUDAT_CREATEOBSERVATIONPARTIALS_H +#endif // TUDAT_CREATEOBSERVATIONPARTIALS_H diff --git a/include/tudat/simulation/estimation_setup/orbitDeterminationManager.h b/include/tudat/simulation/estimation_setup/orbitDeterminationManager.h index 5e7fe72cac..1326d135e2 100644 --- a/include/tudat/simulation/estimation_setup/orbitDeterminationManager.h +++ b/include/tudat/simulation/estimation_setup/orbitDeterminationManager.h @@ -1017,7 +1017,7 @@ class OrbitDeterminationManager // /*! // * Function to convert from one representation of all measurement data (AlternativeEstimationInputType) to the other (EstimationInputType). // * In the former, the vector of times and vector of associated observations are stored separately. In the latter, they are - // * stored as a map with time as key and obsevation as value. + // * stored as a map with time as key and observation as value. // * \param alternativeEstimationInput Original representation of measurement data // * \return Converted measurement data. // */ diff --git a/src/interface/json/estimation/observation.cpp b/src/interface/json/estimation/observation.cpp index ac267ce09f..582c63d135 100644 --- a/src/interface/json/estimation/observation.cpp +++ b/src/interface/json/estimation/observation.cpp @@ -9,8 +9,8 @@ * */ - #include "tudat/interface/json/estimation/observation.h" + #include "tudat/interface/json/support/utilities.h" namespace tudat @@ -23,7 +23,7 @@ namespace observation_models void to_json( nlohmann::json& jsonObject, const std::shared_ptr< ObservationSettings >& observationSettings ) { - if ( !observationSettings ) + if( !observationSettings ) { return; } @@ -32,80 +32,92 @@ void to_json( nlohmann::json& jsonObject, const ObservableType observableType = observationSettings->observableType_; jsonObject[ K::observableType ] = observableType; - assignIfNotEmpty( jsonObject, K::lightTimeCorrectionSettingsList, observationSettings->lightTimeCorrectionsList_ ); + assignIfNotEmpty( jsonObject, + K::lightTimeCorrectionSettingsList, + observationSettings->lightTimeCorrectionsList_ ); if( observationSettings->biasSettings_ != nullptr ) { jsonObject[ K::biasSettings ] = observationSettings->biasSettings_; } - - switch ( observableType ) + switch( observableType ) { - case one_way_range: - case angular_position: - case relative_angular_position: - case position_observable: - case relative_position_observable: - case one_way_doppler: - { - std::shared_ptr< OneWayDopplerObservationSettings > oneWayDopplerObservationSettings = - std::dynamic_pointer_cast< OneWayDopplerObservationSettings >( observationSettings ); - if( !( oneWayDopplerObservationSettings == nullptr ) ) - { - if( oneWayDopplerObservationSettings->transmitterProperTimeRateSettings_ != nullptr ) + case one_way_range: + case angular_position: + case relative_angular_position: + case position_observable: + case relative_position_observable: + case one_way_doppler: { + std::shared_ptr< OneWayDopplerObservationSettings > oneWayDopplerObservationSettings = + std::dynamic_pointer_cast< OneWayDopplerObservationSettings >( + observationSettings ); + if( !( oneWayDopplerObservationSettings == nullptr ) ) { - jsonObject[ K::transmitterProperTimeRateSettings ] = oneWayDopplerObservationSettings->transmitterProperTimeRateSettings_; + if( oneWayDopplerObservationSettings->transmitterProperTimeRateSettings_ != + nullptr ) + { + jsonObject[ K::transmitterProperTimeRateSettings ] = + oneWayDopplerObservationSettings->transmitterProperTimeRateSettings_; + } + + if( oneWayDopplerObservationSettings->receiverProperTimeRateSettings_ != nullptr ) + { + jsonObject[ K::receiverProperTimeRateSettings ] = + oneWayDopplerObservationSettings->receiverProperTimeRateSettings_; + } } - - if( oneWayDopplerObservationSettings->receiverProperTimeRateSettings_ != nullptr ) + return; + } + case one_way_differenced_range: { + std::shared_ptr< OneWayDifferencedRangeRateObservationSettings > + oneWayDifferencedRangeObservationSettings = std::dynamic_pointer_cast< + OneWayDifferencedRangeRateObservationSettings >( observationSettings ); + assertNonnullptrPointer( oneWayDifferencedRangeObservationSettings ); + + std::cerr << "Warning, assuming constant integration time of 1-way differenced range " + "when parsing to JSON, correctness not ensured" + << std::endl; + jsonObject[ K::constantIntegrationTime ] = + oneWayDifferencedRangeObservationSettings->integrationTimeFunction_( 0.0 ); + return; + } + case n_way_range: { + std::shared_ptr< NWayRangeObservationSettings > nWayRangeObservationSettings = + std::dynamic_pointer_cast< NWayRangeObservationSettings >( + observationSettings ); + if( !( nWayRangeObservationSettings == nullptr ) ) { - jsonObject[ K::receiverProperTimeRateSettings ] = oneWayDopplerObservationSettings->receiverProperTimeRateSettings_ ; + jsonObject[ K::oneWayRangeObservationSettings ] = + nWayRangeObservationSettings->oneWayRangeObservationSettings_; + + if( !( nWayRangeObservationSettings->retransmissionTimesFunction_ ) ) + { + std::vector< double > retransmissionTimes = + nWayRangeObservationSettings->retransmissionTimesFunction_( 0.0 ); + std::cerr << "Warning, assuming constant delay times of n-way range when " + "parsing to JSON, correctness not ensured" + << std::endl; + jsonObject[ K::retransmissionTimes ] = retransmissionTimes; + } } + return; } - return; - } - case one_way_differenced_range: - { - std::shared_ptr< OneWayDifferencedRangeRateObservationSettings > oneWayDifferencedRangeObservationSettings = - std::dynamic_pointer_cast< OneWayDifferencedRangeRateObservationSettings >( observationSettings ); - assertNonnullptrPointer( oneWayDifferencedRangeObservationSettings ); - - std::cerr<<"Warning, assuming constant integration time of 1-way differenced range when parsing to JSON, correctness not ensured"<integrationTimeFunction_( 0.0 ); - return; - } - case n_way_range: - { - std::shared_ptr< NWayRangeObservationSettings > nWayRangeObservationSettings = - std::dynamic_pointer_cast< NWayRangeObservationSettings >( observationSettings ); - if( !( nWayRangeObservationSettings == nullptr ) ) - { - jsonObject[ K::oneWayRangeObsevationSettings ] = nWayRangeObservationSettings->oneWayRangeObsevationSettings_; - - if( !( nWayRangeObservationSettings->retransmissionTimesFunction_ ) ) + case two_way_doppler: { + std::shared_ptr< TwoWayDopplerObservationSettings > twoWayDopplerObservationSettings = + std::dynamic_pointer_cast< TwoWayDopplerObservationSettings >( + observationSettings ); + if( !( twoWayDopplerObservationSettings == nullptr ) ) { - std::vector< double > retransmissionTimes = nWayRangeObservationSettings->retransmissionTimesFunction_( 0.0 ); - std::cerr<<"Warning, assuming constant delay times of n-way range when parsing to JSON, correctness not ensured"<uplinkOneWayDopplerSettings_; + jsonObject[ K::downlinkOneWayDopplerSettings ] = + twoWayDopplerObservationSettings->downlinkOneWayDopplerSettings_; } + return; } - return; - } - case two_way_doppler: - { - std::shared_ptr< TwoWayDopplerObservationSettings > twoWayDopplerObservationSettings = - std::dynamic_pointer_cast< TwoWayDopplerObservationSettings >( observationSettings ); - if( !( twoWayDopplerObservationSettings == nullptr ) ) - { - jsonObject[ K::uplinkOneWayDopplerSettings ] = twoWayDopplerObservationSettings->uplinkOneWayDopplerSettings_; - jsonObject[ K::downlinkOneWayDopplerSettings ] = twoWayDopplerObservationSettings->downlinkOneWayDopplerSettings_; + default: { + return; } - return; - } - default: - { - return; - } } } @@ -120,181 +132,181 @@ void from_json( const nlohmann::json& jsonObject, std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList = getValue< std::vector< std::shared_ptr< LightTimeCorrectionSettings > > >( - jsonObject, K::lightTimeCorrectionSettingsList, - std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ) ); + jsonObject, + K::lightTimeCorrectionSettingsList, + std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ) ); std::shared_ptr< ObservationBiasSettings > biasSettings = getValue< std::shared_ptr< ObservationBiasSettings > >( - jsonObject, K::biasSettings, nullptr ); + jsonObject, K::biasSettings, nullptr ); - switch ( observableType ) - { - case one_way_doppler: + switch( observableType ) { - std::shared_ptr< DopplerProperTimeRateSettings > transmitterProperTimeRateSettings = - getValue< std::shared_ptr< DopplerProperTimeRateSettings > >( - jsonObject, K::transmitterProperTimeRateSettings, nullptr ); - std::shared_ptr< DopplerProperTimeRateSettings > receiverProperTimeRateSettings = - getValue< std::shared_ptr< DopplerProperTimeRateSettings > >( - jsonObject, K::receiverProperTimeRateSettings, nullptr ); - - observationSettings = std::make_shared< OneWayDopplerObservationSettings >( + case one_way_doppler: { + std::shared_ptr< DopplerProperTimeRateSettings > transmitterProperTimeRateSettings = + getValue< std::shared_ptr< DopplerProperTimeRateSettings > >( + jsonObject, K::transmitterProperTimeRateSettings, nullptr ); + std::shared_ptr< DopplerProperTimeRateSettings > receiverProperTimeRateSettings = + getValue< std::shared_ptr< DopplerProperTimeRateSettings > >( + jsonObject, K::receiverProperTimeRateSettings, nullptr ); + + observationSettings = std::make_shared< OneWayDopplerObservationSettings >( lightTimeCorrectionsList, transmitterProperTimeRateSettings, receiverProperTimeRateSettings, biasSettings ); - return; - } - case one_way_differenced_range: - { - double constantIntegrationTime = getValue< double >( - jsonObject, K::constantIntegrationTime ); + return; + } + case one_way_differenced_range: { + double constantIntegrationTime = + getValue< double >( jsonObject, K::constantIntegrationTime ); - observationSettings = std::make_shared< OneWayDifferencedRangeRateObservationSettings >( - [ = ]( const double ){ return constantIntegrationTime; }, + observationSettings = std::make_shared< OneWayDifferencedRangeRateObservationSettings >( + [ = ]( const double ) { return constantIntegrationTime; }, lightTimeCorrectionsList, biasSettings ); - return; - } - case n_way_range: - { - std::vector< std::shared_ptr< ObservationSettings > > oneWayRangeObsevationSettings = - getValue< std::vector< std::shared_ptr< ObservationSettings > > >( - jsonObject, K::oneWayRangeObsevationSettings, - std::vector< std::shared_ptr< ObservationSettings > >( ) ); - if( oneWayRangeObsevationSettings.size( ) == 0 ) - { - observationSettings = std::make_shared< ObservationSettings >( - observableType, - lightTimeCorrectionsList, - biasSettings ); - } - else - { - std::vector< double > retransmissionTimes = - getValue< std::vector< double > >( - jsonObject, K::retransmissionTimes, - std::vector< double >( ) ); - observationSettings = std::make_shared< NWayRangeObservationSettings >( - oneWayRangeObsevationSettings, [ = ]( const double ){ return retransmissionTimes; }, - biasSettings ); + return; } - return; - } - case two_way_doppler: - { - std::shared_ptr< ObservationSettings > uplinkOneWayDopplerSettings = - getValue< std::shared_ptr< ObservationSettings > >( - jsonObject, K::uplinkOneWayDopplerSettings, nullptr ); - std::shared_ptr< ObservationSettings > downlinkOneWayDopplerSettings = - getValue< std::shared_ptr< ObservationSettings > >( - jsonObject, K::downlinkOneWayDopplerSettings, nullptr ); - if( ( uplinkOneWayDopplerSettings == nullptr ) && ( downlinkOneWayDopplerSettings == nullptr ) ) - { - observationSettings = std::make_shared< ObservationSettings >( - observableType, - lightTimeCorrectionsList, + case n_way_range: { + std::vector< std::shared_ptr< ObservationSettings > > oneWayRangeObservationSettings = + getValue< std::vector< std::shared_ptr< ObservationSettings > > >( + jsonObject, + K::oneWayRangeObservationSettings, + std::vector< std::shared_ptr< ObservationSettings > >( ) ); + if( oneWayRangeObservationSettings.size( ) == 0 ) + { + observationSettings = std::make_shared< ObservationSettings >( + observableType, lightTimeCorrectionsList, biasSettings ); + } + else + { + std::vector< double > retransmissionTimes = getValue< std::vector< double > >( + jsonObject, K::retransmissionTimes, std::vector< double >( ) ); + observationSettings = std::make_shared< NWayRangeObservationSettings >( + oneWayRangeObservationSettings, + [ = ]( const double ) { return retransmissionTimes; }, biasSettings ); + } + return; } - else if( ( uplinkOneWayDopplerSettings != nullptr ) && ( downlinkOneWayDopplerSettings != nullptr ) ) - { - if( std::dynamic_pointer_cast< OneWayDopplerObservationSettings >( uplinkOneWayDopplerSettings ) == nullptr || - std::dynamic_pointer_cast< OneWayDopplerObservationSettings >( downlinkOneWayDopplerSettings ) == nullptr ) + case two_way_doppler: { + std::shared_ptr< ObservationSettings > uplinkOneWayDopplerSettings = + getValue< std::shared_ptr< ObservationSettings > >( + jsonObject, K::uplinkOneWayDopplerSettings, nullptr ); + std::shared_ptr< ObservationSettings > downlinkOneWayDopplerSettings = + getValue< std::shared_ptr< ObservationSettings > >( + jsonObject, K::downlinkOneWayDopplerSettings, nullptr ); + if( ( uplinkOneWayDopplerSettings == nullptr ) && + ( downlinkOneWayDopplerSettings == nullptr ) ) { - throw std::runtime_error( "Error when reading 2-way Doppler dettings from JSON file, link doppler settings are inconsistent" ); + observationSettings = std::make_shared< ObservationSettings >( + observableType, lightTimeCorrectionsList, biasSettings ); } - observationSettings = std::make_shared< TwoWayDopplerObservationSettings >( - std::dynamic_pointer_cast< OneWayDopplerObservationSettings >( uplinkOneWayDopplerSettings ), - std::dynamic_pointer_cast< OneWayDopplerObservationSettings >( downlinkOneWayDopplerSettings ), + else if( ( uplinkOneWayDopplerSettings != nullptr ) && + ( downlinkOneWayDopplerSettings != nullptr ) ) + { + if( std::dynamic_pointer_cast< OneWayDopplerObservationSettings >( + uplinkOneWayDopplerSettings ) == nullptr || + std::dynamic_pointer_cast< OneWayDopplerObservationSettings >( + downlinkOneWayDopplerSettings ) == nullptr ) + { + throw std::runtime_error( + "Error when reading 2-way Doppler dettings from JSON file, link " + "doppler settings are inconsistent" ); + } + observationSettings = std::make_shared< TwoWayDopplerObservationSettings >( + std::dynamic_pointer_cast< OneWayDopplerObservationSettings >( + uplinkOneWayDopplerSettings ), + std::dynamic_pointer_cast< OneWayDopplerObservationSettings >( + downlinkOneWayDopplerSettings ), biasSettings ); + } + else + { + throw std::runtime_error( + "Error when reading 2-way Doppler dettings from JSON file, settings are " + "inconsistent" ); + } + return; } - else - { - throw std::runtime_error( "Error when reading 2-way Doppler dettings from JSON file, settings are inconsistent" ); + default: { + observationSettings = std::make_shared< ObservationSettings >( + observableType, lightTimeCorrectionsList, biasSettings ); + return; } - return; - } - default: - { - observationSettings = std::make_shared< ObservationSettings >( - observableType, - lightTimeCorrectionsList, - biasSettings ); - return; - } } } //! Create a `json` object from a shared pointer to a `ObservationSettings` object. -void to_json( nlohmann::json& jsonObject, const std::shared_ptr< ObservationBiasSettings >& biasSettings ) +void to_json( nlohmann::json& jsonObject, + const std::shared_ptr< ObservationBiasSettings >& biasSettings ) { - if ( !biasSettings ) + if( !biasSettings ) { return; } using namespace json_interface; using K = Keys::ObservationBias; - const ObservationBiasTypes biasType = biasSettings->observationBiasType_;; + const ObservationBiasTypes biasType = biasSettings->observationBiasType_; + ; jsonObject[ K::biasType ] = biasType; switch( biasType ) { - case multiple_observation_biases: - { - std::shared_ptr< MultipleObservationBiasSettings > multipleBiasSettings = - std::dynamic_pointer_cast< MultipleObservationBiasSettings >( biasSettings ); - assertNonnullptrPointer( multipleBiasSettings ); - jsonObject[ K::multipleBiasesList ] = multipleBiasSettings->biasSettingsList_; - return; - } - case constant_absolute_bias: - { - std::shared_ptr< ConstantObservationBiasSettings > constantBiasSettings = - std::dynamic_pointer_cast< ConstantObservationBiasSettings >( biasSettings ); - assertNonnullptrPointer( constantBiasSettings ); + case multiple_observation_biases: { + std::shared_ptr< MultipleObservationBiasSettings > multipleBiasSettings = + std::dynamic_pointer_cast< MultipleObservationBiasSettings >( biasSettings ); + assertNonnullptrPointer( multipleBiasSettings ); + jsonObject[ K::multipleBiasesList ] = multipleBiasSettings->biasSettingsList_; + return; + } + case constant_absolute_bias: { + std::shared_ptr< ConstantObservationBiasSettings > constantBiasSettings = + std::dynamic_pointer_cast< ConstantObservationBiasSettings >( biasSettings ); + assertNonnullptrPointer( constantBiasSettings ); - jsonObject[ K::constantBias ] = constantBiasSettings->observationBias_; - return; - } - case constant_relative_bias: - { - std::shared_ptr< ConstantObservationBiasSettings > constantBiasSettings = - std::dynamic_pointer_cast< ConstantObservationBiasSettings >( biasSettings ); - assertNonnullptrPointer( constantBiasSettings ); + jsonObject[ K::constantBias ] = constantBiasSettings->observationBias_; + return; + } + case constant_relative_bias: { + std::shared_ptr< ConstantObservationBiasSettings > constantBiasSettings = + std::dynamic_pointer_cast< ConstantObservationBiasSettings >( biasSettings ); + assertNonnullptrPointer( constantBiasSettings ); - jsonObject[ K::constantBias ] = constantBiasSettings->observationBias_; - return; - } - case arc_wise_constant_absolute_bias: - { - std::shared_ptr< ArcWiseConstantObservationBiasSettings > arcWiseBiasSettings = - std::dynamic_pointer_cast< ArcWiseConstantObservationBiasSettings >( biasSettings ); - assertNonnullptrPointer( arcWiseBiasSettings ); + jsonObject[ K::constantBias ] = constantBiasSettings->observationBias_; + return; + } + case arc_wise_constant_absolute_bias: { + std::shared_ptr< ArcWiseConstantObservationBiasSettings > arcWiseBiasSettings = + std::dynamic_pointer_cast< ArcWiseConstantObservationBiasSettings >( + biasSettings ); + assertNonnullptrPointer( arcWiseBiasSettings ); - jsonObject[ K::arcWiseBiasList ] = arcWiseBiasSettings->observationBiases_; - jsonObject[ K::arcStartTimes ] = arcWiseBiasSettings->arcStartTimes_; - jsonObject[ K::referenceLinkEnd ] = arcWiseBiasSettings->linkEndForTime_; + jsonObject[ K::arcWiseBiasList ] = arcWiseBiasSettings->observationBiases_; + jsonObject[ K::arcStartTimes ] = arcWiseBiasSettings->arcStartTimes_; + jsonObject[ K::referenceLinkEnd ] = arcWiseBiasSettings->linkEndForTime_; - return; - } - case arc_wise_constant_relative_bias: - { - std::shared_ptr< ArcWiseConstantObservationBiasSettings > arcWiseBiasSettings = - std::dynamic_pointer_cast< ArcWiseConstantObservationBiasSettings >( biasSettings ); - assertNonnullptrPointer( arcWiseBiasSettings ); + return; + } + case arc_wise_constant_relative_bias: { + std::shared_ptr< ArcWiseConstantObservationBiasSettings > arcWiseBiasSettings = + std::dynamic_pointer_cast< ArcWiseConstantObservationBiasSettings >( + biasSettings ); + assertNonnullptrPointer( arcWiseBiasSettings ); - jsonObject[ K::arcWiseBiasList ] = arcWiseBiasSettings->observationBiases_; - jsonObject[ K::arcStartTimes ] = arcWiseBiasSettings->arcStartTimes_; - jsonObject[ K::referenceLinkEnd ] = arcWiseBiasSettings->linkEndForTime_; + jsonObject[ K::arcWiseBiasList ] = arcWiseBiasSettings->observationBiases_; + jsonObject[ K::arcStartTimes ] = arcWiseBiasSettings->arcStartTimes_; + jsonObject[ K::referenceLinkEnd ] = arcWiseBiasSettings->linkEndForTime_; - return; - } - default: - return; + return; + } + default: + return; } } -void from_json( const nlohmann::json& jsonObject, std::shared_ptr< ObservationBiasSettings >& biasSettings ) +void from_json( const nlohmann::json& jsonObject, + std::shared_ptr< ObservationBiasSettings >& biasSettings ) { using namespace json_interface; using K = Keys::ObservationBias; @@ -304,63 +316,58 @@ void from_json( const nlohmann::json& jsonObject, std::shared_ptr< ObservationBi switch( biasType ) { - case constant_absolute_bias: - { - Eigen::VectorXd constantBias = - getValue< Eigen::VectorXd >( jsonObject, K::constantBias ); - biasSettings = std::make_shared< ConstantObservationBiasSettings >( constantBias, true ); - return; - } - case constant_relative_bias: - { - Eigen::VectorXd constantBias = - getValue< Eigen::VectorXd >( jsonObject, K::constantBias ); - biasSettings = std::make_shared< ConstantObservationBiasSettings >( constantBias, false ); - return; - } - case arc_wise_constant_absolute_bias: - { - std::vector< double > arcStartTimes = - getValue< std::vector< double > >( jsonObject, K::arcStartTimes ); - std::vector< Eigen::VectorXd > observationBiases = - getValue< std::vector< Eigen::VectorXd > >( jsonObject, K::arcWiseBiasList ); - LinkEndType linkEndForTime = - getValue< LinkEndType >( jsonObject, K::referenceLinkEnd ); - biasSettings = std::make_shared< ArcWiseConstantObservationBiasSettings >( + case constant_absolute_bias: { + Eigen::VectorXd constantBias = + getValue< Eigen::VectorXd >( jsonObject, K::constantBias ); + biasSettings = + std::make_shared< ConstantObservationBiasSettings >( constantBias, true ); + return; + } + case constant_relative_bias: { + Eigen::VectorXd constantBias = + getValue< Eigen::VectorXd >( jsonObject, K::constantBias ); + biasSettings = + std::make_shared< ConstantObservationBiasSettings >( constantBias, false ); + return; + } + case arc_wise_constant_absolute_bias: { + std::vector< double > arcStartTimes = + getValue< std::vector< double > >( jsonObject, K::arcStartTimes ); + std::vector< Eigen::VectorXd > observationBiases = + getValue< std::vector< Eigen::VectorXd > >( jsonObject, K::arcWiseBiasList ); + LinkEndType linkEndForTime = getValue< LinkEndType >( jsonObject, K::referenceLinkEnd ); + biasSettings = std::make_shared< ArcWiseConstantObservationBiasSettings >( arcStartTimes, observationBiases, linkEndForTime, true ); - return; - } - case arc_wise_constant_relative_bias: - { - std::vector< double > arcStartTimes = - getValue< std::vector< double > >( jsonObject, K::arcStartTimes ); - std::vector< Eigen::VectorXd > observationBiases = - getValue< std::vector< Eigen::VectorXd > >( jsonObject, K::arcWiseBiasList ); - LinkEndType linkEndForTime = - getValue< LinkEndType >( jsonObject, K::referenceLinkEnd ); - biasSettings = std::make_shared< ArcWiseConstantObservationBiasSettings >( + return; + } + case arc_wise_constant_relative_bias: { + std::vector< double > arcStartTimes = + getValue< std::vector< double > >( jsonObject, K::arcStartTimes ); + std::vector< Eigen::VectorXd > observationBiases = + getValue< std::vector< Eigen::VectorXd > >( jsonObject, K::arcWiseBiasList ); + LinkEndType linkEndForTime = getValue< LinkEndType >( jsonObject, K::referenceLinkEnd ); + biasSettings = std::make_shared< ArcWiseConstantObservationBiasSettings >( arcStartTimes, observationBiases, linkEndForTime, false ); - return; - } - case multiple_observation_biases: - { - std::vector< std::shared_ptr< ObservationBiasSettings > > biasSettingsList = - getValue< std::vector< std::shared_ptr< ObservationBiasSettings > > >( jsonObject, K::multipleBiasesList ); - biasSettings = std::make_shared< MultipleObservationBiasSettings >( biasSettingsList ); - - return; - } - default: - return; - + return; + } + case multiple_observation_biases: { + std::vector< std::shared_ptr< ObservationBiasSettings > > biasSettingsList = + getValue< std::vector< std::shared_ptr< ObservationBiasSettings > > >( + jsonObject, K::multipleBiasesList ); + biasSettings = std::make_shared< MultipleObservationBiasSettings >( biasSettingsList ); + return; + } + default: + return; } } //! Create a `json` object from a shared pointer to a `ObservationSettings` object. -void to_json( nlohmann::json& jsonObject, const std::shared_ptr< DopplerProperTimeRateSettings >& properTimeRateSettings ) +void to_json( nlohmann::json& jsonObject, + const std::shared_ptr< DopplerProperTimeRateSettings >& properTimeRateSettings ) { - if ( !properTimeRateSettings ) + if( !properTimeRateSettings ) { return; } @@ -372,20 +379,21 @@ void to_json( nlohmann::json& jsonObject, const std::shared_ptr< DopplerProperTi jsonObject[ K::properTimeRateType ] = rateType; switch( rateType ) { - case direct_first_order_doppler_proper_time_rate: - { - std::shared_ptr< DirectFirstOrderDopplerProperTimeRateSettings > firstOrderRateType = - std::dynamic_pointer_cast< DirectFirstOrderDopplerProperTimeRateSettings >( properTimeRateSettings ); - assertNonnullptrPointer( firstOrderRateType ); - jsonObject[ K::centralBody ] = firstOrderRateType->centralBodyName_; - return; - } - default: - return; + case direct_first_order_doppler_proper_time_rate: { + std::shared_ptr< DirectFirstOrderDopplerProperTimeRateSettings > firstOrderRateType = + std::dynamic_pointer_cast< DirectFirstOrderDopplerProperTimeRateSettings >( + properTimeRateSettings ); + assertNonnullptrPointer( firstOrderRateType ); + jsonObject[ K::centralBody ] = firstOrderRateType->centralBodyName_; + return; + } + default: + return; } } -void from_json( const nlohmann::json& jsonObject, std::shared_ptr< DopplerProperTimeRateSettings >& properTimeRateSettings ) +void from_json( const nlohmann::json& jsonObject, + std::shared_ptr< DopplerProperTimeRateSettings >& properTimeRateSettings ) { using namespace json_interface; using K = Keys::Observation; @@ -395,23 +403,23 @@ void from_json( const nlohmann::json& jsonObject, std::shared_ptr< DopplerProper switch( rateType ) { - case direct_first_order_doppler_proper_time_rate: - { - properTimeRateSettings = std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( - getValue< std::string >( jsonObject, K::centralBody ) ); - return; - } - default: - { - return; - } + case direct_first_order_doppler_proper_time_rate: { + properTimeRateSettings = + std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( + getValue< std::string >( jsonObject, K::centralBody ) ); + return; + } + default: { + return; + } } } //! Create a `json` object from a shared pointer to a `ObservationSettings` object. -void to_json( nlohmann::json& jsonObject, const std::shared_ptr< LightTimeCorrectionSettings >& lightTimeCorrectionSettings ) +void to_json( nlohmann::json& jsonObject, + const std::shared_ptr< LightTimeCorrectionSettings >& lightTimeCorrectionSettings ) { - if ( !lightTimeCorrectionSettings ) + if( !lightTimeCorrectionSettings ) { return; } @@ -419,26 +427,30 @@ void to_json( nlohmann::json& jsonObject, const std::shared_ptr< LightTimeCorrec using namespace json_interface; using K = Keys::Observation; - const LightTimeCorrectionType correctionType = lightTimeCorrectionSettings->getCorrectionType( ); + const LightTimeCorrectionType correctionType = + lightTimeCorrectionSettings->getCorrectionType( ); jsonObject[ K::lightTimeCorrectionType ] = correctionType; switch( correctionType ) { - case first_order_relativistic: - { - std::shared_ptr< FirstOrderRelativisticLightTimeCorrectionSettings > firstOrderCorrectionSettings = - std::dynamic_pointer_cast< FirstOrderRelativisticLightTimeCorrectionSettings >( lightTimeCorrectionSettings ); - assertNonnullptrPointer( firstOrderCorrectionSettings ); - jsonObject[ K::perturbingBodies ] = firstOrderCorrectionSettings->getPerturbingBodies( ); + case first_order_relativistic: { + std::shared_ptr< FirstOrderRelativisticLightTimeCorrectionSettings > + firstOrderCorrectionSettings = std::dynamic_pointer_cast< + FirstOrderRelativisticLightTimeCorrectionSettings >( + lightTimeCorrectionSettings ); + assertNonnullptrPointer( firstOrderCorrectionSettings ); + jsonObject[ K::perturbingBodies ] = + firstOrderCorrectionSettings->getPerturbingBodies( ); - return; - } - default: - return; + return; + } + default: + return; } } -void from_json( const nlohmann::json& jsonObject, std::shared_ptr< LightTimeCorrectionSettings >& lightTimeCorrectionSettings ) +void from_json( const nlohmann::json& jsonObject, + std::shared_ptr< LightTimeCorrectionSettings >& lightTimeCorrectionSettings ) { using namespace json_interface; using K = Keys::Observation; @@ -450,26 +462,28 @@ void from_json( const nlohmann::json& jsonObject, std::shared_ptr< LightTimeCorr switch( correctionType ) { - case first_order_relativistic: - { - lightTimeCorrectionSettings = std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( - getValue< std::vector< std::string > >( jsonObject, K::perturbingBodies ) ); - return; - } - default: - { - return; - } + case first_order_relativistic: { + lightTimeCorrectionSettings = + std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( + getValue< std::vector< std::string > >( jsonObject, + K::perturbingBodies ) ); + return; + } + default: { + return; + } } } - catch( ... ){ } + catch( ... ) + { } } - //! Create a `json` object from a shared pointer to a `ObservationSettings` object. -void to_json( nlohmann::json& jsonObject, const std::shared_ptr< ObservationSimulationTimeSettings< double > >& observationSimulationTimeSettings ) +void to_json( nlohmann::json& jsonObject, + const std::shared_ptr< ObservationSimulationTimeSettings< double > >& + observationSimulationTimeSettings ) { - if ( !observationSimulationTimeSettings ) + if( !observationSimulationTimeSettings ) { return; } @@ -480,8 +494,10 @@ void to_json( nlohmann::json& jsonObject, const std::shared_ptr< ObservationSimu if( std::dynamic_pointer_cast< TabulatedObservationSimulationTimeSettings< double > >( observationSimulationTimeSettings ) != nullptr ) { - std::vector< double > simulationTimes = std::dynamic_pointer_cast< - TabulatedObservationSimulationTimeSettings< double > >( observationSimulationTimeSettings )->simulationTimes_; + std::vector< double > simulationTimes = + std::dynamic_pointer_cast< TabulatedObservationSimulationTimeSettings< double > >( + observationSimulationTimeSettings ) + ->simulationTimes_; jsonObject[ K::observationSimulationTimesType ] = tabulated_observation_simulation_times; jsonObject[ K::observationSimulationTimesList ] = simulationTimes; @@ -491,23 +507,25 @@ void to_json( nlohmann::json& jsonObject, const std::shared_ptr< ObservationSimu { return; } - } //! Create a `json` object from a shared pointer to a `ObservationViabilitySettings` object. -void to_json( nlohmann::json& jsonObject, const std::shared_ptr< ObservationViabilitySettings >& observationViabilitySettings ) +void to_json( nlohmann::json& jsonObject, + const std::shared_ptr< ObservationViabilitySettings >& observationViabilitySettings ) { - if ( !observationViabilitySettings ) + if( !observationViabilitySettings ) { return; } using namespace json_interface; using K = Keys::Observation; - const ObservationViabilityType viabilityType = observationViabilitySettings->observationViabilityType_; + const ObservationViabilityType viabilityType = + observationViabilitySettings->observationViabilityType_; jsonObject[ K::observableViabilityType ] = viabilityType; jsonObject[ K::associatedLinkEnd ] = observationViabilitySettings->getAssociatedLinkEnd( ); - if( observationViabilitySettings->getDoubleParameter( ) == observationViabilitySettings->getDoubleParameter( ) ) + if( observationViabilitySettings->getDoubleParameter( ) == + observationViabilitySettings->getDoubleParameter( ) ) { jsonObject[ K::doubleParameter ] = observationViabilitySettings->getDoubleParameter( ); } @@ -517,43 +535,39 @@ void to_json( nlohmann::json& jsonObject, const std::shared_ptr< ObservationViab } } +} // namespace observation_models - -} - -} // namespace tudat +} // namespace tudat namespace boost { template<> -tudat::observation_models::ObservableType lexical_cast( const std::string & s ) +tudat::observation_models::ObservableType lexical_cast( const std::string& s ) { return tudat::observation_models::observationTypesInverse.at( s ); } template<> -std::string lexical_cast(const tudat::observation_models::ObservableType & component ) +std::string lexical_cast( const tudat::observation_models::ObservableType& component ) { return tudat::observation_models::observationTypes.at( component ); } template<> -tudat::observation_models::LinkEndType lexical_cast( const std::string & s ) +tudat::observation_models::LinkEndType lexical_cast( const std::string& s ) { return tudat::observation_models::linkEndTypesInverse.at( s ); } - template<> -std::string lexical_cast(const tudat::observation_models::LinkEndType & component ) +std::string lexical_cast( const tudat::observation_models::LinkEndType& component ) { return tudat::observation_models::linkEndTypes.at( component ); } - template<> -tudat::observation_models::LinkEnds lexical_cast( const std::string & s ) +tudat::observation_models::LinkEnds lexical_cast( const std::string& s ) { tudat::observation_models::LinkEnds linkEnds; std::vector< std::string > individualLinkEnds = tudat::json_interface::split( s, ';' ); @@ -562,9 +576,10 @@ tudat::observation_models::LinkEnds lexical_cast( const std::string & s ) std::vector< std::string > typeAndIdentifier = tudat::json_interface::split( individualLinkEnds.at( i ), ':' ); - std::vector< std::string > splitLinkEnd = - tudat::json_interface::split( - typeAndIdentifier.at( 1 ).substr(1, typeAndIdentifier.at( 1 ).size( ) - 2 ), ',', false ); + std::vector< std::string > splitLinkEnd = tudat::json_interface::split( + typeAndIdentifier.at( 1 ).substr( 1, typeAndIdentifier.at( 1 ).size( ) - 2 ), + ',', + false ); if( splitLinkEnd.size( ) == 1 ) { splitLinkEnd.push_back( "" ); @@ -578,7 +593,7 @@ tudat::observation_models::LinkEnds lexical_cast( const std::string & s ) } template<> -std::string lexical_cast(const tudat::observation_models::LinkEnds & component ) +std::string lexical_cast( const tudat::observation_models::LinkEnds& component ) { std::string linkEndsString; @@ -589,9 +604,10 @@ std::string lexical_cast(const tudat::observation_models::LinkEnds & component ) linkEndsString += "; "; } - linkEndsString += boost::lexical_cast< std::string >( it->first ) + ":(" + it->second.first + "," + it->second.second + ")"; + linkEndsString += boost::lexical_cast< std::string >( it->first ) + ":(" + + it->second.first + "," + it->second.second + ")"; } return linkEndsString; } -} +} // namespace boost diff --git a/src/interface/json/support/keys.cpp b/src/interface/json/support/keys.cpp index 8aefa62188..f08768013d 100644 --- a/src/interface/json/support/keys.cpp +++ b/src/interface/json/support/keys.cpp @@ -9,10 +9,11 @@ * */ +#include "tudat/interface/json/support/keys.h" + #include #include "tudat/interface/json/support/utilities.h" -#include "tudat/interface/json/support/keys.h" namespace tudat { @@ -28,30 +29,22 @@ const std::string SpecialKeys::up = "<-"; const std::string SpecialKeys::rootObject = "#root"; const std::string SpecialKeys::keyPath = "#keypath"; -const std::vector< std::string > SpecialKeys::objectContaining = -{ - SpecialKeys::rootObject, - SpecialKeys::keyPath -}; - -const std::vector< std::string > SpecialKeys::all = -{ - SpecialKeys::root, - SpecialKeys::up, - SpecialKeys::rootObject, - SpecialKeys::keyPath -}; +const std::vector< std::string > SpecialKeys::objectContaining = { SpecialKeys::rootObject, + SpecialKeys::keyPath }; +const std::vector< std::string > SpecialKeys::all = { SpecialKeys::root, + SpecialKeys::up, + SpecialKeys::rootObject, + SpecialKeys::keyPath }; // Keys recognised by json_interface. - const std::string Keys::simulationType = "simulationType"; +const std::string Keys::simulationType = "simulationType"; const std::string Keys::initialEpoch = "initialEpoch"; const std::string Keys::finalEpoch = "finalEpoch"; const std::string Keys::globalFrameOrigin = "globalFrameOrigin"; const std::string Keys::globalFrameOrientation = "globalFrameOrientation"; - // Spice const std::string Keys::spice = "spice"; const std::string Keys::Spice::useStandardKernels = "useStandardKernels"; @@ -61,7 +54,6 @@ const std::string Keys::Spice::preloadEphemeris = "preloadEphemeris"; const std::string Keys::Spice::interpolationOffsets = "interpolationOffsets"; const std::string Keys::Spice::interpolationStep = "interpolationStep"; - // Body const std::string Keys::bodies = "bodies"; @@ -82,7 +74,8 @@ const std::string Keys::Body::State::vx = "vx"; const std::string Keys::Body::State::vy = "vy"; const std::string Keys::Body::State::vz = "vz"; // Keplerian -const std::string Keys::Body::State::centralBodyGravitationalParameter = "centralBodyGravitationalParameter"; +const std::string Keys::Body::State::centralBodyGravitationalParameter = + "centralBodyGravitationalParameter"; const std::string Keys::Body::State::centralBodyAverageRadius = "centralBodyAverageRadius"; const std::string Keys::Body::State::semiMajorAxis = "semiMajorAxis"; const std::string Keys::Body::State::eccentricity = "eccentricity"; @@ -109,7 +102,6 @@ const std::string Keys::Body::State::speed = "speed"; const std::string Keys::Body::State::flightPathAngle = "flightPathAngle"; const std::string Keys::Body::State::headingAngle = "headingAngle"; - // // Body::aerodynamics const std::string Keys::Body::aerodynamics = "aerodynamics"; const std::string Keys::Body::Aerodynamics::coefficientsType = "coefficientsType"; @@ -118,8 +110,10 @@ const std::string Keys::Body::Aerodynamics::referenceArea = "referenceArea"; const std::string Keys::Body::Aerodynamics::lateralReferenceLength = "lateralReferenceLength"; const std::string Keys::Body::Aerodynamics::momentReferencePoint = "momentReferencePoint"; const std::string Keys::Body::Aerodynamics::independentVariableNames = "independentVariableNames"; -const std::string Keys::Body::Aerodynamics::areCoefficientsInAerodynamicFrame = "areCoefficientsInAerodynamicFrame"; -const std::string Keys::Body::Aerodynamics::areCoefficientsInNegativeAxisDirection = "areCoefficientsInNegativeAxisDirection"; +const std::string Keys::Body::Aerodynamics::areCoefficientsInAerodynamicFrame = + "areCoefficientsInAerodynamicFrame"; +const std::string Keys::Body::Aerodynamics::areCoefficientsInNegativeAxisDirection = + "areCoefficientsInNegativeAxisDirection"; const std::string Keys::Body::Aerodynamics::controlSurface = "controlSurface"; // Constant const std::string Keys::Body::Aerodynamics::dragCoefficient = "dragCoefficient"; @@ -130,7 +124,6 @@ const std::string Keys::Body::Aerodynamics::independentVariableValues = "indepen // Tabulated< 1 > const std::string Keys::Body::Aerodynamics::interpolator = "interpolator"; - // // Body::Atmosphere const std::string Keys::Body::atmosphere = "atmosphere"; const std::string Keys::Body::Atmosphere::type = "type"; @@ -151,8 +144,10 @@ const std::string Keys::Body::Ephemeris::type = "type"; const std::string Keys::Body::Ephemeris::frameOrigin = "frameOrigin"; const std::string Keys::Body::Ephemeris::frameOrientation = "frameOrientation"; // const std::string Keys::Body::Ephemeris::makeMultiArc = "makeMultiArc"; -const std::string Keys::Body::Ephemeris::correctForStellarAberration = "correctForStellarAberration"; -const std::string Keys::Body::Ephemeris::correctForLightTimeAberration = "correctForLightTimeAberration"; +const std::string Keys::Body::Ephemeris::correctForStellarAberration = + "correctForStellarAberration"; +const std::string Keys::Body::Ephemeris::correctForLightTimeAberration = + "correctForLightTimeAberration"; const std::string Keys::Body::Ephemeris::convergeLighTimeAberration = "convergeLighTimeAberration"; const std::string Keys::Body::Ephemeris::initialTime = "initialTime"; const std::string Keys::Body::Ephemeris::finalTime = "finalTime"; @@ -160,14 +155,19 @@ const std::string Keys::Body::Ephemeris::timeStep = "timeStep"; const std::string Keys::Body::Ephemeris::interpolator = "interpolator"; const std::string Keys::Body::Ephemeris::useLongDoubleStates = "useLongDoubleStates"; const std::string Keys::Body::Ephemeris::bodyIdentifier = "bodyIdentifier"; -const std::string Keys::Body::Ephemeris::useCircularCoplanarApproximation = "useCircularCoplanarApproximation"; +const std::string Keys::Body::Ephemeris::useCircularCoplanarApproximation = + "useCircularCoplanarApproximation"; const std::string Keys::Body::Ephemeris::constantState = "constantState"; // const std::string Keys::Body::Ephemeris::customStateFunction = "customStateFunction"; -const std::string Keys::Body::Ephemeris::initialStateInKeplerianElements = "initialStateInKeplerianElements"; +const std::string Keys::Body::Ephemeris::initialStateInKeplerianElements = + "initialStateInKeplerianElements"; const std::string Keys::Body::Ephemeris::epochOfInitialState = "epochOfInitialState"; -const std::string Keys::Body::Ephemeris::centralBodyGravitationalParameter = "centralBodyGravitationalParameter"; -const std::string Keys::Body::Ephemeris::rootFinderAbsoluteTolerance = "rootFinderAbsoluteTolerance"; -const std::string Keys::Body::Ephemeris::rootFinderMaximumNumberOfIterations = "rootFinderMaximumNumberOfIterations"; +const std::string Keys::Body::Ephemeris::centralBodyGravitationalParameter = + "centralBodyGravitationalParameter"; +const std::string Keys::Body::Ephemeris::rootFinderAbsoluteTolerance = + "rootFinderAbsoluteTolerance"; +const std::string Keys::Body::Ephemeris::rootFinderMaximumNumberOfIterations = + "rootFinderMaximumNumberOfIterations"; const std::string Keys::Body::Ephemeris::bodyStateHistory = "bodyStateHistory"; // // Body::GravityField @@ -182,7 +182,8 @@ const std::string Keys::Body::GravityField::model = "model"; const std::string Keys::Body::GravityField::file = "file"; const std::string Keys::Body::GravityField::maximumDegree = "maximumDegree"; const std::string Keys::Body::GravityField::maximumOrder = "maximumOrder"; -const std::string Keys::Body::GravityField::gravitationalParameterIndex = "gravitationalParameterIndex"; +const std::string Keys::Body::GravityField::gravitationalParameterIndex = + "gravitationalParameterIndex"; const std::string Keys::Body::GravityField::referenceRadiusIndex = "referenceRadiusIndex"; // // Body::RadiationPressure @@ -191,7 +192,8 @@ const std::string Keys::Body::RadiationPressure::type = "type"; const std::string Keys::Body::RadiationPressure::sourceBody = "sourceBody"; const std::string Keys::Body::RadiationPressure::occultingBodies = "occultingBodies"; const std::string Keys::Body::RadiationPressure::referenceArea = "referenceArea"; -const std::string Keys::Body::RadiationPressure::radiationPressureCoefficient = "radiationPressureCoefficient"; +const std::string Keys::Body::RadiationPressure::radiationPressureCoefficient = + "radiationPressureCoefficient"; // // Body::RotationModel const std::string Keys::Body::rotationModel = "rotationModel"; @@ -204,7 +206,6 @@ const std::string Keys::Body::RotationModel::rotationRate = "rotationRate"; const std::string Keys::Body::RotationModel::precessionNutationTheory = "precessionNutationTheory"; const std::string Keys::Body::RotationModel::centralBodyName = "centralBodyName"; - // // Body::ShapeModel const std::string Keys::Body::shapeModel = "shapeModel"; const std::string Keys::Body::ShapeModel::type = "type"; @@ -212,15 +213,16 @@ const std::string Keys::Body::ShapeModel::radius = "radius"; const std::string Keys::Body::ShapeModel::equatorialRadius = "equatorialRadius"; const std::string Keys::Body::ShapeModel::flattening = "flattening"; - // // Body::GravityFieldVariation const std::string Keys::Body::gravityFieldVariation = "gravityFieldVariation"; const std::string Keys::Body::GravityFieldVariation::bodyDeformationType = "bodyDeformationType"; const std::string Keys::Body::GravityFieldVariation::deformingBodies = "deformingBodies"; const std::string Keys::Body::GravityFieldVariation::loveNumbers = "loveNumbers"; const std::string Keys::Body::GravityFieldVariation::modelInterpolation = "modelInterpolation"; -const std::string Keys::Body::GravityFieldVariation::cosineCoefficientCorrections = "cosineCoefficientCorrections"; -const std::string Keys::Body::GravityFieldVariation::sineCoefficientCorrections = "sineCoefficientCorrections"; +const std::string Keys::Body::GravityFieldVariation::cosineCoefficientCorrections = + "cosineCoefficientCorrections"; +const std::string Keys::Body::GravityFieldVariation::sineCoefficientCorrections = + "sineCoefficientCorrections"; const std::string Keys::Body::GravityFieldVariation::minimumDegree = "minimumDegree"; const std::string Keys::Body::GravityFieldVariation::minimumOrder = "minimumOrder"; const std::string Keys::Body::GravityFieldVariation::interpolator = "interpolator"; @@ -240,10 +242,10 @@ const std::string Keys::Variable::componentIndices = "componentIndices"; const std::string Keys::Variable::useAccelerationNorm = "useAccelerationNorm"; const std::string Keys::Variable::relativeToBody = "relativeToBody"; const std::string Keys::Variable::accelerationType = "accelerationType"; -//const std::string Keys::Variable::bodyUndergoingAcceleration = "bodyUndergoingAcceleration"; +// const std::string Keys::Variable::bodyUndergoingAcceleration = "bodyUndergoingAcceleration"; const std::string Keys::Variable::bodyExertingAcceleration = "bodyExertingAcceleration"; const std::string Keys::Variable::torqueType = "torqueType"; -//const std::string Keys::Variable::bodyUndergoingTorque = "bodyUndergoingTorque"; +// const std::string Keys::Variable::bodyUndergoingTorque = "bodyUndergoingTorque"; const std::string Keys::Variable::bodyExertingTorque = "bodyExertingTorque"; const std::string Keys::Variable::baseFrame = "baseFrame"; const std::string Keys::Variable::targetFrame = "targetFrame"; @@ -253,7 +255,6 @@ const std::string Keys::Variable::identifier = "identifier"; const std::string Keys::Variable::derivativeWrtBody = "derivativeWrtBody"; const std::string Keys::Variable::thirdBody = "thirdBody"; - // Parameter const std::string Keys::parametersToEstimate = "parametersToEstimate"; @@ -287,17 +288,21 @@ const std::string Keys::Parameter::useComplexValue = "useComplexValue"; const std::string Keys::estimationSettings = "estimation"; const std::string Keys::Estimation::inverseAprioriCovariance = "inverseAprioriCovariance"; -const std::string Keys::Estimation::reintegrateEquationsOnFirstIteration = "reintegrateOnFirstIteration"; -const std::string Keys::Estimation::reintegrateVariationalEquations = "reintegrateVariationalEquations"; +const std::string Keys::Estimation::reintegrateEquationsOnFirstIteration = + "reintegrateOnFirstIteration"; +const std::string Keys::Estimation::reintegrateVariationalEquations = + "reintegrateVariationalEquations"; const std::string Keys::Estimation::saveInformationMatrix = "savePartials"; const std::string Keys::Estimation::printOutput = "printOutput"; -const std::string Keys::Estimation::saveResidualsAndParametersFromEachIteration = "saveEstimationHistory"; +const std::string Keys::Estimation::saveResidualsAndParametersFromEachIteration = + "saveEstimationHistory"; const std::string Keys::Estimation::saveStateHistoryForEachIteration = "saveStateHistory"; const std::string Keys::Estimation::maximumNumberOfIterations = "maximumIteration"; const std::string Keys::Estimation::minimumResidualChange = "minimumResidualChange"; const std::string Keys::Estimation::minimumResidual = "minimumResidual"; -const std::string Keys::Estimation::numberOfIterationsWithoutImprovement = "numberOfUnimprovedIterations"; +const std::string Keys::Estimation::numberOfIterationsWithoutImprovement = + "numberOfUnimprovedIterations"; const std::string Keys::Estimation::dataWeights = "dataWeights"; @@ -307,12 +312,13 @@ const std::string Keys::Observation::observableType = "observableType"; const std::string Keys::Observation::lightTimeCorrectionSettingsList = "lightTimeCorrections"; const std::string Keys::Observation::biasSettings = "bias"; -const std::string Keys::Observation::transmitterProperTimeRateSettings = "transmitterProperTimeRate"; +const std::string Keys::Observation::transmitterProperTimeRateSettings = + "transmitterProperTimeRate"; const std::string Keys::Observation::receiverProperTimeRateSettings = "receiverProperTimeRate"; const std::string Keys::Observation::constantIntegrationTime = "constantIntegrationTime"; -const std::string Keys::Observation::oneWayRangeObsevationSettings = "oneWayRangeObsevation"; +const std::string Keys::Observation::oneWayRangeObservationSettings = "oneWayRangeObsevation"; const std::string Keys::Observation::retransmissionTimes = "retransmissionTimes"; const std::string Keys::Observation::uplinkOneWayDopplerSettings = "uplinkOneWayDoppler"; @@ -324,15 +330,16 @@ const std::string Keys::Observation::centralBody = "centralBody"; const std::string Keys::Observation::lightTimeCorrectionType = "lightTimeCorrectionType"; const std::string Keys::Observation::perturbingBodies = "perturbingBodies"; -const std::string Keys::Observation::observationSimulationTimesType = "observationSimulationTimesType"; -const std::string Keys::Observation::observationSimulationTimesList = "observationSimulationTimesList"; +const std::string Keys::Observation::observationSimulationTimesType = + "observationSimulationTimesType"; +const std::string Keys::Observation::observationSimulationTimesList = + "observationSimulationTimesList"; const std::string Keys::Observation::observableViabilityType = "viabilityType"; const std::string Keys::Observation::associatedLinkEnd = "associatedLinkEnd"; const std::string Keys::Observation::doubleParameter = "doubleParameter"; const std::string Keys::Observation::stringParameter = "stringParameter"; - // ObservationBias const std::string Keys::ObservationBias::biasType = "biasType"; const std::string Keys::ObservationBias::multipleBiasesList = "multipleBiasesList"; @@ -342,7 +349,6 @@ const std::string Keys::ObservationBias::arcWiseBiasList = "arcWiseBiasList"; const std::string Keys::ObservationBias::arcStartTimes = "arcStartTimes"; const std::string Keys::ObservationBias::referenceLinkEnd = "referenceLinkEnd"; - // Propagator const std::string Keys::propagators = "propagators"; @@ -359,17 +365,27 @@ const std::string Keys::Propagator::accelerations = "accelerations"; const std::string Keys::Propagator::Acceleration::type = "type"; const std::string Keys::Propagator::Acceleration::maximumDegree = "maximumDegree"; const std::string Keys::Propagator::Acceleration::maximumOrder = "maximumOrder"; -const std::string Keys::Propagator::Acceleration::maximumDegreeOfBodyExertingAcceleration = "maximumDegreeOfBodyExertingAcceleration"; -const std::string Keys::Propagator::Acceleration::maximumOrderOfBodyExertingAcceleration = "maximumOrderOfBodyExertingAcceleration"; -const std::string Keys::Propagator::Acceleration::maximumDegreeOfBodyUndergoingAcceleration = "maximumDegreeOfBodyUndergoingAcceleration"; -const std::string Keys::Propagator::Acceleration::maximumOrderOfBodyUndergoingAcceleration = "maximumOrderOfBodyUndergoingAcceleration"; -const std::string Keys::Propagator::Acceleration::maximumDegreeOfCentralBody = "maximumDegreeOfCentralBody"; -const std::string Keys::Propagator::Acceleration::maximumOrderOfCentralBody = "maximumOrderOfCentralBody"; -const std::string Keys::Propagator::Acceleration::calculateSchwarzschildCorrection = "calculateSchwarzschildCorrection"; -const std::string Keys::Propagator::Acceleration::calculateLenseThirringCorrection = "calculateLenseThirringCorrection"; -const std::string Keys::Propagator::Acceleration::calculateDeSitterCorrection = "calculateDeSitterCorrection"; +const std::string Keys::Propagator::Acceleration::maximumDegreeOfBodyExertingAcceleration = + "maximumDegreeOfBodyExertingAcceleration"; +const std::string Keys::Propagator::Acceleration::maximumOrderOfBodyExertingAcceleration = + "maximumOrderOfBodyExertingAcceleration"; +const std::string Keys::Propagator::Acceleration::maximumDegreeOfBodyUndergoingAcceleration = + "maximumDegreeOfBodyUndergoingAcceleration"; +const std::string Keys::Propagator::Acceleration::maximumOrderOfBodyUndergoingAcceleration = + "maximumOrderOfBodyUndergoingAcceleration"; +const std::string Keys::Propagator::Acceleration::maximumDegreeOfCentralBody = + "maximumDegreeOfCentralBody"; +const std::string Keys::Propagator::Acceleration::maximumOrderOfCentralBody = + "maximumOrderOfCentralBody"; +const std::string Keys::Propagator::Acceleration::calculateSchwarzschildCorrection = + "calculateSchwarzschildCorrection"; +const std::string Keys::Propagator::Acceleration::calculateLenseThirringCorrection = + "calculateLenseThirringCorrection"; +const std::string Keys::Propagator::Acceleration::calculateDeSitterCorrection = + "calculateDeSitterCorrection"; const std::string Keys::Propagator::Acceleration::primaryBody = "primaryBody"; -const std::string Keys::Propagator::Acceleration::centralBodyAngularMomentum = "centralBodyAngularMomentum"; +const std::string Keys::Propagator::Acceleration::centralBodyAngularMomentum = + "centralBodyAngularMomentum"; const std::string Keys::Propagator::Acceleration::constantAcceleration = "constantAcceleration"; const std::string Keys::Propagator::Acceleration::sineAcceleration = "sineAcceleration"; const std::string Keys::Propagator::Acceleration::cosineAcceleration = "cosineAcceleration"; @@ -380,37 +396,40 @@ const std::string Keys::Propagator::Acceleration::cosineAcceleration = "cosineAc const std::string Keys::Propagator::Acceleration::Thrust::direction = "direction"; const std::string Keys::Propagator::Acceleration::Thrust::Direction::type = "type"; const std::string Keys::Propagator::Acceleration::Thrust::Direction::relativeBody = "relativeBody"; -const std::string Keys::Propagator::Acceleration::Thrust::Direction::colinearWithVelocity = "colinearWithVelocity"; -const std::string Keys::Propagator::Acceleration::Thrust::Direction::towardsRelativeBody = "towardsRelativeBody"; +const std::string Keys::Propagator::Acceleration::Thrust::Direction::colinearWithVelocity = + "colinearWithVelocity"; +const std::string Keys::Propagator::Acceleration::Thrust::Direction::towardsRelativeBody = + "towardsRelativeBody"; // // // // Acceleration::Thrust::Direction const std::string Keys::Propagator::Acceleration::Thrust::magnitude = "magnitude"; const std::string Keys::Propagator::Acceleration::Thrust::Magnitude::type = "type"; const std::string Keys::Propagator::Acceleration::Thrust::Magnitude::originID = "originID"; -const std::string Keys::Propagator::Acceleration::Thrust::Magnitude::constantMagnitude = "constantMagnitude"; -const std::string Keys::Propagator::Acceleration::Thrust::Magnitude::specificImpulse = "specificImpulse"; -const std::string Keys::Propagator::Acceleration::Thrust::Magnitude::bodyFixedDirection = "bodyFixedDirection"; -const std::string Keys::Propagator::Acceleration::Thrust::Magnitude::useAllEngines = "useAllEngines"; +const std::string Keys::Propagator::Acceleration::Thrust::Magnitude::constantMagnitude = + "constantMagnitude"; +const std::string Keys::Propagator::Acceleration::Thrust::Magnitude::specificImpulse = + "specificImpulse"; +const std::string Keys::Propagator::Acceleration::Thrust::Magnitude::bodyFixedDirection = + "bodyFixedDirection"; +const std::string Keys::Propagator::Acceleration::Thrust::Magnitude::useAllEngines = + "useAllEngines"; const std::string Keys::Propagator::Acceleration::Thrust::dataInterpolation = "dataInterpolation"; const std::string Keys::Propagator::Acceleration::Thrust::specificImpulse = "specificImpulse"; const std::string Keys::Propagator::Acceleration::Thrust::frame = "frame"; const std::string Keys::Propagator::Acceleration::Thrust::centralBody = "centralBody"; - // // Mass rate model const std::string Keys::Propagator::massRateModels = "massRateModels"; const std::string Keys::Propagator::MassRateModel::type = "type"; const std::string Keys::Propagator::MassRateModel::useAllThrustModels = "useAllThrustModels"; -const std::string Keys::Propagator::MassRateModel::associatedThrustSource = "associatedThrustSource"; - +const std::string Keys::Propagator::MassRateModel::associatedThrustSource = + "associatedThrustSource"; // // Torque const std::string Keys::Propagator::torques = "torques"; const std::string Keys::Propagator::Torque::type = "type"; - - // Termination const std::string Keys::termination = "termination"; const std::string Keys::Termination::anyOf = "anyOf"; @@ -419,7 +438,6 @@ const std::string Keys::Termination::variable = "variable"; const std::string Keys::Termination::lowerLimit = "lowerLimit"; const std::string Keys::Termination::upperLimit = "upperLimit"; - // Integrator const std::string Keys::integrator = "integrator"; const std::string Keys::Integrator::type = "type"; @@ -435,8 +453,10 @@ const std::string Keys::Integrator::relativeErrorTolerance = "relativeErrorToler const std::string Keys::Integrator::absoluteErrorTolerance = "absoluteErrorTolerance"; const std::string Keys::Integrator::areTolerancesDefinedAsScalar = "areTolerancesDefinedAsScalar"; const std::string Keys::Integrator::safetyFactorForNextStepSize = "safetyFactorForNextStepSize"; -const std::string Keys::Integrator::maximumFactorIncreaseForNextStepSize = "maximumFactorIncreaseForNextStepSize"; -const std::string Keys::Integrator::minimumFactorDecreaseForNextStepSize = "minimumFactorDecreaseForNextStepSize"; +const std::string Keys::Integrator::maximumFactorIncreaseForNextStepSize = + "maximumFactorIncreaseForNextStepSize"; +const std::string Keys::Integrator::minimumFactorDecreaseForNextStepSize = + "minimumFactorDecreaseForNextStepSize"; const std::string Keys::Integrator::bandwidth = "bandwidth"; const std::string Keys::Integrator::extrapolationSequence = "extrapolationSequence"; const std::string Keys::Integrator::maximumNumberOfSteps = "maximumNumberOfSteps"; @@ -448,17 +468,21 @@ const std::string Keys::Integrator::minimumOrder = "minimumOrder"; // // Interpolation::DataMap const std::string Keys::Interpolation::DataMap::map = "map"; const std::string Keys::Interpolation::DataMap::file = "file"; -const std::string Keys::Interpolation::DataMap::independentVariableValues = "independentVariableValues"; +const std::string Keys::Interpolation::DataMap::independentVariableValues = + "independentVariableValues"; const std::string Keys::Interpolation::DataMap::dependentVariableValues = "dependentVariableValues"; -const std::string Keys::Interpolation::DataMap::dependentVariableFirstDerivativeValues = "dependentVariableFirstDerivativeValues"; +const std::string Keys::Interpolation::DataMap::dependentVariableFirstDerivativeValues = + "dependentVariableFirstDerivativeValues"; // // Interpolation::Interpolator const std::string Keys::Interpolation::Interpolator::type = "type"; const std::string Keys::Interpolation::Interpolator::lookupScheme = "lookupScheme"; -const std::string Keys::Interpolation::Interpolator::useLongDoubleTimeStep = "useLongDoubleTimeStep"; +const std::string Keys::Interpolation::Interpolator::useLongDoubleTimeStep = + "useLongDoubleTimeStep"; const std::string Keys::Interpolation::Interpolator::order = "order"; const std::string Keys::Interpolation::Interpolator::boundaryHandling = "boundaryHandling"; -const std::string Keys::Interpolation::Interpolator::lagrangeBoundaryHandling = "lagrangeBoundaryHandling"; +const std::string Keys::Interpolation::Interpolator::lagrangeBoundaryHandling = + "lagrangeBoundaryHandling"; // // Interpolation::DataInterpolation const std::string Keys::Interpolation::DataInterpolation::data = "data"; @@ -470,7 +494,6 @@ const std::string Keys::Interpolation::ModelInterpolation::finalTime = "finalTim const std::string Keys::Interpolation::ModelInterpolation::timeStep = "timeStep"; const std::string Keys::Interpolation::ModelInterpolation::interpolator = "interpolator"; - // Export const std::string Keys::xport = "export"; const std::string Keys::Export::file = "file"; @@ -490,8 +513,8 @@ const std::string Keys::Options::printInterval = "printInterval"; const std::string Keys::Options::defaultValueUsedForMissingKey = "defaultValueUsedForMissingKey"; const std::string Keys::Options::unusedKey = "unusedKey"; const std::string Keys::Options::fullSettingsFile = "fullSettingsFile"; -const std::string Keys::Options::tagOutputFilesIfPropagationFails = "tagOutputFilesIfPropagationFails"; - +const std::string Keys::Options::tagOutputFilesIfPropagationFails = + "tagOutputFilesIfPropagationFails"; // KEYPATH @@ -500,13 +523,14 @@ int indexFromKey( const std::string& key ) { boost::cmatch groups; boost::regex_match( key.c_str( ), groups, boost::regex( R"(\@(\d+))" ) ); - if ( groups[ 1 ].matched ) + if( groups[ 1 ].matched ) { try { return std::stoi( groups[ 1 ] ); } - catch ( ... ) { } + catch( ... ) + { } } return -1; } @@ -515,11 +539,11 @@ int indexFromKey( const std::string& key ) KeyPath::KeyPath( const std::string& keyPathStringRepresentation ) : std::vector< std::string >( ) { const std::vector< std::string > keys = split( keyPathStringRepresentation, SpecialKeys::dot ); - for ( const std::string key : keys ) + for( const std::string key: keys ) { boost::cmatch groups; boost::regex_match( key.c_str( ), groups, boost::regex( R"((.+?)\[(\d+?)\])" ) ); - if ( groups[ 1 ].matched && groups[ 2 ].matched ) + if( groups[ 1 ].matched && groups[ 2 ].matched ) { const std::string arrayKey( groups[ 1 ] ); const std::string arrayIndex( groups[ 2 ] ); @@ -534,22 +558,22 @@ KeyPath::KeyPath( const std::string& keyPathStringRepresentation ) : std::vector } //! String representation for `KeyPath`, as key.subkey.vectorIndex.subsubkey ... -std::ostream& operator << ( std::ostream& stringRepresentation, const KeyPath& keyPath ) +std::ostream& operator<<( std::ostream& stringRepresentation, const KeyPath& keyPath ) { bool somethingAdded = false; - for ( unsigned int i = 0; i < keyPath.size( ); ++i ) + for( unsigned int i = 0; i < keyPath.size( ); ++i ) { const std::string key = keyPath.at( i ); - if ( key != SpecialKeys::root ) + if( key != SpecialKeys::root ) { const int intKey = indexFromKey( key ); - if ( intKey >= 0 ) + if( intKey >= 0 ) { stringRepresentation << '[' << intKey << ']'; } else { - if ( somethingAdded ) + if( somethingAdded ) { stringRepresentation << SpecialKeys::dot; } @@ -568,7 +592,7 @@ KeyPath KeyPath::canonical( const KeyPath& basePath ) const KeyPath compoundKeyPath; // Check absolute paths - if ( ! this->isAbsolute( ) && basePath.isAbsolute( ) ) + if( !this->isAbsolute( ) && basePath.isAbsolute( ) ) { compoundKeyPath = basePath / *this; } @@ -579,11 +603,11 @@ KeyPath KeyPath::canonical( const KeyPath& basePath ) const // Remove .. KeyPath canonicalKeyPath = { }; - for ( std::string key : compoundKeyPath ) + for( std::string key: compoundKeyPath ) { - if ( key == SpecialKeys::up ) + if( key == SpecialKeys::up ) { - if ( canonicalKeyPath.size( ) > 0 ) + if( canonicalKeyPath.size( ) > 0 ) { canonicalKeyPath.pop_back( ); } @@ -597,7 +621,6 @@ KeyPath KeyPath::canonical( const KeyPath& basePath ) const return canonicalKeyPath; } +} // namespace json_interface -} // namespace json_interface - -} // namespace tudat +} // namespace tudat diff --git a/tests/src/interface/json/inputs/unitTestObservation/observationOutput.json b/tests/src/interface/json/inputs/unitTestObservation/observationOutput.json index d6c1e1a843..9058f8231d 100644 --- a/tests/src/interface/json/inputs/unitTestObservation/observationOutput.json +++ b/tests/src/interface/json/inputs/unitTestObservation/observationOutput.json @@ -192,7 +192,7 @@ "transmitter:(Vehicle,); reflector1:(Earth,Station1); receiver:(Vehicle,)": [ { "observableType": "nWayRange", - "oneWayRangeObsevation": [ + "oneWayRangeObservation": [ { "observableType": "oneWayRange" }, @@ -205,7 +205,7 @@ "transmitter:(Vehicle,); reflector1:(Earth,Station2); receiver:(Vehicle,)": [ { "observableType": "nWayRange", - "oneWayRangeObsevation": [ + "oneWayRangeObservation": [ { "lightTimeCorrections": [ { diff --git a/tests/src/interface/json/unitTestObservation.cpp b/tests/src/interface/json/unitTestObservation.cpp index 31f09d7657..7c4e16b152 100644 --- a/tests/src/interface/json/unitTestObservation.cpp +++ b/tests/src/interface/json/unitTestObservation.cpp @@ -11,14 +11,13 @@ #define BOOST_TEST_DYN_LINK #define BOOST_TEST_MAIN -#include "tudat/interface/json/tests/unitTestSupport.h" -#include "tudat/interface/json/propagation/acceleration.h" #include "tudat/interface/json/estimation/observation.h" #include "tudat/interface/json/estimation/parameter.h" +#include "tudat/interface/json/propagation/acceleration.h" #include "tudat/interface/json/support/valueConversions.h" - -#include "tudat/simulation/estimation_setup/createObservationModel.h" +#include "tudat/interface/json/tests/unitTestSupport.h" #include "tudat/simulation/estimation_setup/createEstimatableParameters.h" +#include "tudat/simulation/estimation_setup/createObservationModel.h" #include "tudat/simulation/propagation_setup/createAccelerationModels.h" namespace tudat @@ -27,8 +26,9 @@ namespace tudat namespace unit_tests { -#define INPUT( filename ) \ - ( json_interface::inputDirectory( ) / boost::filesystem::path( __FILE__ ).stem( ) / filename ).string( ) +#define INPUT( filename ) \ + ( json_interface::inputDirectory( ) / boost::filesystem::path( __FILE__ ).stem( ) / filename ) \ + .string( ) BOOST_AUTO_TEST_SUITE( test_json_observation ) @@ -37,7 +37,7 @@ BOOST_AUTO_TEST_CASE( test_json_acceleration_sphericalHarmonicGravity ) { using namespace tudat; using namespace tudat::simulation_setup; - //using namespace tudat::unit_tests; + // using namespace tudat::unit_tests; using namespace tudat::simulation_setup; using namespace tudat::observation_models; @@ -50,8 +50,8 @@ BOOST_AUTO_TEST_CASE( test_json_acceleration_sphericalHarmonicGravity ) groundStationNames.push_back( "Station2" ); groundStationNames.push_back( "Station3" ); - - // Create list of link ends in which station is receiver and in which station is transmitter (with spacecraft other link end). + // Create list of link ends in which station is receiver and in which station is transmitter + // (with spacecraft other link end). std::vector< LinkEnds > stationReceiverLinkEnds; std::vector< LinkEnds > stationTransmitterLinkEnds; std::vector< LinkEnds > twoWayLinkEnds; @@ -76,7 +76,8 @@ BOOST_AUTO_TEST_CASE( test_json_acceleration_sphericalHarmonicGravity ) twoWayLinkEnds.push_back( linkEnds ); } - // Define (arbitrarily) link ends to be used for 1-way range, 1-way doppler and angular position observables + // Define (arbitrarily) link ends to be used for 1-way range, 1-way doppler and angular position + // observables std::map< ObservableType, std::vector< LinkEnds > > linkEndsPerObservable; linkEndsPerObservable[ one_way_range ].push_back( stationReceiverLinkEnds[ 0 ] ); linkEndsPerObservable[ one_way_range ].push_back( stationTransmitterLinkEnds[ 0 ] ); @@ -98,12 +99,16 @@ BOOST_AUTO_TEST_CASE( test_json_acceleration_sphericalHarmonicGravity ) // n_way_range = 5, // two_way_doppler = 6 - // Iterate over all observable types and associated link ends, and creatin settings for observation + // Iterate over all observable types and associated link ends, and creatin settings for + // observation observation_models::ObservationSettingsListPerLinkEnd observationSettingsMap; - //std::map< LinkEnds, std::vector< std::shared_ptr < ObservationSettings > > > observationSettingsMap; + // std::map< LinkEnds, std::vector< std::shared_ptr < ObservationSettings > > > + // observationSettingsMap; - for( std::map< ObservableType, std::vector< LinkEnds > >::iterator linkEndIterator = linkEndsPerObservable.begin( ); - linkEndIterator != linkEndsPerObservable.end( ); linkEndIterator++ ) + for( std::map< ObservableType, std::vector< LinkEnds > >::iterator linkEndIterator = + linkEndsPerObservable.begin( ); + linkEndIterator != linkEndsPerObservable.end( ); + linkEndIterator++ ) { ObservableType currentObservable = linkEndIterator->first; std::vector< LinkEnds > currentLinkEndsList = linkEndIterator->second; @@ -116,19 +121,21 @@ BOOST_AUTO_TEST_CASE( test_json_acceleration_sphericalHarmonicGravity ) if( currentObservable == one_way_range ) { biasSettings = std::make_shared< ConstantObservationBiasSettings >( - Eigen::Vector1d::Constant( 1.0 ), true ); + Eigen::Vector1d::Constant( 1.0 ), true ); std::vector< std::string > perturbingBodies = { "Mars", "Moon" }; - lightTimeCorrections = std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( - perturbingBodies ); + lightTimeCorrections = + std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( + perturbingBodies ); } if( currentObservable == angular_position && i == 1 ) { biasSettings = std::make_shared< ConstantObservationBiasSettings >( - Eigen::Vector2d::Constant( 1.0E-6 ), false ); + Eigen::Vector2d::Constant( 1.0E-6 ), false ); std::vector< std::string > perturbingBodies = { "Mars", "Moon" }; - lightTimeCorrections = std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( - perturbingBodies ); + lightTimeCorrections = + std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( + perturbingBodies ); } if( currentObservable == angular_position && i == 1 ) @@ -145,13 +152,13 @@ BOOST_AUTO_TEST_CASE( test_json_acceleration_sphericalHarmonicGravity ) observationBiases[ 2 ]( 1 ) = 1.0E5; biasSettings = std::make_shared< ArcWiseConstantObservationBiasSettings >( - arcStartTimes, observationBiases, transmitter, true ); + arcStartTimes, observationBiases, transmitter, true ); } if( currentObservable == one_way_doppler && i == 0 ) { biasSettings = std::make_shared< ConstantObservationBiasSettings >( - Eigen::Vector1d::Constant( 1.0E-12 ), true ); + Eigen::Vector1d::Constant( 1.0E-12 ), true ); } if( currentObservable == one_way_doppler && i == 1 ) @@ -167,36 +174,35 @@ BOOST_AUTO_TEST_CASE( test_json_acceleration_sphericalHarmonicGravity ) observationBiases.push_back( Eigen::Vector1d::Constant( 2.0E-12 ) ); biasSettings = std::make_shared< ArcWiseConstantObservationBiasSettings >( - arcStartTimes, observationBiases, transmitter, true ); - + arcStartTimes, observationBiases, transmitter, true ); } if( currentObservable == one_way_range && i == 0 ) { std::vector< std::shared_ptr< ObservationBiasSettings > > biasSettingsList; - biasSettingsList.push_back( - std::make_shared< ConstantObservationBiasSettings >( - Eigen::Vector1d::Constant( 1.0E-6 ), false ) ); - biasSettingsList.push_back( - std::make_shared< ConstantObservationBiasSettings >( - Eigen::Vector1d::Constant( 2.0 ), true ) ); - biasSettings = std::make_shared< MultipleObservationBiasSettings >( biasSettingsList ); + biasSettingsList.push_back( std::make_shared< ConstantObservationBiasSettings >( + Eigen::Vector1d::Constant( 1.0E-6 ), false ) ); + biasSettingsList.push_back( std::make_shared< ConstantObservationBiasSettings >( + Eigen::Vector1d::Constant( 2.0 ), true ) ); + biasSettings = + std::make_shared< MultipleObservationBiasSettings >( biasSettingsList ); } std::shared_ptr< ObservationSettings > currentObservationSettings; if( currentObservable == one_way_doppler && i == 0 ) { currentObservationSettings = std::make_shared< OneWayDopplerObservationSettings >( - lightTimeCorrections, - std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Earth" ), - std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Sun" ), - biasSettings ); + lightTimeCorrections, + std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( + "Earth" ), + std::make_shared< DirectFirstOrderDopplerProperTimeRateSettings >( "Sun" ), + biasSettings ); } else if( currentObservable == one_way_differenced_range ) { - currentObservationSettings = std::make_shared< OneWayDifferencedRangeRateObservationSettings >( - [ = ]( const double ){ return 60.0; } ); - + currentObservationSettings = + std::make_shared< OneWayDifferencedRangeRateObservationSettings >( + [ = ]( const double ) { return 60.0; } ); } else if( currentObservable == n_way_range ) { @@ -205,39 +211,48 @@ BOOST_AUTO_TEST_CASE( test_json_acceleration_sphericalHarmonicGravity ) if( i == 0 ) { currentObservationSettings = std::make_shared< NWayRangeObservationSettings >( - lightTimeCorrections, 3, [ = ]( const double ){ return retransmissionTimes; }, biasSettings ); + lightTimeCorrections, + 3, + [ = ]( const double ) { return retransmissionTimes; }, + biasSettings ); } else if( i == 1 ) { std::vector< std::string > perturbingBodies = { "Mars", "Moon" }; - std::vector< std::shared_ptr< ObservationSettings > > oneWayRangeObsevationSettings; - oneWayRangeObsevationSettings.push_back( - std::make_shared< ObservationSettings >( - one_way_range, std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( - perturbingBodies ) ) ); - oneWayRangeObsevationSettings.push_back( - std::make_shared< ObservationSettings >( - one_way_range, nullptr, std::make_shared< ConstantObservationBiasSettings >( - Eigen::Vector1d::Constant( 1.0 ), true ) ) ); + std::vector< std::shared_ptr< ObservationSettings > > + oneWayRangeObservationSettings; + oneWayRangeObservationSettings.push_back( std::make_shared< + ObservationSettings >( + one_way_range, + std::make_shared< FirstOrderRelativisticLightTimeCorrectionSettings >( + perturbingBodies ) ) ); + oneWayRangeObservationSettings.push_back( + std::make_shared< ObservationSettings >( + one_way_range, + nullptr, + std::make_shared< ConstantObservationBiasSettings >( + Eigen::Vector1d::Constant( 1.0 ), true ) ) ); currentObservationSettings = std::make_shared< NWayRangeObservationSettings >( - oneWayRangeObsevationSettings, [ = ]( const double ){ return retransmissionTimes; }, biasSettings ); + oneWayRangeObservationSettings, + [ = ]( const double ) { return retransmissionTimes; }, + biasSettings ); } } else { currentObservationSettings = std::make_shared< ObservationSettings >( - currentObservable, lightTimeCorrections, biasSettings ); + currentObservable, lightTimeCorrections, biasSettings ); } - // Define settings for observable, no light-time corrections, and biases for selected links - observationSettingsMap[ currentLinkEndsList.at( i ) ].push_back( currentObservationSettings ); - + // Define settings for observable, no light-time corrections, and biases for selected + // links + observationSettingsMap[ currentLinkEndsList.at( i ) ].push_back( + currentObservationSettings ); } } - nlohmann::json jsonObject; to_json( jsonObject, observationSettingsMap ); @@ -247,7 +262,6 @@ BOOST_AUTO_TEST_CASE( test_json_acceleration_sphericalHarmonicGravity ) outputFile << jsonObject.dump( 2 ); outputFile.close( ); - observation_models::ObservationSettingsListPerLinkEnd observationSettingsMapFromJson; from_json( jsonObject, observationSettingsMapFromJson ); @@ -259,14 +273,11 @@ BOOST_AUTO_TEST_CASE( test_json_acceleration_sphericalHarmonicGravity ) observation_models::ObservationSettingsListPerLinkEnd observationSettingsMapFromFile = parseJSONFile< observation_models::ObservationSettingsListPerLinkEnd >( fileName ); - - BOOST_CHECK_EQUAL_JSON( observationSettingsMap, observationSettingsMapFromFile ); } - BOOST_AUTO_TEST_SUITE_END( ) -} // namespace unit_tests +} // namespace unit_tests -} // namespace tudat +} // namespace tudat From c2ec600edfc8260144680c97aadfe55136b9d9d5 Mon Sep 17 00:00:00 2001 From: Valerio Date: Thu, 14 Nov 2024 13:26:31 +0100 Subject: [PATCH 2/9] Use ObservatioModelSetting class as base class for DsnNWayRangeObservationSettings. Changes: - modified: include/tudat/simulation/estimation_setup/createObservationModel.h --- .../estimation_setup/createObservationModel.h | 57 +++++++++++++++---- 1 file changed, 47 insertions(+), 10 deletions(-) diff --git a/include/tudat/simulation/estimation_setup/createObservationModel.h b/include/tudat/simulation/estimation_setup/createObservationModel.h index ae8f981aeb..46caa9281a 100644 --- a/include/tudat/simulation/estimation_setup/createObservationModel.h +++ b/include/tudat/simulation/estimation_setup/createObservationModel.h @@ -1017,9 +1017,17 @@ class DsnNWayAveragedDopplerObservationSettings : public ObservationModelSetting bool subtractDopplerSignature_; }; -class DsnNWayRangeObservationSettings : public NWayRangeObservationSettings +class DsnNWayRangeObservationSettings : public ObservationModelSettings { public: + //! Constructor + /*! + * Constructor for same light-time corrections per link + * \param lightTimeCorrections Settings for a single light-time correction that is to be used + * for the observation model (nullptr if none) + * \param biasSettings Settings for the observation bias model that is to be used (default none: + * nullptr) + */ DsnNWayRangeObservationSettings( const LinkDefinition& linkEnds, const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > @@ -1028,22 +1036,51 @@ class DsnNWayRangeObservationSettings : public NWayRangeObservationSettings const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr, const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = std::make_shared< LightTimeConvergenceCriteria >( ) ) : - NWayRangeObservationSettings( linkEnds, - lightTimeCorrectionsList, - biasSettings, - lightTimeConvergenceCriteria ) - { } + ObservationModelSettings( dsn_n_way_range, + linkEnds, + lightTimeCorrectionsList, + biasSettings ), + multiLegLightTimeConvergenceCriteria_( lightTimeConvergenceCriteria ) + { + for( unsigned int i = 0; i < linkEnds.size( ) - 1; i++ ) + { + oneWayRangeObservationSettings_.push_back( std::make_shared< ObservationModelSettings >( + one_way_range, + getSingleLegLinkEnds( linkEnds.linkEnds_, i ), + lightTimeCorrectionsList, + nullptr, + lightTimeConvergenceCriteria ) ); + } + } + //! Constructor + /*! + * Constructor for different light-time corrections per link + * \param oneWayRangeObservationSettings List of settings for the one-way range observation + * models \param biasSettings Settings for the observation bias model that is to be used + * (default none: nullptr) \param lightTimeConvergenceCriteria Settings for the light-time + * convergence criteria + */ DsnNWayRangeObservationSettings( const std::vector< std::shared_ptr< ObservationModelSettings > > oneWayRangeObservationSettings, const std::shared_ptr< ObservationBiasSettings > biasSettings = nullptr, const std::shared_ptr< LightTimeConvergenceCriteria > lightTimeConvergenceCriteria = std::make_shared< LightTimeConvergenceCriteria >( ) ) : - NWayRangeObservationSettings( oneWayRangeObservationSettings, - biasSettings, - lightTimeConvergenceCriteria ) + ObservationModelSettings( dsn_n_way_range, + mergeOneWayLinkEnds( getObservationModelListLinkEnds( + oneWayRangeObservationSettings ) ), + std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), + biasSettings ), + oneWayRangeObservationSettings_( oneWayRangeObservationSettings ), + multiLegLightTimeConvergenceCriteria_( lightTimeConvergenceCriteria ) { } + + //! Destructor + ~DsnNWayRangeObservationSettings( ) { } + + std::vector< std::shared_ptr< ObservationModelSettings > > oneWayRangeObservationSettings_; + std::shared_ptr< LightTimeConvergenceCriteria > multiLegLightTimeConvergenceCriteria_; }; inline std::shared_ptr< ObservationModelSettings > oneWayRangeSettings( @@ -1297,7 +1334,7 @@ inline std::shared_ptr< ObservationModelSettings > dsnNWayAveragedDopplerObserva subtractDopplerSignature ); } -inline std::shared_ptr< DsnNWayRangeObservationSettings > dsnNWayRangeObservationSettings( +inline std::shared_ptr< ObservationModelSettings > dsnNWayRangeObservationSettings( const LinkDefinition& linkEnds, const std::vector< std::shared_ptr< LightTimeCorrectionSettings > > lightTimeCorrectionsList = From d97eadd3c65dd04c4570cf12ba600f0a09bfcdd9 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Fri, 22 Nov 2024 09:19:15 +0100 Subject: [PATCH 3/9] Ongoing test --- .../observation_models/dsnNWayRangeObservationModel.h | 2 ++ .../tudat/astro/observation_models/lightTimeSolution.h | 9 +++++++++ .../corrections/solarCoronaCorrection.cpp | 5 +++++ 3 files changed, 16 insertions(+) diff --git a/include/tudat/astro/observation_models/dsnNWayRangeObservationModel.h b/include/tudat/astro/observation_models/dsnNWayRangeObservationModel.h index b650b7d3de..09c0254ba2 100644 --- a/include/tudat/astro/observation_models/dsnNWayRangeObservationModel.h +++ b/include/tudat/astro/observation_models/dsnNWayRangeObservationModel.h @@ -136,9 +136,11 @@ class DsnNWayRangeObservationModel: public ObservationModel< 1, ObservationScala FrequencyBands uplinkBand = frequencyBands.at( 0 ); FrequencyBands downlinkBand = frequencyBands.at( 1 ); + std::cout<calculateLightTimeWithLinkEndsStates( time, linkEndAssociatedWithTime, linkEndTimes, linkEndStates, ancillarySettings ); + std::cout<getNominalCartesianPosition( ); diff --git a/include/tudat/astro/observation_models/lightTimeSolution.h b/include/tudat/astro/observation_models/lightTimeSolution.h index fb3fde9a03..65447e782a 100644 --- a/include/tudat/astro/observation_models/lightTimeSolution.h +++ b/include/tudat/astro/observation_models/lightTimeSolution.h @@ -870,6 +870,10 @@ class MultiLegLightTimeCalculator linkEndsDelays_ = ancillarySettings->getAncilliaryDoubleVectorData( link_ends_delays, false ); } + linkEndsDelays_.clear( ); + linkEndsDelays_.push_back( 0.0); + linkEndsDelays_.push_back( 1.4149E-6 ); + linkEndsDelays_.push_back( 0.0 ); if ( !linkEndsDelays_.empty( ) ) { // Delays vector already including delays at receiving and transmitting stations @@ -1034,10 +1038,12 @@ class MultiLegLightTimeCalculator // Move 'backwards' from reference link end to transmitter. for( unsigned int currentDownIndex = startLinkEndIndex; currentDownIndex > 0; --currentDownIndex ) { + std::cout<<"Calculator down"<calculateLightTimeWithMultiLegLinkEndsStates( linkEndStates, linkEndTimes, currentLinkEndReceptionTime, true, transmitterIndex, ancillarySettings, computeLightTimeCorrections ); + std::cout<<"Delay down"<calculateLightTimeWithMultiLegLinkEndsStates( linkEndStates, linkEndTimes, currentLinkEndTransmissionTime, false, transmitterIndex, @@ -1060,6 +1067,8 @@ class MultiLegLightTimeCalculator // If an additional leg is required, retrieve retransmission delay and update current time currentLightTime += linkEndsDelays_.at( currentUpIndex + 1 ); + std::cout<<"Delay up"< 0.1 * mathematical_constants::PI / 180.0 ) { + std::cout<<"Is integer"<( )<( )<( ); From 7c248a094fc5bcb4b68d0a2e07d50adb6181cb5a Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Fri, 22 Nov 2024 11:51:43 +0100 Subject: [PATCH 4/9] First version of error supporession --- .../tudat/interface/spice/spiceInterface.h | 12 ++ src/interface/spice/spiceInterface.cpp | 189 ++++++++++++++---- .../spice/unitTestSpiceInterface.cpp | 21 ++ 3 files changed, 185 insertions(+), 37 deletions(-) diff --git a/include/tudat/interface/spice/spiceInterface.h b/include/tudat/interface/spice/spiceInterface.h index 234120e85a..83d45fca34 100644 --- a/include/tudat/interface/spice/spiceInterface.h +++ b/include/tudat/interface/spice/spiceInterface.h @@ -136,6 +136,18 @@ std::vector getStandardSpiceKernels(const std::vector void loadStandardSpiceKernels(const std::vector alternativeEphemerisKernels = std::vector()); +void toggleErrorReturn( ); + +void toggleErrorAbort( ); + +void suppressErrorOutput( ); + +std::string getErrorMessage( ); + +bool checkFailure( ); + + + }// namespace spice_interface }// namespace tudat diff --git a/src/interface/spice/spiceInterface.cpp b/src/interface/spice/spiceInterface.cpp index 7aa7964fb1..e1d4827be4 100644 --- a/src/interface/spice/spiceInterface.cpp +++ b/src/interface/spice/spiceInterface.cpp @@ -16,9 +16,11 @@ #include -namespace tudat { -namespace spice_interface { -using Eigen::Vector6d; +namespace tudat +{ + +namespace spice_interface +{ std::string getCorrectedTargetBodyName( const std::string &targetBodyName ) @@ -60,34 +62,45 @@ double convertDateStringToEphemerisTime(const std::string &dateString) { } //! Get Cartesian state of a body, as observed from another body. -Vector6d getBodyCartesianStateAtEpoch( +Eigen::Vector6d getBodyCartesianStateAtEpoch( const std::string &targetBodyName, const std::string &observerBodyName, const std::string &referenceFrameName, const std::string &aberrationCorrections, - const double ephemerisTime) { + const double ephemerisTime) +{ - if( !( ephemerisTime == ephemerisTime ) ) + if ( !( ephemerisTime == ephemerisTime )) { - throw std::invalid_argument( "Error when retrieving Cartesian state from Spice, input time is " + std::to_string(ephemerisTime) ); + throw std::invalid_argument( + "Error when retrieving Cartesian state from Spice, input time is " + std::to_string( ephemerisTime )); } // Declare variables for cartesian state and light-time to be determined by Spice. double stateAtEpoch[6]; double lightTime; // Call Spice function to calculate state and light-time. - spkezr_c(getCorrectedTargetBodyName( targetBodyName ).c_str(), ephemerisTime, referenceFrameName.c_str(), - aberrationCorrections.c_str(), - getCorrectedTargetBodyName( observerBodyName ).c_str(), stateAtEpoch, - &lightTime); + spkezr_c( getCorrectedTargetBodyName( targetBodyName ).c_str( ), ephemerisTime, referenceFrameName.c_str( ), + aberrationCorrections.c_str( ), + getCorrectedTargetBodyName( observerBodyName ).c_str( ), stateAtEpoch, + &lightTime ); // Put result in Eigen Vector. - Vector6d cartesianStateVector; - for (unsigned int i = 0; i < 6; i++) { - cartesianStateVector(i) = stateAtEpoch[i]; + Eigen::Vector6d cartesianStateVector; + if ( !checkFailure( ) ) + { + for ( unsigned int i = 0; i < 6; i++ ) + { + cartesianStateVector( i ) = stateAtEpoch[ i ]; + } + } + else + { + cartesianStateVector.setZero( ); } + // Convert from km(/s) to m(/s). - return unit_conversions::convertKilometersToMeters( + return unit_conversions::convertKilometersToMeters( cartesianStateVector); } @@ -114,8 +127,17 @@ Eigen::Vector3d getBodyCartesianPositionAtEpoch(const std::string &targetBodyNam // Put result in Eigen Vector. Eigen::Vector3d cartesianPositionVector; - for (unsigned int i = 0; i < 3; i++) { - cartesianPositionVector(i) = positionAtEpoch[i]; + + if ( !checkFailure( ) ) + { + for (unsigned int i = 0; i < 3; i++) + { + cartesianPositionVector(i) = positionAtEpoch[i]; + } + } + else + { + cartesianPositionVector.setZero( ); } // Convert from km to m. @@ -124,7 +146,7 @@ Eigen::Vector3d getBodyCartesianPositionAtEpoch(const std::string &targetBodyNam } //! Get Cartesian state of a satellite from its two-line element set at a specified epoch. -Vector6d getCartesianStateFromTleAtEpoch(double epoch, std::shared_ptr tle) { +Eigen::Vector6d getCartesianStateFromTleAtEpoch(double epoch, std::shared_ptr tle) { if( !( epoch == epoch )) { throw std::invalid_argument( "Error when retrieving TLE from Spice, input time is " + std::to_string(epoch) ); @@ -153,13 +175,21 @@ Vector6d getCartesianStateFromTleAtEpoch(double epoch, std::shared_ptr(cartesianStateVector); + return unit_conversions::convertKilometersToMeters(cartesianStateVector); } //! Compute quaternion of rotation between two frames. @@ -179,11 +209,18 @@ Eigen::Quaterniond computeRotationQuaternionBetweenFrames(const std::string &ori // Put rotation matrix in Eigen Matrix3d. Eigen::Matrix3d rotationMatrix; - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = 0; j < 3; j++) { - rotationMatrix(i, j) = rotationArray[i][j]; + if ( !checkFailure( ) ) + { + for (unsigned int i = 0; i < 3; i++) { + for (unsigned int j = 0; j < 3; j++) { + rotationMatrix(i, j) = rotationArray[i][j]; + } } } + else + { + rotationMatrix.setIdentity( ); + } // Convert matrix3d to Quaternion. return Eigen::Quaterniond(rotationMatrix); @@ -213,11 +250,21 @@ Eigen::Matrix6d computeStateRotationMatrixBetweenFrames(const std::string &origi // Put rotation matrix in Eigen Matrix6d Eigen::Matrix6d stateTransitionMatrix = Eigen::Matrix6d::Zero(); - for (unsigned int i = 0; i < 6; i++) { - for (unsigned int j = 0; j < 6; j++) { - stateTransitionMatrix(i, j) = stateTransition[i][j]; + if ( !checkFailure( ) ) + { + for (unsigned int i = 0; i < 6; i++) + { + for (unsigned int j = 0; j < 6; j++) + { + stateTransitionMatrix(i, j) = stateTransition[i][j]; + } } } + else + { + stateTransitionMatrix.setIdentity( ); + } + return stateTransitionMatrix; } @@ -239,11 +286,20 @@ Eigen::Matrix3d computeRotationMatrixDerivativeBetweenFrames(const std::string & // Put rotation matrix derivative in Eigen Matrix3d Eigen::Matrix3d matrixDerivative = Eigen::Matrix3d::Zero(); - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = 0; j < 3; j++) { - matrixDerivative(i, j) = stateTransition[i + 3][j]; + if ( !checkFailure( ) ) + { + for (unsigned int i = 0; i < 3; i++) + { + for (unsigned int j = 0; j < 3; j++) + { + matrixDerivative(i, j) = stateTransition[i + 3][j]; + } } } + else + { + matrixDerivative.setZero( ); + } return matrixDerivative; } @@ -269,7 +325,14 @@ Eigen::Vector3d getAngularVelocityVectorOfFrameInOriginalFrame(const std::string // Calculate angular velocity vector. xf2rav_c(stateTransition, rotation, angularVelocity); - return (Eigen::Vector3d() << angularVelocity[0], angularVelocity[1], angularVelocity[2]).finished(); + if ( !checkFailure( ) ) + { + return (Eigen::Vector3d() << angularVelocity[0], angularVelocity[1], angularVelocity[2]).finished(); + } + else + { + return Eigen::Vector3d::Zero( ); + } } std::pair computeRotationQuaternionAndRotationMatrixDerivativeBetweenFrames( @@ -285,13 +348,23 @@ std::pair computeRotationQuaternionAndRotat Eigen::Matrix3d matrixDerivative; Eigen::Matrix3d rotationMatrix; - - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = 0; j < 3; j++) { - rotationMatrix(i, j) = stateTransition[i][j]; - matrixDerivative(i, j) = stateTransition[i + 3][j]; + if ( !checkFailure( ) ) + { + for (unsigned int i = 0; i < 3; i++) + { + for (unsigned int j = 0; j < 3; j++) + { + rotationMatrix(i, j) = stateTransition[i][j]; + matrixDerivative(i, j) = stateTransition[i + 3][j]; + } } } + else + { + rotationMatrix.setIdentity( ); + matrixDerivative.setZero( ); + } + return std::make_pair(Eigen::Quaterniond(rotationMatrix), matrixDerivative); } @@ -475,5 +548,47 @@ void loadStandardSpiceKernels(const std::vector alternativeEphemeri loadSpiceKernelInTudat(kernelPath + "/naif0012.tls"); } +void toggleErrorReturn( ) +{ + erract_c ( "SET", 0, "RETURN" ); +} + +void toggleErrorAbort( ) +{ + errdev_c ( "SET", 0, "ABORT" ); +} + +void suppressErrorOutput( ) +{ + errdev_c ( "SET", 0, "NULL" ); +} + +std::string getErrorMessage( ) +{ + if( failed_c( ) ) + { + SpiceChar message[1841]; + getmsg_c( "LONG", 1841, message ); + return static_cast< std::string >( message ); + } + else + { + return ""; + } +} + +bool checkFailure( ) +{ + if ( failed_c( ) ) + { + reset_c( ); + return true; + } + else + { + return false; + } + +} }// namespace spice_interface }// namespace tudat diff --git a/tests/src/interface/spice/unitTestSpiceInterface.cpp b/tests/src/interface/spice/unitTestSpiceInterface.cpp index 70b85c40f8..beb6225bae 100644 --- a/tests/src/interface/spice/unitTestSpiceInterface.cpp +++ b/tests/src/interface/spice/unitTestSpiceInterface.cpp @@ -64,10 +64,31 @@ BOOST_AUTO_TEST_SUITE( test_spice_wrappers ) // Eigen::Vector6d marsCentricState = getBodyCartesianStateAtEpoch( "Mars Express", "Mars", "IAU_Mars", "NONE", testTime ); //} +// Test 1: Test Julian day <-> Ephemeris time conversions at J2000. +BOOST_AUTO_TEST_CASE( testNoKernelCrash ) +{ + using namespace spice_interface; + using namespace input_output; + using namespace physical_constants; + + // Create settings at which states are to be evaluated. + const std::string aberrationCorrections = "NONE"; + const std::string observer = "Solar System Barycenter"; + const std::string target = "Mars"; + const std::string referenceFrame = "J2000"; + const double ephemerisTime = 1.0E6; + + // Get state from wrapper for state: + toggleErrorReturn( ); + suppressErrorOutput( ); + const Eigen::Vector6d wrapperState = getBodyCartesianStateAtEpoch( + target, observer, referenceFrame, aberrationCorrections, ephemerisTime ); +} // Test 1: Test Julian day <-> Ephemeris time conversions at J2000. BOOST_AUTO_TEST_CASE( testSpiceWrappers_1 ) { + std::cout<<"Test*******************"< Date: Mon, 25 Nov 2024 09:59:40 +0100 Subject: [PATCH 5/9] With additional preliminary code for tetsing spice error handling --- .../ground_stations/transmittingFrequencies.h | 32 +++++++++++-------- src/interface/spice/spiceInterface.cpp | 6 ++-- 2 files changed, 21 insertions(+), 17 deletions(-) diff --git a/include/tudat/astro/ground_stations/transmittingFrequencies.h b/include/tudat/astro/ground_stations/transmittingFrequencies.h index cb7119e18a..a200485bb8 100644 --- a/include/tudat/astro/ground_stations/transmittingFrequencies.h +++ b/include/tudat/astro/ground_stations/transmittingFrequencies.h @@ -16,6 +16,7 @@ #include "tudat/math/quadrature/trapezoidQuadrature.h" #include "tudat/math/interpolators.h" +#include "tudat/astro/basic_astro/dateTime.h" namespace tudat { @@ -200,20 +201,23 @@ class PiecewiseLinearFrequencyInterpolator: public StationFrequencyInterpolator // If there are discontinuities if ( startTimes_.at( i ) != endTimes_.at( i - 1 ) ) { - // If the start and end times are inconsistent throw error - if ( endTimes_.at( i - 1 ) > startTimes_.at( i ) ) - { - throw std::runtime_error( - "Error when creating piecewise linear frequency interpolator: inconsistency between ramp end " - "time (" + std::to_string( double( endTimes_.at( i - 1 ) ) ) + ") and start time of the following ramp (" + - std::to_string( double( startTimes_.at( i ) ) ) + "); the end is smaller than the start time." ); - } - // If there are gaps in the data save that information - else - { - invalidTimeBlocksStartTimes_.push_back( endTimes_.at( i - 1 ) ); - invalidTimeBlocksEndTimes_.push_back( startTimes_.at( i ) ); - } +// // If the start and end times are inconsistent throw error +// if ( endTimes_.at( i - 1 ) > startTimes_.at( i ) ) +// { +// throw std::runtime_error( +// "Error when creating piecewise linear frequency interpolator: inconsistency between ramp end " +// "time (" + std::to_string( double( endTimes_.at( i - 1 ) ) ) + ";" + +// basic_astrodynamics::getCalendarDateFromTime( endTimes_.at( i - 1 ) ).isoString( ) + ") and start time of the following ramp (" + +// std::to_string( double( startTimes_.at( i ) ) ) + ";" + +// basic_astrodynamics::getCalendarDateFromTime( startTimes_.at( i ) ).isoString( ) + +// "); the end is smaller than the start time." ); +// } +// // If there are gaps in the data save that information +// else +// { +// invalidTimeBlocksStartTimes_.push_back( endTimes_.at( i - 1 ) ); +// invalidTimeBlocksEndTimes_.push_back( startTimes_.at( i ) ); +// } } } diff --git a/src/interface/spice/spiceInterface.cpp b/src/interface/spice/spiceInterface.cpp index e1d4827be4..ce6f8fb921 100644 --- a/src/interface/spice/spiceInterface.cpp +++ b/src/interface/spice/spiceInterface.cpp @@ -95,7 +95,7 @@ Eigen::Vector6d getBodyCartesianStateAtEpoch( } else { - cartesianStateVector.setZero( ); + cartesianStateVector.setConstant( 1.0E12 ); } @@ -137,7 +137,7 @@ Eigen::Vector3d getBodyCartesianPositionAtEpoch(const std::string &targetBodyNam } else { - cartesianPositionVector.setZero( ); + cartesianPositionVector.setConstant( 1.0E12 ); } // Convert from km to m. @@ -185,7 +185,7 @@ Eigen::Vector6d getCartesianStateFromTleAtEpoch(double epoch, std::shared_ptr Date: Thu, 5 Dec 2024 16:11:48 +0100 Subject: [PATCH 6/9] Removed debug printing and add range ancillary setting 'range_conversion_factor'. Changes: - modified: include/tudat/astro/observation_models/dsnNWayRangeObservationModel.h - modified: include/tudat/astro/observation_models/lightTimeSolution.h - modified: include/tudat/astro/observation_models/observationModel.h - modified: src/astro/observation_models/corrections/solarCoronaCorrection.cpp --- .../dsnNWayRangeObservationModel.h | 185 +++++++++++------- .../observation_models/lightTimeSolution.h | 11 +- .../observation_models/observationModel.h | 16 +- .../corrections/solarCoronaCorrection.cpp | 5 - 4 files changed, 129 insertions(+), 88 deletions(-) diff --git a/include/tudat/astro/observation_models/dsnNWayRangeObservationModel.h b/include/tudat/astro/observation_models/dsnNWayRangeObservationModel.h index 09c0254ba2..0dfab7ca1e 100644 --- a/include/tudat/astro/observation_models/dsnNWayRangeObservationModel.h +++ b/include/tudat/astro/observation_models/dsnNWayRangeObservationModel.h @@ -8,8 +8,8 @@ * http://tudat.tudelft.nl/LICENSE. * * References: - * T. Moyer (2000), Formulation for Observed and Computed Values of Deep Space Network Data Types for Navigation, - * DEEP SPACE COMMUNICATIONS AND NAVIGATION SERIES, JPL/NASA + * T. Moyer (2000), Formulation for Observed and Computed Values of Deep Space Network Data + * Types for Navigation, DEEP SPACE COMMUNICATIONS AND NAVIGATION SERIES, JPL/NASA */ #ifndef TUDAT_DSNNWAYRANGEOBSERVATIONMODEL_H @@ -18,11 +18,11 @@ #include #include -#include "tudat/simulation/simulation.h" - +#include "tudat/astro/basic_astro/physicalConstants.h" +#include "tudat/astro/observation_models/nWayRangeObservationModel.h" #include "tudat/astro/observation_models/observableTypes.h" #include "tudat/astro/observation_models/observationFrequencies.h" -#include "tudat/astro/observation_models/nWayRangeObservationModel.h" +#include "tudat/simulation/simulation.h" namespace tudat { @@ -31,9 +31,9 @@ namespace observation_models { template< typename ObservationScalarType = double, typename TimeType = Time > -class DsnNWayRangeObservationModel: public ObservationModel< 1, ObservationScalarType, TimeType > +class DsnNWayRangeObservationModel : public ObservationModel< 1, ObservationScalarType, TimeType > { -public: + public: typedef Eigen::Matrix< ObservationScalarType, 6, 1 > StateType; /*! Constructor. @@ -43,33 +43,43 @@ class DsnNWayRangeObservationModel: public ObservationModel< 1, ObservationScala * @param arcEndObservationModel N-way range observation model associated with the end of the Doppler integration time. * @param bodyWithGroundStations Body object where the ground stations are located. * @param observationBiasCalculator Object for calculating (system-dependent) errors in the - * observable, i.e. deviations from the physically ideal observable between reference points (default none). + * observable, i.e. deviations from the physically ideal observable between reference points + * (default none). */ DsnNWayRangeObservationModel( const LinkEnds& linkEnds, - const std::shared_ptr< observation_models::MultiLegLightTimeCalculator< ObservationScalarType, TimeType > > lightTimeCalculator, - const std::shared_ptr< ground_stations::StationFrequencyInterpolator > transmittingFrequencyCalculator, + const std::shared_ptr< + observation_models::MultiLegLightTimeCalculator< ObservationScalarType, + TimeType > > + lightTimeCalculator, + const std::shared_ptr< ground_stations::StationFrequencyInterpolator > + transmittingFrequencyCalculator, const std::shared_ptr< ObservationBias< 1 > > observationBiasCalculator = nullptr, - const std::map< LinkEndType, std::shared_ptr< ground_stations::GroundStationState > > groundStationStates = - std::map< LinkEndType, std::shared_ptr< ground_stations::GroundStationState > >( ) ): - ObservationModel< 1, ObservationScalarType, TimeType >( dsn_n_way_range , linkEnds, observationBiasCalculator), - lightTimeCalculator_( lightTimeCalculator ), - numberOfLinkEnds_( linkEnds.size( ) ), + const std::map< LinkEndType, std::shared_ptr< ground_stations::GroundStationState > > + groundStationStates = std::map< + LinkEndType, + std::shared_ptr< ground_stations::GroundStationState > >( ) ) : + ObservationModel< 1, ObservationScalarType, TimeType >( dsn_n_way_range, + linkEnds, + observationBiasCalculator ), + lightTimeCalculator_( lightTimeCalculator ), numberOfLinkEnds_( linkEnds.size( ) ), transmittingFrequencyCalculator_( transmittingFrequencyCalculator ), stationStates_( groundStationStates ) { if( !std::is_same< Time, TimeType >::value ) { -// std::cerr<< -// "Warning when defining DSN N-way range observation model: the selected time type " -// "is not valid, using it would lead to large numerical errors."<& linkEndTimes, std::vector< Eigen::Matrix< double, 6, 1 > >& linkEndStates, - const std::shared_ptr< ObservationAncilliarySimulationSettings > ancillarySettings = nullptr ) + const std::shared_ptr< ObservationAncilliarySimulationSettings > ancillarySettings = + nullptr ) { // Check if selected reference link end is valid - if ( linkEndAssociatedWithTime != receiver ) + if( linkEndAssociatedWithTime != receiver ) { throw std::runtime_error( - "Error when computing DSN N-way range observables: the selected reference link end (" + - getLinkEndTypeString( linkEndAssociatedWithTime ) + ") is not valid." ); + "Error when computing DSN N-way range observables: the selected reference link " + "end (" + + getLinkEndTypeString( linkEndAssociatedWithTime ) + ") is not valid." ); } // Check if ancillary settings were provided if( ancillarySettings == nullptr ) { throw std::runtime_error( - "Error when simulating n-way DSN range observable; no ancillary settings found. " ); + "Error when simulating n-way DSN range observable; no ancillary settings " + "found. " ); } - ObservationScalarType lowestRangingComponent; //referenceFrequency + ObservationScalarType lowestRangingComponent; std::vector< FrequencyBands > frequencyBands; try { - lowestRangingComponent = ancillarySettings->getAncilliaryDoubleData( sequential_range_lowest_ranging_component ); -// referenceFrequency = ancillarySettings->getAncilliaryDoubleData( sequential_range_reference_frequency ); - frequencyBands = convertDoubleVectorToFrequencyBands( ancillarySettings->getAncilliaryDoubleVectorData( frequency_bands ) ); + lowestRangingComponent = ancillarySettings->getAncilliaryDoubleData( + sequential_range_lowest_ranging_component ); + // referenceFrequency = ancillarySettings->getAncilliaryDoubleData( + // sequential_range_reference_frequency ); + frequencyBands = convertDoubleVectorToFrequencyBands( + ancillarySettings->getAncilliaryDoubleVectorData( frequency_bands ) ); } catch( std::runtime_error& caughtException ) { @@ -126,80 +143,106 @@ class DsnNWayRangeObservationModel: public ObservationModel< 1, ObservationScala std::string( caughtException.what( ) ) ); } - if ( frequencyBands.size( ) != numberOfLinkEnds_ - 1 ) + if( frequencyBands.size( ) != numberOfLinkEnds_ - 1 ) { throw std::runtime_error( - "Error when retrieving frequency bands ancillary settings for DSN N-way range observable: " - "size (" + std::to_string( frequencyBands.size( ) ) + ") is inconsistent with number of links (" + + "Error when retrieving frequency bands ancillary settings for DSN N-way range " + "observable: " + "size (" + + std::to_string( frequencyBands.size( ) ) + + ") is inconsistent with number of links (" + std::to_string( numberOfLinkEnds_ - 1 ) + ")." ); } FrequencyBands uplinkBand = frequencyBands.at( 0 ); FrequencyBands downlinkBand = frequencyBands.at( 1 ); - std::cout<calculateLightTimeWithLinkEndsStates( - time, linkEndAssociatedWithTime, linkEndTimes, linkEndStates, - ancillarySettings ); - std::cout<getNominalCartesianPosition( ); + Eigen::Vector3d nominalReceivingStationState = ( stationStates_.count( receiver ) == 0 ) + ? Eigen::Vector3d::Zero( ) + : stationStates_.at( receiver )->getNominalCartesianPosition( ); TimeType utcReceptionTime = terrestrialTimeScaleConverter_->getCurrentTime< TimeType >( - basic_astrodynamics::tdb_scale, basic_astrodynamics::utc_scale, time, nominalReceivingStationState ); + basic_astrodynamics::tdb_scale, + basic_astrodynamics::utc_scale, + time, + nominalReceivingStationState ); TimeType utcTransmissionTime = terrestrialTimeScaleConverter_->getCurrentTime< TimeType >( - basic_astrodynamics::tdb_scale, basic_astrodynamics::utc_scale, time - lightTime, nominalReceivingStationState ); + basic_astrodynamics::tdb_scale, + basic_astrodynamics::utc_scale, + time - lightTime, + nominalReceivingStationState ); ObservationScalarType transmitterFrequencyIntegral = - transmittingFrequencyCalculator_->template getTemplatedFrequencyIntegral< ObservationScalarType, TimeType >( - utcTransmissionTime, utcReceptionTime ); - ObservationScalarType rangeUnitIntegral = 221.0 / ( 749.0 * 2.0 ) * transmitterFrequencyIntegral; + transmittingFrequencyCalculator_ + ->template getTemplatedFrequencyIntegral< ObservationScalarType, TimeType >( + utcTransmissionTime, utcReceptionTime ); + ObservationScalarType rangeUnitIntegral = conversionFactor * transmitterFrequencyIntegral; + + ancillarySettings->setAncilliaryDoubleData( + range_conversion_factor, + 1 / rangeUnitIntegral * physical_constants::SPEED_OF_LIGHT ); // Moyer (2000), eq. 13-54 Eigen::Matrix< ObservationScalarType, 1, 1 > observation = - ( Eigen::Matrix< ObservationScalarType, 1, 1 >( ) << - static_cast< ObservationScalarType >( basic_mathematics::computeModulo( rangeUnitIntegral, std::pow( 2.0, lowestRangingComponent + 6.0 ) ) ) ).finished( ); -// std::cout<getTemplatedCurrentFrequency( utcReceptionTime ) * -// static_cast< double >( utcReceptionTime - utcTransmissionTime ) <<" "<< -// ( utcReceptionTime - utcTransmissionTime ) <<" "<< -// transmittingFrequencyCalculator_->getTemplatedCurrentFrequency( utcReceptionTime )<<" "<< -// transmittingFrequencyCalculator_->getTemplatedCurrentFrequency( utcTransmissionTime )<<" "<< -// transmittingFrequencyCalculator_->getTemplatedCurrentFrequency( utcReceptionTime ) - transmittingFrequencyCalculator_->getTemplatedCurrentFrequency( utcTransmissionTime )<<" "<< -// -// observation<<" "<( ) + << static_cast< ObservationScalarType >( basic_mathematics::computeModulo( + rangeUnitIntegral, std::pow( 2.0, lowestRangingComponent + 6.0 ) ) ) ) + .finished( ); return observation; } - std::shared_ptr< observation_models::MultiLegLightTimeCalculator< ObservationScalarType, TimeType > > getLightTimeCalculator( ) + std::shared_ptr< + observation_models::MultiLegLightTimeCalculator< ObservationScalarType, TimeType > > + getLightTimeCalculator( ) { return lightTimeCalculator_; } -private: - - std::shared_ptr< observation_models::MultiLegLightTimeCalculator< ObservationScalarType, TimeType > > lightTimeCalculator_; - + private: + std::shared_ptr< + observation_models::MultiLegLightTimeCalculator< ObservationScalarType, TimeType > > + lightTimeCalculator_; // Number of link ends unsigned int numberOfLinkEnds_; // Object returning the transmitted frequency as the transmitting link end - std::shared_ptr< ground_stations::StationFrequencyInterpolator > transmittingFrequencyCalculator_; + std::shared_ptr< ground_stations::StationFrequencyInterpolator > + transmittingFrequencyCalculator_; // Function returning the turnaround ratio for given uplink and downlink bands - std::function< double ( FrequencyBands uplinkBand, FrequencyBands downlinkBand ) > turnaroundRatio_; + std::function< double( FrequencyBands uplinkBand, FrequencyBands downlinkBand ) > + turnaroundRatio_; - std::shared_ptr< earth_orientation::TerrestrialTimeScaleConverter > terrestrialTimeScaleConverter_; + std::shared_ptr< earth_orientation::TerrestrialTimeScaleConverter > + terrestrialTimeScaleConverter_; std::map< LinkEndType, std::shared_ptr< ground_stations::GroundStationState > > stationStates_; }; +} // namespace observation_models +} // namespace tudat -} // namespace observation_models - -} // namespace tudat - - -#endif //TUDAT_DSNNWAYRANGEOBSERVATIONMODEL_H +#endif // TUDAT_DSNNWAYRANGEOBSERVATIONMODEL_H diff --git a/include/tudat/astro/observation_models/lightTimeSolution.h b/include/tudat/astro/observation_models/lightTimeSolution.h index 65447e782a..1e3d5faadf 100644 --- a/include/tudat/astro/observation_models/lightTimeSolution.h +++ b/include/tudat/astro/observation_models/lightTimeSolution.h @@ -870,10 +870,9 @@ class MultiLegLightTimeCalculator linkEndsDelays_ = ancillarySettings->getAncilliaryDoubleVectorData( link_ends_delays, false ); } - linkEndsDelays_.clear( ); - linkEndsDelays_.push_back( 0.0); - linkEndsDelays_.push_back( 1.4149E-6 ); - linkEndsDelays_.push_back( 0.0 ); + + linkEndsDelays_[1] = 1.4149E-6; + if ( !linkEndsDelays_.empty( ) ) { // Delays vector already including delays at receiving and transmitting stations @@ -1038,12 +1037,10 @@ class MultiLegLightTimeCalculator // Move 'backwards' from reference link end to transmitter. for( unsigned int currentDownIndex = startLinkEndIndex; currentDownIndex > 0; --currentDownIndex ) { - std::cout<<"Calculator down"<calculateLightTimeWithMultiLegLinkEndsStates( linkEndStates, linkEndTimes, currentLinkEndReceptionTime, true, transmitterIndex, ancillarySettings, computeLightTimeCorrections ); - std::cout<<"Delay down"<calculateLightTimeWithMultiLegLinkEndsStates( linkEndStates, linkEndTimes, currentLinkEndTransmissionTime, false, transmitterIndex, @@ -1067,7 +1063,6 @@ class MultiLegLightTimeCalculator // If an additional leg is required, retrieve retransmission delay and update current time currentLightTime += linkEndsDelays_.at( currentUpIndex + 1 ); - std::cout<<"Delay up"< getDsnNWayRang const std::vector< FrequencyBands > &frequencyBands, const double referenceFrequency, const double lowestRangingComponent, + const double rangeConversionFactor = 0.0, const std::vector< double > linkEndsDelays = std::vector< double >( ) ) { @@ -307,6 +314,7 @@ inline std::shared_ptr< ObservationAncilliarySimulationSettings > getDsnNWayRang lowestRangingComponent ); ancillarySettings->setAncilliaryDoubleData( sequential_range_reference_frequency, referenceFrequency ); + ancillarySettings->setAncilliaryDoubleData( range_conversion_factor, rangeConversionFactor ); ancillarySettings->setAncilliaryDoubleVectorData( frequency_bands, convertFrequencyBandsToDoubleVector( frequencyBands ) ); @@ -657,10 +665,10 @@ class ObservationModel std::vector< Eigen::Matrix< double, 6, 1 > > linkEndStates_; }; // -//extern template class ObservationModel< 1, double, double >; -//extern template class ObservationModel< 2, double, double >; -//extern template class ObservationModel< 3, double, double >; -//extern template class ObservationModel< 6, double, double >; +// extern template class ObservationModel< 1, double, double >; +// extern template class ObservationModel< 2, double, double >; +// extern template class ObservationModel< 3, double, double >; +// extern template class ObservationModel< 6, double, double >; //! Function to compute an observation of size 1 at double precision, with double precision input /*! diff --git a/src/astro/observation_models/corrections/solarCoronaCorrection.cpp b/src/astro/observation_models/corrections/solarCoronaCorrection.cpp index ee5fd68f08..b53c22fa86 100644 --- a/src/astro/observation_models/corrections/solarCoronaCorrection.cpp +++ b/src/astro/observation_models/corrections/solarCoronaCorrection.cpp @@ -120,7 +120,6 @@ double InversePowerSeriesSolarCoronaCorrection::calculateLightTimeCorrectionWith // If all exponents are integer calculate integral of electron density analytically if ( exponentsAreIntegers_ && std::abs( receiverSunTransmitterAngle ) > 0.1 * mathematical_constants::PI / 180.0 ) { - std::cout<<"Is integer"<( )<( )<( ); From 7cec9056844a5ef4efe9ee8629dd0b4cde3e8cc1 Mon Sep 17 00:00:00 2001 From: Valerio Date: Tue, 10 Dec 2024 12:29:32 +0100 Subject: [PATCH 7/9] Fix the calculation of the conversion factor. Note that the conversion factor is now calculated using the reference frequency read from odf files. This should be the uplink frequency. Add the set_transponder_delay method to the ObservationCollection class. Changes: - modified: include/tudat/astro/observation_models/dsnNWayRangeObservationModel.h - modified: include/tudat/astro/observation_models/lightTimeSolution.h - modified: include/tudat/astro/observation_models/observationModel.h - modified: include/tudat/simulation/estimation_setup/createObservationModel.h - modified: include/tudat/simulation/estimation_setup/observations.h - modified: include/tudat/simulation/estimation_setup/processOdfFile.h --- .../dsnNWayRangeObservationModel.h | 5 - .../observation_models/lightTimeSolution.h | 2 - .../observation_models/observationModel.h | 2 - .../estimation_setup/createObservationModel.h | 7 +- .../estimation_setup/observations.h | 3162 +++++++++++------ .../estimation_setup/processOdfFile.h | 75 +- 6 files changed, 2067 insertions(+), 1186 deletions(-) diff --git a/include/tudat/astro/observation_models/dsnNWayRangeObservationModel.h b/include/tudat/astro/observation_models/dsnNWayRangeObservationModel.h index 0dfab7ca1e..029fc9aea0 100644 --- a/include/tudat/astro/observation_models/dsnNWayRangeObservationModel.h +++ b/include/tudat/astro/observation_models/dsnNWayRangeObservationModel.h @@ -18,7 +18,6 @@ #include #include -#include "tudat/astro/basic_astro/physicalConstants.h" #include "tudat/astro/observation_models/nWayRangeObservationModel.h" #include "tudat/astro/observation_models/observableTypes.h" #include "tudat/astro/observation_models/observationFrequencies.h" @@ -198,10 +197,6 @@ class DsnNWayRangeObservationModel : public ObservationModel< 1, ObservationScal utcTransmissionTime, utcReceptionTime ); ObservationScalarType rangeUnitIntegral = conversionFactor * transmitterFrequencyIntegral; - ancillarySettings->setAncilliaryDoubleData( - range_conversion_factor, - 1 / rangeUnitIntegral * physical_constants::SPEED_OF_LIGHT ); - // Moyer (2000), eq. 13-54 Eigen::Matrix< ObservationScalarType, 1, 1 > observation = ( Eigen::Matrix< ObservationScalarType, 1, 1 >( ) diff --git a/include/tudat/astro/observation_models/lightTimeSolution.h b/include/tudat/astro/observation_models/lightTimeSolution.h index 1e3d5faadf..adaece7144 100644 --- a/include/tudat/astro/observation_models/lightTimeSolution.h +++ b/include/tudat/astro/observation_models/lightTimeSolution.h @@ -871,8 +871,6 @@ class MultiLegLightTimeCalculator link_ends_delays, false ); } - linkEndsDelays_[1] = 1.4149E-6; - if ( !linkEndsDelays_.empty( ) ) { // Delays vector already including delays at receiving and transmitting stations diff --git a/include/tudat/astro/observation_models/observationModel.h b/include/tudat/astro/observation_models/observationModel.h index 4e61a5dcb7..a309e7fe73 100644 --- a/include/tudat/astro/observation_models/observationModel.h +++ b/include/tudat/astro/observation_models/observationModel.h @@ -303,7 +303,6 @@ inline std::shared_ptr< ObservationAncilliarySimulationSettings > getDsnNWayRang const std::vector< FrequencyBands > &frequencyBands, const double referenceFrequency, const double lowestRangingComponent, - const double rangeConversionFactor = 0.0, const std::vector< double > linkEndsDelays = std::vector< double >( ) ) { @@ -314,7 +313,6 @@ inline std::shared_ptr< ObservationAncilliarySimulationSettings > getDsnNWayRang lowestRangingComponent ); ancillarySettings->setAncilliaryDoubleData( sequential_range_reference_frequency, referenceFrequency ); - ancillarySettings->setAncilliaryDoubleData( range_conversion_factor, rangeConversionFactor ); ancillarySettings->setAncilliaryDoubleVectorData( frequency_bands, convertFrequencyBandsToDoubleVector( frequencyBands ) ); diff --git a/include/tudat/simulation/estimation_setup/createObservationModel.h b/include/tudat/simulation/estimation_setup/createObservationModel.h index 46caa9281a..60fd7570f4 100644 --- a/include/tudat/simulation/estimation_setup/createObservationModel.h +++ b/include/tudat/simulation/estimation_setup/createObservationModel.h @@ -1057,9 +1057,10 @@ class DsnNWayRangeObservationSettings : public ObservationModelSettings /*! * Constructor for different light-time corrections per link * \param oneWayRangeObservationSettings List of settings for the one-way range observation - * models \param biasSettings Settings for the observation bias model that is to be used - * (default none: nullptr) \param lightTimeConvergenceCriteria Settings for the light-time - * convergence criteria + * models + * \param biasSettings Settings for the observation bias model that is to be used (default none: + * nullptr) + * \param lightTimeConvergenceCriteria Settings for the light-time convergence criteria */ DsnNWayRangeObservationSettings( const std::vector< std::shared_ptr< ObservationModelSettings > > diff --git a/include/tudat/simulation/estimation_setup/observations.h b/include/tudat/simulation/estimation_setup/observations.h index a27a6de38f..89ccba5be0 100644 --- a/include/tudat/simulation/estimation_setup/observations.h +++ b/include/tudat/simulation/estimation_setup/observations.h @@ -11,20 +11,17 @@ #ifndef TUDAT_OBSERVATIONS_H #define TUDAT_OBSERVATIONS_H -#include - -#include -#include - #include +#include +#include +#include +#include "tudat/astro/observation_models/linkTypeDefs.h" +#include "tudat/astro/observation_models/observableTypes.h" #include "tudat/basics/basicTypedefs.h" #include "tudat/basics/timeType.h" #include "tudat/basics/tudatTypeTraits.h" #include "tudat/basics/utilities.h" - -#include "tudat/astro/observation_models/linkTypeDefs.h" -#include "tudat/astro/observation_models/observableTypes.h" #include "tudat/simulation/estimation_setup/observationOutput.h" #include "tudat/simulation/estimation_setup/observationsProcessing.h" @@ -36,85 +33,103 @@ namespace observation_models using namespace simulation_setup; -template< typename ObservationScalarType = double, typename TimeType = double, - typename std::enable_if< is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, int >::type = 0 > +template< typename ObservationScalarType = double, + typename TimeType = double, + typename std::enable_if< + is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, + int >::type = 0 > class SingleObservationSet { -public: + public: SingleObservationSet( const ObservableType observableType, const LinkDefinition& linkEnds, - const std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& observations, + const std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& + observations, const std::vector< TimeType > observationTimes, const LinkEndType referenceLinkEnd, - const std::vector< Eigen::VectorXd >& observationsDependentVariables = std::vector< Eigen::VectorXd >( ), - const std::shared_ptr< simulation_setup::ObservationDependentVariableCalculator > dependentVariableCalculator = nullptr, - const std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > ancilliarySettings = nullptr ): - observableType_( observableType ), - linkEnds_( linkEnds ), - observations_( observations ), - observationTimes_( observationTimes ), - referenceLinkEnd_( referenceLinkEnd ), + const std::vector< Eigen::VectorXd >& observationsDependentVariables = + std::vector< Eigen::VectorXd >( ), + const std::shared_ptr< simulation_setup::ObservationDependentVariableCalculator > + dependentVariableCalculator = nullptr, + const std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > + ancilliarySettings = nullptr ) : + observableType_( observableType ), linkEnds_( linkEnds ), observations_( observations ), + observationTimes_( observationTimes ), referenceLinkEnd_( referenceLinkEnd ), observationsDependentVariables_( observationsDependentVariables ), dependentVariableCalculator_( dependentVariableCalculator ), - ancilliarySettings_( ancilliarySettings ), - numberOfObservations_( observations_.size( ) ) + ancilliarySettings_( ancilliarySettings ), numberOfObservations_( observations_.size( ) ) { if( dependentVariableCalculator_ != nullptr ) { if( dependentVariableCalculator_->getObservableType( ) != observableType_ ) { - std::cout<getObservableType( )<<" "<getObservableType( ) << " " + << observableType_ << std::endl; + throw std::runtime_error( + "Error when creating SingleObservationSet, " + "ObservationDependentVariableCalculator has incompatible type " ); } if( !( dependentVariableCalculator_->getLinkEnds( ) == linkEnds ) ) { - throw std::runtime_error( "Error when creating SingleObservationSet, ObservationDependentVariableCalculator has incompatible link ends " ); + throw std::runtime_error( + "Error when creating SingleObservationSet, " + "ObservationDependentVariableCalculator has incompatible link ends " ); } } if( observations_.size( ) != observationTimes_.size( ) ) { - throw std::runtime_error( "Error when making SingleObservationSet, input sizes are inconsistent." + - std::to_string( observations_.size( ) ) + ", " + std::to_string( observationTimes_.size( ) ) ); + throw std::runtime_error( + "Error when making SingleObservationSet, input sizes are inconsistent." + + std::to_string( observations_.size( ) ) + ", " + + std::to_string( observationTimes_.size( ) ) ); } for( unsigned int i = 1; i < observations.size( ); i++ ) { if( observations.at( i ).rows( ) != observations.at( i - 1 ).rows( ) ) { - throw std::runtime_error( "Error when making SingleObservationSet, input observables not of consistent size." ); + throw std::runtime_error( + "Error when making SingleObservationSet, input observables not of " + "consistent size." ); } } - singleObservationSize_ = getObservableSize( observableType ); // Initialise weights - for ( unsigned int k = 0 ; k < numberOfObservations_ ; k++ ) + for( unsigned int k = 0; k < numberOfObservations_; k++ ) { - weights_.push_back( Eigen::Matrix< double, Eigen::Dynamic, 1 >::Ones( singleObservationSize_, 1 ) ); + weights_.push_back( + Eigen::Matrix< double, Eigen::Dynamic, 1 >::Ones( singleObservationSize_, 1 ) ); } // Initialise residuals - for ( unsigned int k = 0 ; k < numberOfObservations_ ; k++ ) + for( unsigned int k = 0; k < numberOfObservations_; k++ ) { - residuals_.push_back( Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( singleObservationSize_, 1 ) ); + residuals_.push_back( Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( + singleObservationSize_, 1 ) ); } // Check observation dependent variables size - if ( observationsDependentVariables_.size( ) > 0 ) + if( observationsDependentVariables_.size( ) > 0 ) { - if ( observationsDependentVariables_.size( ) != numberOfObservations_ ) + if( observationsDependentVariables_.size( ) != numberOfObservations_ ) { - throw std::runtime_error( "Error when creating SingleObservationSet, the size of the observation dependent variables input should be consistent " - "with the number of observations." ); + throw std::runtime_error( + "Error when creating SingleObservationSet, the size of the observation " + "dependent variables input should be consistent " + "with the number of observations." ); } - if ( observationsDependentVariables_[ 0 ].size( ) != dependentVariableCalculator_->getTotalDependentVariableSize( ) ) + if( observationsDependentVariables_[ 0 ].size( ) != + dependentVariableCalculator_->getTotalDependentVariableSize( ) ) { - throw std::runtime_error( "Error when creating SingleObservationSet, the size of the observation dependent variables input " - "should be consistent with the total dependent variable size." ); + throw std::runtime_error( + "Error when creating SingleObservationSet, the size of the observation " + "dependent variables input " + "should be consistent with the total dependent variable size." ); } } @@ -123,11 +138,8 @@ class SingleObservationSet // Initialise time bounds updateTimeBounds( ); - } - - ObservableType getObservableType( ) { return observableType_; @@ -148,71 +160,85 @@ class SingleObservationSet return observations_; } - void setObservations( const std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& observations ) + void setObservations( + const std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& + observations ) { - if ( observations.size( ) != observations_.size( ) ) + if( observations.size( ) != observations_.size( ) ) { - throw std::runtime_error( "Error when resetting observations, number of observations is incompatible." ); + throw std::runtime_error( + "Error when resetting observations, number of observations is incompatible." ); } observations_ = observations; - if ( !observations_.empty( ) ) + if( !observations_.empty( ) ) { singleObservationSize_ = observations_.at( 0 ).size( ); } } - void setObservations( const Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >& observationsVector ) + void setObservations( + const Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >& observationsVector ) { - if ( observationsVector.size( ) != numberOfObservations_ * singleObservationSize_ ) + if( observationsVector.size( ) != numberOfObservations_ * singleObservationSize_ ) { - throw std::runtime_error( "Error when resetting observations, number of observations is incompatible." ); + throw std::runtime_error( + "Error when resetting observations, number of observations is incompatible." ); } observations_.clear( ); - for ( unsigned int k = 0 ; k < numberOfObservations_ ; k++ ) + for( unsigned int k = 0; k < numberOfObservations_; k++ ) { - observations_.push_back( observationsVector.segment( k * singleObservationSize_, singleObservationSize_ ) ); + observations_.push_back( observationsVector.segment( k * singleObservationSize_, + singleObservationSize_ ) ); } - if ( !observations_.empty( ) ) + if( !observations_.empty( ) ) { singleObservationSize_ = observations_.at( 0 ).size( ); } } - void setResiduals( const std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& residuals ) + void setResiduals( + const std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& + residuals ) { - if ( residuals.size( ) != numberOfObservations_ ) + if( residuals.size( ) != numberOfObservations_ ) { - throw std::runtime_error( "Error when setting residuals, number of observations is inconsistent." ); + throw std::runtime_error( + "Error when setting residuals, number of observations is inconsistent." ); } residuals_ = residuals; } - void setResiduals( const Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >& residualsVector ) + void setResiduals( + const Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >& residualsVector ) { - if ( residualsVector.size( ) != numberOfObservations_ * singleObservationSize_ ) + if( residualsVector.size( ) != numberOfObservations_ * singleObservationSize_ ) { - throw std::runtime_error( "Error when setting residuals, number of observations is inconsistent." ); + throw std::runtime_error( + "Error when setting residuals, number of observations is inconsistent." ); } residuals_.clear( ); - for ( unsigned int k = 0 ; k < numberOfObservations_ ; k++ ) + for( unsigned int k = 0; k < numberOfObservations_; k++ ) { - residuals_.push_back( residualsVector.segment( k * singleObservationSize_, singleObservationSize_ ) ); + residuals_.push_back( + residualsVector.segment( k * singleObservationSize_, singleObservationSize_ ) ); } } - const std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& getObservationsReference( ) + const std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& + getObservationsReference( ) { return observations_; } - - Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > getObservation( const unsigned int index ) + Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > getObservation( + const unsigned int index ) { if( index >= numberOfObservations_ ) { - throw std::runtime_error( "Error when retrieving single observation, index is out of bounds" ); + throw std::runtime_error( + "Error when retrieving single observation, index is out of bounds" ); } return observations_.at( index ); } @@ -222,11 +248,14 @@ class SingleObservationSet { if( index >= numberOfObservations_ ) { - throw std::runtime_error( "Error when setting single observation value, index is out of bounds" ); + throw std::runtime_error( + "Error when setting single observation value, index is out of bounds" ); } - if ( observation.size( ) != singleObservationSize_ ) + if( observation.size( ) != singleObservationSize_ ) { - throw std::runtime_error( "Error when setting single observation value, the observation size is inconsistent." ); + throw std::runtime_error( + "Error when setting single observation value, the observation size is " + "inconsistent." ); } observations_.at( index ) = observation; } @@ -238,9 +267,11 @@ class SingleObservationSet TimeType getObservationTime( unsigned int index ) const { - if ( index >= numberOfObservations_ ) + if( index >= numberOfObservations_ ) { - throw std::runtime_error( "Error when retrieving single observation time, required index incompatible with number of observations." ); + throw std::runtime_error( + "Error when retrieving single observation time, required index incompatible " + "with number of observations." ); } return observationTimes_.at( index ); } @@ -273,31 +304,35 @@ class SingleObservationSet Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > getObservationsVector( ) { Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > observationsVector = - Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( singleObservationSize_ * numberOfObservations_, 1 ); + Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( + singleObservationSize_ * numberOfObservations_, 1 ); for( unsigned int i = 0; i < observations_.size( ); i++ ) { - observationsVector.segment( i * singleObservationSize_, singleObservationSize_ ) = observations_.at( i ); + observationsVector.segment( i * singleObservationSize_, singleObservationSize_ ) = + observations_.at( i ); } return observationsVector; } - std::pair< TimeType, TimeType > getTimeBounds( ) { if( observationTimes_.size( ) == 0 ) { - throw std::runtime_error( "Error when getting time bounds of observation set, no observations found" ); + throw std::runtime_error( + "Error when getting time bounds of observation set, no observations found" ); } return timeBounds_; } - std::map< TimeType, Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > getObservationsHistory( ) + std::map< TimeType, Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > + getObservationsHistory( ) { - return utilities::createMapFromVectors< TimeType, Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >( - observationTimes_, observations_ ); + return utilities::createMapFromVectors< + TimeType, + Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >( observationTimes_, + observations_ ); } - std::vector< Eigen::VectorXd > getObservationsDependentVariables( ) { return observationsDependentVariables_; @@ -305,10 +340,14 @@ class SingleObservationSet Eigen::MatrixXd getObservationsDependentVariablesMatrix( ) { - Eigen::MatrixXd dependentVariablesMatrix = Eigen::MatrixXd::Zero( numberOfObservations_, dependentVariableCalculator_->getTotalDependentVariableSize( ) ); - for ( unsigned int i = 0 ; i < observationsDependentVariables_.size( ) ; i++ ) + Eigen::MatrixXd dependentVariablesMatrix = Eigen::MatrixXd::Zero( + numberOfObservations_, + dependentVariableCalculator_->getTotalDependentVariableSize( ) ); + for( unsigned int i = 0; i < observationsDependentVariables_.size( ); i++ ) { - dependentVariablesMatrix.block( i, 0, 1, dependentVariableCalculator_->getTotalDependentVariableSize( ) ) = observationsDependentVariables_[ i ].transpose( ); + dependentVariablesMatrix.block( + i, 0, 1, dependentVariableCalculator_->getTotalDependentVariableSize( ) ) = + observationsDependentVariables_[ i ].transpose( ); } return dependentVariablesMatrix; } @@ -316,9 +355,11 @@ class SingleObservationSet //! Function returning the dependent variable values for a single observation (indicated by index) Eigen::VectorXd getDependentVariablesForSingleObservation( unsigned int index ) const { - if ( index >= numberOfObservations_ ) + if( index >= numberOfObservations_ ) { - throw std::runtime_error( "Error when retrieving observation dependent variables for single observation, required index incompatible with number of observations." ); + throw std::runtime_error( + "Error when retrieving observation dependent variables for single observation, " + "required index incompatible with number of observations." ); } return observationsDependentVariables_.at( index ); } @@ -329,47 +370,58 @@ class SingleObservationSet const bool returnFirstCompatibleSettings = false ) { // Retrieve full map of dependent variables start indices and sizes based on settings - std::map< std::pair< int, int >, std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > > settingsIndicesAndSizes = - dependentVariableCalculator_->getSettingsIndicesAndSizes( ); + std::map< std::pair< int, int >, + std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > > + settingsIndicesAndSizes = + dependentVariableCalculator_->getSettingsIndicesAndSizes( ); - // Get the start indices and sizes of all dependent variables that would be compatible with the settings provided as inputs. + // Get the start indices and sizes of all dependent variables that would be compatible with + // the settings provided as inputs. std::vector< std::pair< int, int > > indicesAndSizes; - for ( auto it : settingsIndicesAndSizes ) + for( auto it: settingsIndicesAndSizes ) { - if ( dependentVariableSettings->areSettingsCompatible( it.second ) ) + if( dependentVariableSettings->areSettingsCompatible( it.second ) ) { indicesAndSizes.push_back( it.first ); } } // Check that a single settings is identified - if ( indicesAndSizes.size( ) == 0 ) + if( indicesAndSizes.size( ) == 0 ) { - throw std::runtime_error( "Error when getting dependent variable, no dependent variable values found for given settings." ); + throw std::runtime_error( + "Error when getting dependent variable, no dependent variable values found for " + "given settings." ); } - else if ( indicesAndSizes.size( ) > 1 && !returnFirstCompatibleSettings ) + else if( indicesAndSizes.size( ) > 1 && !returnFirstCompatibleSettings ) { - throw std::runtime_error( "Error when getting dependent variable, multiple dependent variables found for given settings." ); + throw std::runtime_error( + "Error when getting dependent variable, multiple dependent variables found for " + "given settings." ); } - // Return the dependent variable values for the first (and/or single) compatible settings identified + // Return the dependent variable values for the first (and/or single) compatible settings + // identified return getSingleDependentVariable( indicesAndSizes.at( 0 ) ); } //! Function returning the list of all dependent variable settings compatible with the settings provided as inputs //! (which might not be fully defined, i.e. with missing link ends information, etc.) - std::vector< std::shared_ptr< ObservationDependentVariableSettings > > getCompatibleDependentVariablesSettingsList( - std::shared_ptr< ObservationDependentVariableSettings > dependentVariableSettings ) const + std::vector< std::shared_ptr< ObservationDependentVariableSettings > > + getCompatibleDependentVariablesSettingsList( + std::shared_ptr< ObservationDependentVariableSettings > dependentVariableSettings ) + const { // Retrieve all dependent variables settings for this observation set - std::vector< std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > > allDependentVariablesSettings = - dependentVariableCalculator_->getDependentVariableSettings( ); + std::vector< std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > > + allDependentVariablesSettings = + dependentVariableCalculator_->getDependentVariableSettings( ); // Check which settings are compatible with the input settings object std::vector< std::shared_ptr< ObservationDependentVariableSettings > > compatibleSettings; - for ( auto it : allDependentVariablesSettings ) + for( auto it: allDependentVariablesSettings ) { - if ( dependentVariableSettings->areSettingsCompatible( it ) ) + if( dependentVariableSettings->areSettingsCompatible( it ) ) { compatibleSettings.push_back( it ); } @@ -380,17 +432,20 @@ class SingleObservationSet //! Function returning a vector containing the values of all dependent variables compatible with the settings provided as input //! The order in which they are provided matches the list of compatible settings given by the getCompatibleDependentVariablesSettingsList function std::vector< Eigen::MatrixXd > getAllCompatibleDependentVariables( - std::shared_ptr< ObservationDependentVariableSettings > dependentVariableSettings ) const + std::shared_ptr< ObservationDependentVariableSettings > dependentVariableSettings ) + const { // Retrieve start indices and sizes for each dependent variable settings - std::map< std::pair< int, int >, std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > > settingsIndicesAndSizes = - dependentVariableCalculator_->getSettingsIndicesAndSizes( ); + std::map< std::pair< int, int >, + std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > > + settingsIndicesAndSizes = + dependentVariableCalculator_->getSettingsIndicesAndSizes( ); // Retrieve all relevant dependent variables std::vector< Eigen::MatrixXd > dependentVariablesList; - for ( auto it : settingsIndicesAndSizes ) + for( auto it: settingsIndicesAndSizes ) { - if ( dependentVariableSettings->areSettingsCompatible( it.second ) ) + if( dependentVariableSettings->areSettingsCompatible( it.second ) ) { dependentVariablesList.push_back( getSingleDependentVariable( it.first ) ); } @@ -407,23 +462,31 @@ class SingleObservationSet //! Function to reset the observation dependent variable values void setObservationsDependentVariables( std::vector< Eigen::VectorXd >& dependentVariables ) { - if ( observationsDependentVariables_.size( ) > 0 ) + if( observationsDependentVariables_.size( ) > 0 ) { - if ( observationsDependentVariables_.size( ) != numberOfObservations_ ) + if( observationsDependentVariables_.size( ) != numberOfObservations_ ) { - throw std::runtime_error( "Error when resetting observation dependent variables in SingleObservationSet, the input size should be consistent " - "with the number of observations." ); + throw std::runtime_error( + "Error when resetting observation dependent variables in " + "SingleObservationSet, the input size should be consistent " + "with the number of observations." ); } - if ( ( dependentVariableCalculator_ != nullptr ) && ( dependentVariables[ 0 ].size( ) != dependentVariableCalculator_->getTotalDependentVariableSize( ) ) ) + if( ( dependentVariableCalculator_ != nullptr ) && + ( dependentVariables[ 0 ].size( ) != + dependentVariableCalculator_->getTotalDependentVariableSize( ) ) ) { - throw std::runtime_error( "Error when resetting observation dependent variables in SingleObservationSet, the size of the observation dependent variables input " - "should be consistent with the total dependent variable size." ); + throw std::runtime_error( + "Error when resetting observation dependent variables in " + "SingleObservationSet, the size of the observation dependent variables " + "input " + "should be consistent with the total dependent variable size." ); } } observationsDependentVariables_ = dependentVariables; } - std::shared_ptr< simulation_setup::ObservationDependentVariableCalculator > getDependentVariableCalculator( ) + std::shared_ptr< simulation_setup::ObservationDependentVariableCalculator > + getDependentVariableCalculator( ) { return dependentVariableCalculator_; } @@ -433,7 +496,7 @@ class SingleObservationSet std::map< TimeType, Eigen::VectorXd > getDependentVariableHistory( ) { return utilities::createMapFromVectors< TimeType, Eigen::VectorXd >( - observationTimes_, observationsDependentVariables_ ); + observationTimes_, observationsDependentVariables_ ); } //! Function that returns the time history of a single dependent variables (specified by settings). It must be noted that the reported epochs are the times @@ -442,17 +505,22 @@ class SingleObservationSet std::shared_ptr< ObservationDependentVariableSettings > dependentVariableSettings, const bool returnFirstCompatibleSettings = false ) { - Eigen::MatrixXd singleDependentVariableValues = getSingleDependentVariable( dependentVariableSettings, returnFirstCompatibleSettings ); + Eigen::MatrixXd singleDependentVariableValues = getSingleDependentVariable( + dependentVariableSettings, returnFirstCompatibleSettings ); std::map< TimeType, Eigen::VectorXd > singleDependentVariableMap; - for ( unsigned int i = 0 ; i < numberOfObservations_ ; i++ ) + for( unsigned int i = 0; i < numberOfObservations_; i++ ) { - Eigen::VectorXd dependentVariableCurrentTime = singleDependentVariableValues.block( i, 0, 1, singleDependentVariableValues.cols( ) ).transpose( ); + Eigen::VectorXd dependentVariableCurrentTime = + singleDependentVariableValues + .block( i, 0, 1, singleDependentVariableValues.cols( ) ) + .transpose( ); singleDependentVariableMap[ observationTimes_[ i ] ] = dependentVariableCurrentTime; } return singleDependentVariableMap; } - std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > getAncilliarySettings( ) + std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > + getAncilliarySettings( ) { return ancilliarySettings_; } @@ -470,19 +538,23 @@ class SingleObservationSet Eigen::VectorXd getWeightsVector( ) const { Eigen::Matrix< double, Eigen::Dynamic, 1 > weightsVector = - Eigen::Matrix< double, Eigen::Dynamic, 1 >::Zero( singleObservationSize_ * numberOfObservations_, 1 ); - for( unsigned int i = 0; i < numberOfObservations_ ; i++ ) + Eigen::Matrix< double, Eigen::Dynamic, 1 >::Zero( + singleObservationSize_ * numberOfObservations_, 1 ); + for( unsigned int i = 0; i < numberOfObservations_; i++ ) { - weightsVector.block( i * singleObservationSize_, 0, singleObservationSize_, 1 ) = weights_.at( i ); + weightsVector.block( i * singleObservationSize_, 0, singleObservationSize_, 1 ) = + weights_.at( i ); } return weightsVector; } Eigen::Matrix< double, Eigen::Dynamic, 1 > getWeight( unsigned int index ) const { - if ( index >= numberOfObservations_ ) + if( index >= numberOfObservations_ ) { - throw std::runtime_error( "Error when retrieving single observation weight, required index incompatible with number of observations." ); + throw std::runtime_error( + "Error when retrieving single observation weight, required index incompatible " + "with number of observations." ); } return weights_.at( index ); } @@ -495,24 +567,30 @@ class SingleObservationSet Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > getResidualsVector( ) const { Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > residualsVector = - Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( singleObservationSize_ * numberOfObservations_, 1 ); - for( unsigned int i = 0; i < numberOfObservations_ ; i++ ) + Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( + singleObservationSize_ * numberOfObservations_, 1 ); + for( unsigned int i = 0; i < numberOfObservations_; i++ ) { - residualsVector.block( i * singleObservationSize_, 0, singleObservationSize_, 1 ) = residuals_.at( i ); + residualsVector.block( i * singleObservationSize_, 0, singleObservationSize_, 1 ) = + residuals_.at( i ); } return residualsVector; } - const std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& getResidualsReference( ) + const std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& + getResidualsReference( ) { return residuals_; } - Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > getResidual( unsigned int index ) const + Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > getResidual( + unsigned int index ) const { - if ( index >= numberOfObservations_ ) + if( index >= numberOfObservations_ ) { - throw std::runtime_error( "Error when retrieving single observation residual, required index incompatible with number of observations." ); + throw std::runtime_error( + "Error when retrieving single observation residual, required index " + "incompatible with number of observations." ); } return residuals_.at( index ); } @@ -520,10 +598,10 @@ class SingleObservationSet Eigen::VectorXd getRmsResiduals( ) { Eigen::VectorXd rmsResiduals = Eigen::VectorXd::Zero( singleObservationSize_ ); - for ( unsigned int i = 0 ; i < singleObservationSize_ ; i++ ) + for( unsigned int i = 0; i < singleObservationSize_; i++ ) { // Calculate RMS of the residuals for each observation component - for( int j = 0; j < numberOfObservations_ ; j++ ) + for( int j = 0; j < numberOfObservations_; j++ ) { rmsResiduals[ i ] += residuals_[ j ]( i, 0 ) * residuals_[ j ]( i, 0 ); } @@ -536,10 +614,10 @@ class SingleObservationSet Eigen::VectorXd getMeanResiduals( ) { Eigen::VectorXd meanResiduals = Eigen::VectorXd::Zero( singleObservationSize_ ); - for ( unsigned int i = 0 ; i < singleObservationSize_ ; i++ ) + for( unsigned int i = 0; i < singleObservationSize_; i++ ) { // Calculate mean residual for each observation component - for ( unsigned int j = 0 ; j < numberOfObservations_ ; j++ ) + for( unsigned int j = 0; j < numberOfObservations_; j++ ) { meanResiduals[ i ] += residuals_[ j ]( i, 0 ); } @@ -550,19 +628,22 @@ class SingleObservationSet void setConstantWeight( const double weight ) { - for ( unsigned int k = 0 ; k < numberOfObservations_ ; k++ ) + for( unsigned int k = 0; k < numberOfObservations_; k++ ) { - weights_.at( k ) = weight * Eigen::Matrix< double, Eigen::Dynamic, 1 >::Ones( singleObservationSize_, 1 ); + weights_.at( k ) = weight * + Eigen::Matrix< double, Eigen::Dynamic, 1 >::Ones( singleObservationSize_, 1 ); } } void setConstantWeight( const Eigen::Matrix< double, Eigen::Dynamic, 1 >& weight ) { - if ( weight.size( ) != singleObservationSize_ ) + if( weight.size( ) != singleObservationSize_ ) { - throw std::runtime_error( "Error when setting constant weight in single observation set, weight size is inconsistent with single observation size." ); + throw std::runtime_error( + "Error when setting constant weight in single observation set, weight size is " + "inconsistent with single observation size." ); } - for ( unsigned int k = 0 ; k < weights_.size( ) ; k++ ) + for( unsigned int k = 0; k < weights_.size( ); k++ ) { weights_.at( k ) = weight; } @@ -570,24 +651,28 @@ class SingleObservationSet void setTabulatedWeights( const Eigen::VectorXd& weightsVector ) { - if( weightsVector.rows( ) != static_cast< int >( singleObservationSize_ * observations_.size( ) ) ) + if( weightsVector.rows( ) != + static_cast< int >( singleObservationSize_ * observations_.size( ) ) ) { - throw std::runtime_error( "Error when setting weights in single observation set, sizes are incompatible." ); + throw std::runtime_error( + "Error when setting weights in single observation set, sizes are " + "incompatible." ); } - for ( unsigned int k = 0 ; k < numberOfObservations_ ; k++ ) + for( unsigned int k = 0; k < numberOfObservations_; k++ ) { - for ( unsigned int i = 0 ; i < singleObservationSize_ ; i++ ) + for( unsigned int i = 0; i < singleObservationSize_; i++ ) { weights_.at( k )[ i ] = weightsVector[ k * singleObservationSize_ + i ]; } } } - - std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > getComputedObservations( ) const + std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > + getComputedObservations( ) const { - std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > computedObservations; - for ( unsigned int k = 0 ; k < observations_.size( ) ; k++ ) + std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > + computedObservations; + for( unsigned int k = 0; k < observations_.size( ); k++ ) { computedObservations.push_back( observations_.at( k ) - residuals_.at( k ) ); } @@ -597,12 +682,16 @@ class SingleObservationSet Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > getComputedObservationsVector( ) const { Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > computedObservationsVector = - Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( singleObservationSize_ * numberOfObservations_, 1 ); + Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( + singleObservationSize_ * numberOfObservations_, 1 ); - std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > computedObservations = getComputedObservations( ); - for( unsigned int i = 0; i < numberOfObservations_ ; i++ ) + std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > + computedObservations = getComputedObservations( ); + for( unsigned int i = 0; i < numberOfObservations_; i++ ) { - computedObservationsVector.segment( i * singleObservationSize_, singleObservationSize_ ) = computedObservations.at( i ); + computedObservationsVector.segment( i * singleObservationSize_, + singleObservationSize_ ) = + computedObservations.at( i ); } return computedObservationsVector; } @@ -610,24 +699,26 @@ class SingleObservationSet unsigned int getNumberOfFilteredObservations( ) const { int numberFilteredObservations = 0; - if ( filteredObservationSet_ != nullptr ) + if( filteredObservationSet_ != nullptr ) { numberFilteredObservations = filteredObservationSet_->getNumberOfObservables( ); } return numberFilteredObservations; } - std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > getFilteredObservationSet( ) const + std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > + getFilteredObservationSet( ) const { return filteredObservationSet_; } - void removeSingleObservation( unsigned int indexToRemove ) { - if ( indexToRemove >= numberOfObservations_ ) + if( indexToRemove >= numberOfObservations_ ) { - throw std::runtime_error( "Error when removing single observation from SingleObservationSet, index incompatible with number of observations." ); + throw std::runtime_error( + "Error when removing single observation from SingleObservationSet, index " + "incompatible with number of observations." ); } // Update observations @@ -636,9 +727,10 @@ class SingleObservationSet residuals_.erase( residuals_.begin( ) + indexToRemove ); weights_.erase( weights_.begin( ) + indexToRemove ); - if ( observationsDependentVariables_.size( ) > 0 ) + if( observationsDependentVariables_.size( ) > 0 ) { - observationsDependentVariables_.erase( observationsDependentVariables_.begin( ) + indexToRemove ); + observationsDependentVariables_.erase( observationsDependentVariables_.begin( ) + + indexToRemove ); } // Update number of observations and time bounds @@ -649,58 +741,91 @@ class SingleObservationSet void removeObservations( const std::vector< unsigned int >& indicesToRemove ) { unsigned int counter = 0; - for ( auto ind : indicesToRemove ) + for( auto ind: indicesToRemove ) { - removeSingleObservation( ind - counter ); // observations are already filtered and sorted + removeSingleObservation( ind - + counter ); // observations are already filtered and sorted counter += 1; } } - void filterObservations( const std::shared_ptr< ObservationFilterBase > observationFilter, const bool saveFilteredObservations = true ) + void filterObservations( const std::shared_ptr< ObservationFilterBase > observationFilter, + const bool saveFilteredObservations = true ) { - if ( observationFilter->filterOut( ) && filteredObservationSet_ == nullptr ) + if( observationFilter->filterOut( ) && filteredObservationSet_ == nullptr ) { // Initialise empty filtered observation set - filteredObservationSet_ = std::make_shared >( - observableType_, linkEnds_, std::vector >( ), - std::vector< TimeType >( ), referenceLinkEnd_, std::vector< Eigen::VectorXd >( ), dependentVariableCalculator_, ancilliarySettings_); - } - if ( !observationFilter->filterOut( ) && filteredObservationSet_ == nullptr ) - { - throw std::runtime_error( "Error when attempting to un-filter observations, filtered observation set is empty." ); - } - - unsigned int nbObservationsToTest = ( observationFilter->filterOut( ) ? numberOfObservations_ : filteredObservationSet_->getNumberOfObservables( ) ); + filteredObservationSet_ = std::make_shared< + SingleObservationSet< ObservationScalarType, TimeType > >( + observableType_, + linkEnds_, + std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >( ), + std::vector< TimeType >( ), + referenceLinkEnd_, + std::vector< Eigen::VectorXd >( ), + dependentVariableCalculator_, + ancilliarySettings_ ); + } + if( !observationFilter->filterOut( ) && filteredObservationSet_ == nullptr ) + { + throw std::runtime_error( + "Error when attempting to un-filter observations, filtered observation set is " + "empty." ); + } + + unsigned int nbObservationsToTest = + ( observationFilter->filterOut( ) + ? numberOfObservations_ + : filteredObservationSet_->getNumberOfObservables( ) ); bool useOppositeCondition = observationFilter->useOppositeCondition( ); std::vector< unsigned int > indicesToRemove; - switch ( observationFilter->getFilterType( ) ) + switch( observationFilter->getFilterType( ) ) { - case residual_filtering: - { + case residual_filtering: { Eigen::VectorXd residualCutOff = Eigen::VectorXd::Zero( singleObservationSize_ ); - if ( std::dynamic_pointer_cast< ObservationFilter< double > >( observationFilter ) != nullptr ) + if( std::dynamic_pointer_cast< ObservationFilter< double > >( observationFilter ) != + nullptr ) { - residualCutOff = std::dynamic_pointer_cast< ObservationFilter< double > >( observationFilter )->getFilterValue( ) * Eigen::VectorXd::Ones( singleObservationSize_ ); + residualCutOff = std::dynamic_pointer_cast< ObservationFilter< double > >( + observationFilter ) + ->getFilterValue( ) * + Eigen::VectorXd::Ones( singleObservationSize_ ); } - else if ( std::dynamic_pointer_cast< ObservationFilter< Eigen::VectorXd > >( observationFilter ) != nullptr ) + else if( std::dynamic_pointer_cast< ObservationFilter< Eigen::VectorXd > >( + observationFilter ) != nullptr ) { - if ( std::dynamic_pointer_cast< ObservationFilter< Eigen::VectorXd > >( observationFilter )->getFilterValue( ).size( ) != singleObservationSize_ ) + if( std::dynamic_pointer_cast< ObservationFilter< Eigen::VectorXd > >( + observationFilter ) + ->getFilterValue( ) + .size( ) != singleObservationSize_ ) { - throw std::runtime_error( "Error when performing residual filtering, size of the residual cut off vector inconsistent with observable size." ); + throw std::runtime_error( + "Error when performing residual filtering, size of the residual " + "cut off vector inconsistent with observable size." ); } - residualCutOff = std::dynamic_pointer_cast< ObservationFilter< Eigen::VectorXd > >( observationFilter )->getFilterValue( ); + residualCutOff = + std::dynamic_pointer_cast< ObservationFilter< Eigen::VectorXd > >( + observationFilter ) + ->getFilterValue( ); } - for ( unsigned int j = 0 ; j < nbObservationsToTest ; j++ ) + for( unsigned int j = 0; j < nbObservationsToTest; j++ ) { - Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > singleObservationResidual = - ( observationFilter->filterOut( ) ? residuals_.at( j ) : filteredObservationSet_->getResidual( j ) ); + Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > + singleObservationResidual = + ( observationFilter->filterOut( ) + ? residuals_.at( j ) + : filteredObservationSet_->getResidual( j ) ); bool removeObservation = false; - for( unsigned int k = 0 ; k < singleObservationSize_ ; k++ ) + for( unsigned int k = 0; k < singleObservationSize_; k++ ) { - if( ( !useOppositeCondition && ( std::fabs( singleObservationResidual[ k ] ) > residualCutOff[ k ] ) ) || - ( useOppositeCondition && ( std::fabs( singleObservationResidual[ k ] ) <= residualCutOff[ k ] ) ) ) + if( ( !useOppositeCondition && + ( std::fabs( singleObservationResidual[ k ] ) > + residualCutOff[ k ] ) ) || + ( useOppositeCondition && + ( std::fabs( singleObservationResidual[ k ] ) <= + residualCutOff[ k ] ) ) ) { removeObservation = true; } @@ -712,31 +837,48 @@ class SingleObservationSet } break; } - case absolute_value_filtering: - { - Eigen::VectorXd absoluteValueCutOff = Eigen::VectorXd::Zero( singleObservationSize_ ); - if ( std::dynamic_pointer_cast< ObservationFilter< double > >( observationFilter ) != nullptr ) + case absolute_value_filtering: { + Eigen::VectorXd absoluteValueCutOff = + Eigen::VectorXd::Zero( singleObservationSize_ ); + if( std::dynamic_pointer_cast< ObservationFilter< double > >( observationFilter ) != + nullptr ) { - absoluteValueCutOff = std::dynamic_pointer_cast< ObservationFilter< double > >( observationFilter )->getFilterValue( ) * Eigen::VectorXd::Ones( singleObservationSize_ ); + absoluteValueCutOff = std::dynamic_pointer_cast< ObservationFilter< double > >( + observationFilter ) + ->getFilterValue( ) * + Eigen::VectorXd::Ones( singleObservationSize_ ); } - else if ( std::dynamic_pointer_cast< ObservationFilter< Eigen::VectorXd > >( observationFilter ) != nullptr ) + else if( std::dynamic_pointer_cast< ObservationFilter< Eigen::VectorXd > >( + observationFilter ) != nullptr ) { - if ( std::dynamic_pointer_cast< ObservationFilter< Eigen::VectorXd > >( observationFilter )->getFilterValue( ).size( ) != singleObservationSize_ ) + if( std::dynamic_pointer_cast< ObservationFilter< Eigen::VectorXd > >( + observationFilter ) + ->getFilterValue( ) + .size( ) != singleObservationSize_ ) { - throw std::runtime_error( "Error when performing observation value filtering, size of the filter value inconsistent with observable size." ); + throw std::runtime_error( + "Error when performing observation value filtering, size of the " + "filter value inconsistent with observable size." ); } - absoluteValueCutOff = std::dynamic_pointer_cast< ObservationFilter< Eigen::VectorXd > >( observationFilter )->getFilterValue( ); + absoluteValueCutOff = + std::dynamic_pointer_cast< ObservationFilter< Eigen::VectorXd > >( + observationFilter ) + ->getFilterValue( ); } - for ( unsigned int j = 0 ; j < nbObservationsToTest ; j++ ) + for( unsigned int j = 0; j < nbObservationsToTest; j++ ) { Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > singleObservation = - ( observationFilter->filterOut( ) ? observations_.at( j ) : filteredObservationSet_->getObservation( j ) ); + ( observationFilter->filterOut( ) + ? observations_.at( j ) + : filteredObservationSet_->getObservation( j ) ); bool removeObservation = false; - for( unsigned int k = 0 ; k < singleObservationSize_ ; k++ ) + for( unsigned int k = 0; k < singleObservationSize_; k++ ) { - if ( ( !useOppositeCondition && ( singleObservation[ k ] > absoluteValueCutOff[ k ] ) ) || - ( useOppositeCondition && ( singleObservation[ k ] <= absoluteValueCutOff[ k ] ) ) ) + if( ( !useOppositeCondition && + ( singleObservation[ k ] > absoluteValueCutOff[ k ] ) ) || + ( useOppositeCondition && + ( singleObservation[ k ] <= absoluteValueCutOff[ k ] ) ) ) { removeObservation = true; } @@ -748,70 +890,112 @@ class SingleObservationSet } break; } - case epochs_filtering: - { - std::vector< double > filterEpochs = std::dynamic_pointer_cast< ObservationFilter< std::vector< double > > >( observationFilter )->getFilterValue( ); - for ( unsigned int j = 0 ; j < nbObservationsToTest ; j++ ) + case epochs_filtering: { + std::vector< double > filterEpochs = + std::dynamic_pointer_cast< ObservationFilter< std::vector< double > > >( + observationFilter ) + ->getFilterValue( ); + for( unsigned int j = 0; j < nbObservationsToTest; j++ ) { - TimeType singleObservationTime = ( observationFilter->filterOut( ) ? observationTimes_.at( j ) : filteredObservationSet_->getObservationTime( j ) ); - if ( ( !useOppositeCondition && ( std::count( filterEpochs.begin( ), filterEpochs.end( ), singleObservationTime ) > 0 ) ) || - ( useOppositeCondition && ( std::count( filterEpochs.begin( ), filterEpochs.end( ), singleObservationTime ) == 0 ) ) ) + TimeType singleObservationTime = + ( observationFilter->filterOut( ) + ? observationTimes_.at( j ) + : filteredObservationSet_->getObservationTime( j ) ); + if( ( !useOppositeCondition && + ( std::count( filterEpochs.begin( ), + filterEpochs.end( ), + singleObservationTime ) > 0 ) ) || + ( useOppositeCondition && + ( std::count( filterEpochs.begin( ), + filterEpochs.end( ), + singleObservationTime ) == 0 ) ) ) { indicesToRemove.push_back( j ); } } break; } - case time_bounds_filtering: - { - std::pair< double, double > timeBounds = std::dynamic_pointer_cast< ObservationFilter< std::pair< double, double > > >( observationFilter )->getFilterValue( ); - for ( unsigned int j = 0 ; j < nbObservationsToTest ; j++ ) + case time_bounds_filtering: { + std::pair< double, double > timeBounds = + std::dynamic_pointer_cast< + ObservationFilter< std::pair< double, double > > >( + observationFilter ) + ->getFilterValue( ); + for( unsigned int j = 0; j < nbObservationsToTest; j++ ) { - TimeType singleObservationTime = ( observationFilter->filterOut( ) ? observationTimes_.at( j ) : filteredObservationSet_->getObservationTime( j ) ); - if ( ( !useOppositeCondition && ( ( singleObservationTime >= timeBounds.first ) && ( singleObservationTime <= timeBounds.second ) ) ) || - ( useOppositeCondition && ( ( singleObservationTime < timeBounds.first ) || ( singleObservationTime > timeBounds.second ) ) ) ) + TimeType singleObservationTime = + ( observationFilter->filterOut( ) + ? observationTimes_.at( j ) + : filteredObservationSet_->getObservationTime( j ) ); + if( ( !useOppositeCondition && + ( ( singleObservationTime >= timeBounds.first ) && + ( singleObservationTime <= timeBounds.second ) ) ) || + ( useOppositeCondition && + ( ( singleObservationTime < timeBounds.first ) || + ( singleObservationTime > timeBounds.second ) ) ) ) { indicesToRemove.push_back( j ); } } break; } - case dependent_variable_filtering: - { - if ( std::dynamic_pointer_cast< ObservationDependentVariableFilter >( observationFilter ) == nullptr ) + case dependent_variable_filtering: { + if( std::dynamic_pointer_cast< ObservationDependentVariableFilter >( + observationFilter ) == nullptr ) { - throw std::runtime_error( "Error when performing dependent variable filtering, inconsistent filter input (should be ObservationDependentVariableFilter object)." ); + throw std::runtime_error( + "Error when performing dependent variable filtering, inconsistent " + "filter input (should be ObservationDependentVariableFilter object)." ); } // Retrieve dependent variable settings and size std::shared_ptr< ObservationDependentVariableSettings > settings = - std::dynamic_pointer_cast< ObservationDependentVariableFilter >( observationFilter )->getDependentVariableSettings( ); - unsigned int dependentVariableSize = getObservationDependentVariableSize( settings, linkEnds_.linkEnds_ ); + std::dynamic_pointer_cast< ObservationDependentVariableFilter >( + observationFilter ) + ->getDependentVariableSettings( ); + unsigned int dependentVariableSize = + getObservationDependentVariableSize( settings, linkEnds_.linkEnds_ ); // Retrieve dependent variable cut-off value - Eigen::VectorXd dependentVariableCutOff = std::dynamic_pointer_cast< ObservationDependentVariableFilter >( observationFilter )->getFilterValue( ); - if ( dependentVariableCutOff.size( ) != dependentVariableSize ) + Eigen::VectorXd dependentVariableCutOff = + std::dynamic_pointer_cast< ObservationDependentVariableFilter >( + observationFilter ) + ->getFilterValue( ); + if( dependentVariableCutOff.size( ) != dependentVariableSize ) { - throw std::runtime_error( "Error when performing dependent variable filtering, size of the dependent variable cut off vector inconsistent with dependent variable size." ); + throw std::runtime_error( + "Error when performing dependent variable filtering, size of the " + "dependent variable cut off vector inconsistent with dependent " + "variable size." ); } // Retrieve dependent variable values Eigen::MatrixXd singleDependentVariableValues = - ( observationFilter->filterOut( ) ? getSingleDependentVariable( settings ) : filteredObservationSet_->getSingleDependentVariable( settings ) ); - if ( ( singleDependentVariableValues.rows( ) != nbObservationsToTest ) || ( singleDependentVariableValues.cols( ) != dependentVariableSize ) ) + ( observationFilter->filterOut( ) + ? getSingleDependentVariable( settings ) + : filteredObservationSet_->getSingleDependentVariable( + settings ) ); + if( ( singleDependentVariableValues.rows( ) != nbObservationsToTest ) || + ( singleDependentVariableValues.cols( ) != dependentVariableSize ) ) { - throw std::runtime_error( "Error when performing dependent variable filtering, size of observation dependent variables is inconsistent with the " - "number of observations and presupposed dependent variable size." ); + throw std::runtime_error( + "Error when performing dependent variable filtering, size of " + "observation dependent variables is inconsistent with the " + "number of observations and presupposed dependent variable size." ); } // Check dependent variable values against cut-off value - for ( unsigned int j = 0 ; j < nbObservationsToTest ; j++ ) + for( unsigned int j = 0; j < nbObservationsToTest; j++ ) { bool removeObservation = false; - for( unsigned int k = 0 ; k < dependentVariableSize ; k++ ) + for( unsigned int k = 0; k < dependentVariableSize; k++ ) { - if( ( !useOppositeCondition && ( singleDependentVariableValues( j, k ) ) > dependentVariableCutOff[ k ] ) || - ( useOppositeCondition && ( singleDependentVariableValues( j, k ) ) <= dependentVariableCutOff[ k ] ) ) + if( ( !useOppositeCondition && + ( singleDependentVariableValues( j, k ) ) > + dependentVariableCutOff[ k ] ) || + ( useOppositeCondition && + ( singleDependentVariableValues( j, k ) ) <= + dependentVariableCutOff[ k ] ) ) { removeObservation = true; } @@ -827,7 +1011,7 @@ class SingleObservationSet throw std::runtime_error( "Observation filter type not recognised." ); } - if ( observationFilter->filterOut( ) ) + if( observationFilter->filterOut( ) ) { moveObservationsInOutFilteredSet( indicesToRemove, true, saveFilteredObservations ); } @@ -838,61 +1022,76 @@ class SingleObservationSet } void addObservations( - const std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& observations, + const std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& + observations, const std::vector< TimeType >& times, const std::vector< Eigen::VectorXd >& dependentVariables = { }, const std::vector< Eigen::Matrix< double, Eigen::Dynamic, 1 > >& weights = { }, - const std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& residuals = { }, + const std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& + residuals = { }, const bool sortObservations = true ) { - if ( ( observations.size( ) != times.size( ) ) || - ( weights.size( ) > 0 && ( observations.size( ) != weights.size( ) ) ) || - ( residuals.size( ) > 0 && ( observations.size( ) != residuals.size( ) ) ) || - ( dependentVariables.size( ) > 0 && ( observations.size( ) != dependentVariables.size( ) ) ) ) + if( ( observations.size( ) != times.size( ) ) || + ( weights.size( ) > 0 && ( observations.size( ) != weights.size( ) ) ) || + ( residuals.size( ) > 0 && ( observations.size( ) != residuals.size( ) ) ) || + ( dependentVariables.size( ) > 0 && + ( observations.size( ) != dependentVariables.size( ) ) ) ) { - throw std::runtime_error( "Error when adding observations to SingleObservationSet, input sizes are inconsistent." ); + throw std::runtime_error( + "Error when adding observations to SingleObservationSet, input sizes are " + "inconsistent." ); } - for ( unsigned int k = 0 ; k < observations.size( ) ; k++ ) + for( unsigned int k = 0; k < observations.size( ); k++ ) { - if ( observations.at( k ).size( ) != singleObservationSize_ ) + if( observations.at( k ).size( ) != singleObservationSize_ ) { - throw std::runtime_error( "Error when adding observations to SingleObservationSet, new observation size is inconsistent." ); + throw std::runtime_error( + "Error when adding observations to SingleObservationSet, new observation " + "size is inconsistent." ); } observations_.push_back( observations.at( k ) ); observationTimes_.push_back( times.at( k ) ); // If residuals are provided as inputs - if ( residuals.size( ) > 0 ) + if( residuals.size( ) > 0 ) { - if ( residuals.at( k ).size( ) != singleObservationSize_ ) + if( residuals.at( k ).size( ) != singleObservationSize_ ) { - throw std::runtime_error( "Error when adding observations to SingleObservationSet, new residual size is inconsistent." ); + throw std::runtime_error( + "Error when adding observations to SingleObservationSet, new residual " + "size is inconsistent." ); } residuals_.push_back( residuals.at( k ) ); } - else // Otherwise, set to zero by default + else // Otherwise, set to zero by default { - residuals_.push_back( Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( singleObservationSize_, 1 ) ); + residuals_.push_back( + Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( + singleObservationSize_, 1 ) ); } // If weights are provided as inputs - if ( weights.size( ) > 0 ) + if( weights.size( ) > 0 ) { - if ( weights.at( k ).size( ) != singleObservationSize_ ) + if( weights.at( k ).size( ) != singleObservationSize_ ) { - throw std::runtime_error( "Error when adding observations to SingleObservationSet, new weight size is inconsistent." ); + throw std::runtime_error( + "Error when adding observations to SingleObservationSet, new weight " + "size is inconsistent." ); } weights_.push_back( weights.at( k ) ); } - else // Otherwise, set to one by default + else // Otherwise, set to one by default { - weights_.push_back( Eigen::Matrix< double, Eigen::Dynamic, 1 >::Ones( singleObservationSize_, 1 ) ); + weights_.push_back( Eigen::Matrix< double, Eigen::Dynamic, 1 >::Ones( + singleObservationSize_, 1 ) ); } // if dependent variables are set - if ( ( observationsDependentVariables_.size( ) > 0 || numberOfObservations_ == 0 ) && dependentVariables.size( ) > 0 ) + if( ( observationsDependentVariables_.size( ) > 0 || numberOfObservations_ == 0 ) && + dependentVariables.size( ) > 0 ) { observationsDependentVariables_.push_back( dependentVariables.at( k ) ); } @@ -900,9 +1099,8 @@ class SingleObservationSet numberOfObservations_ += 1; } - // Sort observations - if ( sortObservations ) + if( sortObservations ) { orderObservationsAndMetadata( ); } @@ -911,28 +1109,35 @@ class SingleObservationSet updateTimeBounds( ); } - void addDependentVariables( const std::vector< std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > > dependentVariableSettings, - const simulation_setup::SystemOfBodies& bodies ) + void addDependentVariables( + const std::vector< + std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > > + dependentVariableSettings, + const simulation_setup::SystemOfBodies& bodies ) { - if ( dependentVariableCalculator_ == nullptr ) + if( dependentVariableCalculator_ == nullptr ) { - dependentVariableCalculator_ = std::make_shared< ObservationDependentVariableCalculator >( observableType_, linkEnds_.linkEnds_ ); + dependentVariableCalculator_ = + std::make_shared< ObservationDependentVariableCalculator >( + observableType_, linkEnds_.linkEnds_ ); } dependentVariableCalculator_->addDependentVariables( dependentVariableSettings, bodies ); } -private: - + private: void orderObservationsAndMetadata( ) { if( !std::is_sorted( observationTimes_.begin( ), observationTimes_.end( ) ) ) { if( observations_.size( ) != numberOfObservations_ ) { - throw std::runtime_error( "Error when making SingleObservationSet, number of observations is incompatible after time ordering" ); + throw std::runtime_error( + "Error when making SingleObservationSet, number of observations is " + "incompatible after time ordering" ); } - std::multimap< TimeType, Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > observationsMap; + std::multimap< TimeType, Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > + observationsMap; for( unsigned int i = 0; i < observations_.size( ); i++ ) { observationsMap.insert( { observationTimes_.at( i ), observations_.at( i ) } ); @@ -944,20 +1149,26 @@ class SingleObservationSet { if( observationsDependentVariables_.size( ) != numberOfObservations_ ) { - throw std::runtime_error( "Error when making SingleObservationSet, dependent variables vector size is incompatible after time ordering" ); + throw std::runtime_error( + "Error when making SingleObservationSet, dependent variables vector " + "size is incompatible after time ordering" ); } std::multimap< TimeType, Eigen::VectorXd > observationsDependentVariablesMap; for( unsigned int i = 0; i < observationsDependentVariables_.size( ); i++ ) { - observationsDependentVariablesMap.insert( { observationTimes_.at( i ), observationsDependentVariables_.at( i ) } ); + observationsDependentVariablesMap.insert( + { observationTimes_.at( i ), + observationsDependentVariables_.at( i ) } ); } - observationsDependentVariables_ = utilities::createVectorFromMultiMapValues( observationsDependentVariablesMap ); + observationsDependentVariables_ = utilities::createVectorFromMultiMapValues( + observationsDependentVariablesMap ); } - if( weights_.size( ) != numberOfObservations_ ) { - throw std::runtime_error( "Error when making SingleObservationSet, weights size is incompatible after time ordering" ); + throw std::runtime_error( + "Error when making SingleObservationSet, weights size is incompatible " + "after time ordering" ); } std::multimap< TimeType, Eigen::Matrix< double, Eigen::Dynamic, 1 > > weightsMap; for( unsigned int i = 0; i < weights_.size( ); i++ ) @@ -968,12 +1179,15 @@ class SingleObservationSet if( residuals_.size( ) != numberOfObservations_ ) { - throw std::runtime_error( "Error when making SingleObservationSet, residuals size is incompatible after time ordering" ); + throw std::runtime_error( + "Error when making SingleObservationSet, residuals size is incompatible " + "after time ordering" ); } - std::multimap< TimeType, Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > residualsMap; + std::multimap< TimeType, Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > + residualsMap; for( unsigned int i = 0; i < residuals_.size( ); i++ ) { - residualsMap.insert( { observationTimes_.at( i ), residuals_.at( i ) } ); + residualsMap.insert( { observationTimes_.at( i ), residuals_.at( i ) } ); } residuals_ = utilities::createVectorFromMultiMapValues( residualsMap ); } @@ -987,12 +1201,15 @@ class SingleObservationSet } else { - timeBounds_ = std::make_pair ( *std::min_element( observationTimes_.begin( ), observationTimes_.end( ) ), - *std::max_element( observationTimes_.begin( ), observationTimes_.end( ) ) ); + timeBounds_ = std::make_pair( + *std::min_element( observationTimes_.begin( ), observationTimes_.end( ) ), + *std::max_element( observationTimes_.begin( ), observationTimes_.end( ) ) ); } } - void moveObservationsInOutFilteredSet( const std::vector< unsigned int >& indices, const bool moveInFilteredSet = true, const bool saveFilteredObservations = true ) + void moveObservationsInOutFilteredSet( const std::vector< unsigned int >& indices, + const bool moveInFilteredSet = true, + const bool saveFilteredObservations = true ) { std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > observations; std::vector< TimeType > times; @@ -1000,13 +1217,15 @@ class SingleObservationSet std::vector< Eigen::Matrix< double, Eigen::Dynamic, 1 > > weights; std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > residuals; - if ( moveInFilteredSet ) + if( moveInFilteredSet ) { - for ( auto index : indices ) + for( auto index: indices ) { - if ( index >= numberOfObservations_ ) + if( index >= numberOfObservations_ ) { - throw std::runtime_error( "Error when moving observation to filtered observation set, index incompatible with number of observations." ); + throw std::runtime_error( + "Error when moving observation to filtered observation set, index " + "incompatible with number of observations." ); } observations.push_back( observations_.at( index ) ); @@ -1015,29 +1234,34 @@ class SingleObservationSet residuals.push_back( residuals_.at( index ) ); // If dependent variables not empty - if ( observationsDependentVariables_.size( ) > 0 ) + if( observationsDependentVariables_.size( ) > 0 ) { dependentVariables.push_back( observationsDependentVariables_.at( index ) ); } } - if ( saveFilteredObservations ) + if( saveFilteredObservations ) { - filteredObservationSet_->addObservations( observations, times, dependentVariables, weights, residuals, true ); + filteredObservationSet_->addObservations( + observations, times, dependentVariables, weights, residuals, true ); } removeObservations( indices ); } else { - for ( auto index : indices ) + for( auto index: indices ) { - if ( getNumberOfFilteredObservations( ) == 0 ) + if( getNumberOfFilteredObservations( ) == 0 ) { - throw std::runtime_error( "Error when moving observation back from filtered observation set, filtered observation set is empty." ); + throw std::runtime_error( + "Error when moving observation back from filtered observation set, " + "filtered observation set is empty." ); } - if ( index >= getNumberOfFilteredObservations( ) ) + if( index >= getNumberOfFilteredObservations( ) ) { - throw std::runtime_error( "Error when moving observation back from filtered observation set, index incompatible with number of observations." ); + throw std::runtime_error( + "Error when moving observation back from filtered observation set, " + "index incompatible with number of observations." ); } observations.push_back( filteredObservationSet_->getObservations( ).at( index ) ); @@ -1046,11 +1270,12 @@ class SingleObservationSet residuals.push_back( filteredObservationSet_->getResiduals( ).at( index ) ); // If dependent variables are set - if ( filteredObservationSet_->getObservationsDependentVariables( ).size( ) > 0 ) + if( filteredObservationSet_->getObservationsDependentVariables( ).size( ) > 0 ) { - dependentVariables.push_back( filteredObservationSet_->getObservationsDependentVariables( ).at( index ) ); + dependentVariables.push_back( + filteredObservationSet_->getObservationsDependentVariables( ).at( + index ) ); } - } addObservations( observations, times, dependentVariables, weights, residuals, true ); filteredObservationSet_->removeObservations( indices ); @@ -1058,20 +1283,29 @@ class SingleObservationSet } //! Function extracting the values of a single dependent variable - Eigen::MatrixXd getSingleDependentVariable( std::pair< int, int > dependentVariableIndexAndSize ) const + Eigen::MatrixXd getSingleDependentVariable( + std::pair< int, int > dependentVariableIndexAndSize ) const { - Eigen::MatrixXd singleDependentVariable = Eigen::MatrixXd::Zero( numberOfObservations_, dependentVariableIndexAndSize.second ); - for ( unsigned int i = 0 ; i < observationsDependentVariables_.size( ) ; i++ ) + Eigen::MatrixXd singleDependentVariable = Eigen::MatrixXd::Zero( + numberOfObservations_, dependentVariableIndexAndSize.second ); + for( unsigned int i = 0; i < observationsDependentVariables_.size( ); i++ ) { - if ( dependentVariableIndexAndSize.first + dependentVariableIndexAndSize.second > observationsDependentVariables_.at( i ).size( ) ) + if( dependentVariableIndexAndSize.first + dependentVariableIndexAndSize.second > + observationsDependentVariables_.at( i ).size( ) ) { - throw std::runtime_error( "Error when retrieving single observation dependent variable, required index and size incompatible with " - "dependent variables size." ); + throw std::runtime_error( + "Error when retrieving single observation dependent variable, required " + "index and size incompatible with " + "dependent variables size." ); } else { - Eigen::VectorXd singleDependentVariableVector = observationsDependentVariables_.at( i ).segment( dependentVariableIndexAndSize.first, dependentVariableIndexAndSize.second ); - singleDependentVariable.block( i, 0, 1, dependentVariableIndexAndSize.second ) = singleDependentVariableVector.transpose( ); + Eigen::VectorXd singleDependentVariableVector = + observationsDependentVariables_.at( i ).segment( + dependentVariableIndexAndSize.first, + dependentVariableIndexAndSize.second ); + singleDependentVariable.block( i, 0, 1, dependentVariableIndexAndSize.second ) = + singleDependentVariableVector.transpose( ); } } return singleDependentVariable; @@ -1091,9 +1325,11 @@ class SingleObservationSet std::vector< Eigen::VectorXd > observationsDependentVariables_; - std::shared_ptr< simulation_setup::ObservationDependentVariableCalculator > dependentVariableCalculator_; + std::shared_ptr< simulation_setup::ObservationDependentVariableCalculator > + dependentVariableCalculator_; - const std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > ancilliarySettings_; + const std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > + ancilliarySettings_; unsigned int numberOfObservations_; @@ -1101,32 +1337,45 @@ class SingleObservationSet std::vector< Eigen::Matrix< double, Eigen::Dynamic, 1 > > weights_; -// Eigen::VectorXd weightsVector_; + // Eigen::VectorXd weightsVector_; std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > residuals_; - std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > filteredObservationSet_; - + std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > + filteredObservationSet_; }; -template< typename ObservationScalarType = double, typename TimeType = double, - typename std::enable_if< is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, int >::type = 0 > +template< typename ObservationScalarType = double, + typename TimeType = double, + typename std::enable_if< + is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, + int >::type = 0 > std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > filterObservations( - const std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > singleObservationSet, - const std::shared_ptr< ObservationFilterBase > observationFilter, const bool saveFilteredObservations = false ) + const std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > + singleObservationSet, + const std::shared_ptr< ObservationFilterBase > observationFilter, + const bool saveFilteredObservations = false ) { - if ( !observationFilter->filterOut( ) ) + if( !observationFilter->filterOut( ) ) { - throw std::runtime_error( "Error when creating new single observation set post-filtering, the filterOut option should be set to true" ); + throw std::runtime_error( + "Error when creating new single observation set post-filtering, the filterOut " + "option should be set to true" ); } // Create new observation set std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > newObservationSet = std::make_shared< SingleObservationSet< ObservationScalarType, TimeType > >( - singleObservationSet->getObservableType( ), singleObservationSet->getLinkEnds( ), singleObservationSet->getObservationsReference( ), - singleObservationSet->getObservationTimesReference( ), singleObservationSet->getReferenceLinkEnd( ), singleObservationSet->getObservationsDependentVariablesReference( ), - singleObservationSet->getDependentVariableCalculator( ), singleObservationSet->getAncilliarySettings( ) ); + singleObservationSet->getObservableType( ), + singleObservationSet->getLinkEnds( ), + singleObservationSet->getObservationsReference( ), + singleObservationSet->getObservationTimesReference( ), + singleObservationSet->getReferenceLinkEnd( ), + singleObservationSet->getObservationsDependentVariablesReference( ), + singleObservationSet->getDependentVariableCalculator( ), + singleObservationSet->getAncilliarySettings( ) ); newObservationSet->setTabulatedWeights( singleObservationSet->getWeightsVector( ) ); - std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > residuals = singleObservationSet->getResidualsReference( ); + std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > residuals = + singleObservationSet->getResidualsReference( ); newObservationSet->setResiduals( singleObservationSet->getResidualsReference( ) /*residuals*/ ); // Filter observations from new observation set @@ -1135,40 +1384,53 @@ std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > filte return newObservationSet; } -template< typename ObservationScalarType = double, typename TimeType = double, - typename std::enable_if< is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, int >::type = 0 > -std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > splitObservationSet( - const std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > observationSet, +template< typename ObservationScalarType = double, + typename TimeType = double, + typename std::enable_if< + is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, + int >::type = 0 > +std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > +splitObservationSet( + const std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > + observationSet, const std::shared_ptr< ObservationSetSplitterBase > observationSetSplitter, const bool printWarning = true ) { - if ( printWarning && observationSet->getFilteredObservationSet( ) != nullptr ) + if( printWarning && observationSet->getFilteredObservationSet( ) != nullptr ) { - std::cerr << "Warning when splitting single observation set, the filtered observation set pointer is not empty and " - " any filtered observation will be lost after splitting." << std::endl; + std::cerr << "Warning when splitting single observation set, the filtered observation set " + "pointer is not empty and " + " any filtered observation will be lost after splitting." + << std::endl; } std::vector< int > rawStartIndicesNewSets = { 0 }; std::vector< TimeType > observationTimes = observationSet->getObservationTimes( ); - std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > observations = observationSet->getObservations( ); - std::vector< Eigen::VectorXd > dependentVariables = observationSet->getObservationsDependentVariables( ); + std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > observations = + observationSet->getObservations( ); + std::vector< Eigen::VectorXd > dependentVariables = + observationSet->getObservationsDependentVariables( ); Eigen::Matrix< double, Eigen::Dynamic, 1 > weightsVector = observationSet->getWeightsVector( ); - std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > residuals = observationSet->getResiduals( ); + std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > residuals = + observationSet->getResiduals( ); - switch ( observationSetSplitter->getSplitterType( ) ) + switch( observationSetSplitter->getSplitterType( ) ) { - case time_tags_splitter: - { - std::vector< double > timeTags = std::dynamic_pointer_cast< ObservationSetSplitter< std::vector< double > > >( observationSetSplitter )->getSplitterValue( ); - for ( auto currentTimeTag : timeTags ) + case time_tags_splitter: { + std::vector< double > timeTags = + std::dynamic_pointer_cast< ObservationSetSplitter< std::vector< double > > >( + observationSetSplitter ) + ->getSplitterValue( ); + for( auto currentTimeTag: timeTags ) { - if ( currentTimeTag > observationSet->getTimeBounds( ).first ) + if( currentTimeTag > observationSet->getTimeBounds( ).first ) { bool detectedStartSet = false; int indexObs = rawStartIndicesNewSets.at( rawStartIndicesNewSets.size( ) - 1 ); - while ( !detectedStartSet && indexObs < static_cast< int >( observationTimes.size( ) ) ) + while( !detectedStartSet && + indexObs < static_cast< int >( observationTimes.size( ) ) ) { - if ( observationTimes.at( indexObs ) > currentTimeTag ) + if( observationTimes.at( indexObs ) > currentTimeTag ) { rawStartIndicesNewSets.push_back( indexObs ); detectedStartSet = true; @@ -1180,9 +1442,10 @@ std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeT rawStartIndicesNewSets.push_back( static_cast< int >( observationTimes.size( ) ) ); break; } - case time_interval_splitter: - { - double maxTimeInterval = std::dynamic_pointer_cast< ObservationSetSplitter< double > >( observationSetSplitter )->getSplitterValue( ); + case time_interval_splitter: { + double maxTimeInterval = std::dynamic_pointer_cast< ObservationSetSplitter< double > >( + observationSetSplitter ) + ->getSplitterValue( ); for( unsigned int i = 1; i < observationTimes.size( ); i++ ) { if( ( observationTimes.at( i ) - observationTimes.at( i - 1 ) ) > maxTimeInterval ) @@ -1193,10 +1456,11 @@ std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeT rawStartIndicesNewSets.push_back( static_cast< int >( observationTimes.size( ) ) ); break; } - case time_span_splitter: - { - double maxTimeSpan = std::dynamic_pointer_cast< ObservationSetSplitter< double > >( observationSetSplitter )->getSplitterValue( ); - if ( observationSet->getNumberOfObservables( ) > 0 ) + case time_span_splitter: { + double maxTimeSpan = std::dynamic_pointer_cast< ObservationSetSplitter< double > >( + observationSetSplitter ) + ->getSplitterValue( ); + if( observationSet->getNumberOfObservables( ) > 0 ) { double referenceEpoch = observationTimes.at( 0 ); for( unsigned int i = 1; i < observationTimes.size( ); i++ ) @@ -1211,14 +1475,19 @@ std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeT } break; } - case nb_observations_splitter: - { - int maxNbObs = std::dynamic_pointer_cast< ObservationSetSplitter< int > >( observationSetSplitter )->getSplitterValue( ); - if ( maxNbObs < observationSetSplitter->getMinNumberObservations( ) ) + case nb_observations_splitter: { + int maxNbObs = std::dynamic_pointer_cast< ObservationSetSplitter< int > >( + observationSetSplitter ) + ->getSplitterValue( ); + if( maxNbObs < observationSetSplitter->getMinNumberObservations( ) ) { - throw std::runtime_error( "Error when splitting observation sets, the maximum number of observations cannot be smaller than the minimum number of observations." ); + throw std::runtime_error( + "Error when splitting observation sets, the maximum number of observations " + "cannot be smaller than the minimum number of observations." ); } - for ( int ind = maxNbObs ; ind < static_cast< int >( observationSet->getNumberOfObservables( ) ) ; ind+=maxNbObs ) + for( int ind = maxNbObs; + ind < static_cast< int >( observationSet->getNumberOfObservables( ) ); + ind += maxNbObs ) { rawStartIndicesNewSets.push_back( ind ); } @@ -1233,38 +1502,55 @@ std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeT std::vector< std::pair< int, int > > indicesNewSets; for( unsigned int j = 1; j < rawStartIndicesNewSets.size( ); j++ ) { - if( ( rawStartIndicesNewSets.at( j ) - rawStartIndicesNewSets.at( j - 1 ) ) >= observationSetSplitter->getMinNumberObservations( ) ) + if( ( rawStartIndicesNewSets.at( j ) - rawStartIndicesNewSets.at( j - 1 ) ) >= + observationSetSplitter->getMinNumberObservations( ) ) { - indicesNewSets.push_back( std::make_pair( rawStartIndicesNewSets.at( j-1 ), rawStartIndicesNewSets.at( j ) - rawStartIndicesNewSets.at( j-1 ) ) ); + indicesNewSets.push_back( std::make_pair( + rawStartIndicesNewSets.at( j - 1 ), + rawStartIndicesNewSets.at( j ) - rawStartIndicesNewSets.at( j - 1 ) ) ); } } // Split current observation set based on indices - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > newObsSets; - for ( unsigned int k = 0 ; k < indicesNewSets.size( ) ; k++ ) + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + newObsSets; + for( unsigned int k = 0; k < indicesNewSets.size( ); k++ ) { int startIndex = indicesNewSets.at( k ).first; int sizeCurrentSet = indicesNewSets.at( k ).second; std::vector< Eigen::VectorXd > newDependentVariables; - if ( !dependentVariables.empty( ) ) + if( !dependentVariables.empty( ) ) { newDependentVariables = utilities::getStlVectorSegment( - observationSet->getObservationsDependentVariablesReference( ), startIndex, sizeCurrentSet ); - } - - std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > newSet = std::make_shared< SingleObservationSet< ObservationScalarType, TimeType > >( - observationSet->getObservableType( ), observationSet->getLinkEnds( ), - utilities::getStlVectorSegment( observationSet->getObservationsReference( ), startIndex, sizeCurrentSet ), - utilities::getStlVectorSegment( observationSet->getObservationTimesReference( ), startIndex, sizeCurrentSet ), - observationSet->getReferenceLinkEnd( ), newDependentVariables, observationSet->getDependentVariableCalculator( ), - observationSet->getAncilliarySettings( ) ); - - Eigen::Matrix< double, Eigen::Dynamic, 1 > newWeightsVector = weightsVector.segment( startIndex, sizeCurrentSet * observationSet->getSingleObservableSize( ) ); + observationSet->getObservationsDependentVariablesReference( ), + startIndex, + sizeCurrentSet ); + } + + std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > newSet = + std::make_shared< SingleObservationSet< ObservationScalarType, TimeType > >( + observationSet->getObservableType( ), + observationSet->getLinkEnds( ), + utilities::getStlVectorSegment( observationSet->getObservationsReference( ), + startIndex, + sizeCurrentSet ), + utilities::getStlVectorSegment( + observationSet->getObservationTimesReference( ), + startIndex, + sizeCurrentSet ), + observationSet->getReferenceLinkEnd( ), + newDependentVariables, + observationSet->getDependentVariableCalculator( ), + observationSet->getAncilliarySettings( ) ); + + Eigen::Matrix< double, Eigen::Dynamic, 1 > newWeightsVector = weightsVector.segment( + startIndex, sizeCurrentSet * observationSet->getSingleObservableSize( ) ); newSet->setTabulatedWeights( newWeightsVector ); std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > newResiduals = - utilities::getStlVectorSegment( observationSet->getResidualsReference( ), startIndex, sizeCurrentSet ); + utilities::getStlVectorSegment( + observationSet->getResidualsReference( ), startIndex, sizeCurrentSet ); newSet->setResiduals( newResiduals ); newObsSets.push_back( newSet ); @@ -1273,33 +1559,51 @@ std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeT return newObsSets; } - -template< typename ObservationScalarType = double, typename TimeType = double, - typename std::enable_if< is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, int >::type = 0 > -std::map< ObservableType, std::map< LinkEnds, std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > > > -createSortedObservationSetList( const std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > observationSetList ) +template< typename ObservationScalarType = double, + typename TimeType = double, + typename std::enable_if< + is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, + int >::type = 0 > +std::map< ObservableType, + std::map< LinkEnds, + std::vector< std::shared_ptr< + SingleObservationSet< ObservationScalarType, TimeType > > > > > +createSortedObservationSetList( + const std::vector< + std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + observationSetList ) { - std::map< ObservableType, std::map< LinkEnds, std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > > > sortedObservations; - for( unsigned int i = 0; i < observationSetList.size( ); i++ ) - { - - sortedObservations[ observationSetList.at( i )->getObservableType( ) ][ observationSetList.at( i )->getLinkEnds( ).linkEnds_ ].push_back( - observationSetList.at( i ) ); - } - return sortedObservations; + std::map< ObservableType, + std::map< LinkEnds, + std::vector< std::shared_ptr< + SingleObservationSet< ObservationScalarType, TimeType > > > > > + sortedObservations; + for( unsigned int i = 0; i < observationSetList.size( ); i++ ) + { + sortedObservations[ observationSetList.at( i )->getObservableType( ) ] + [ observationSetList.at( i )->getLinkEnds( ).linkEnds_ ] + .push_back( observationSetList.at( i ) ); + } + return sortedObservations; } - -template< typename ObservationScalarType = double, typename TimeType = double, - typename std::enable_if< is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, int >::type = 0 > +template< typename ObservationScalarType = double, + typename TimeType = double, + typename std::enable_if< + is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, + int >::type = 0 > class ObservationCollection { -public: - - typedef std::map< ObservableType, std::map< LinkEnds, std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > > > SortedObservationSets; + public: + typedef std::map< + ObservableType, + std::map< LinkEnds, + std::vector< std::shared_ptr< + SingleObservationSet< ObservationScalarType, TimeType > > > > > + SortedObservationSets; ObservationCollection( - const SortedObservationSets& observationSetList = SortedObservationSets( ) ): + const SortedObservationSets& observationSetList = SortedObservationSets( ) ) : observationSetList_( observationSetList ) { setObservationSetIndices( ); @@ -1307,8 +1611,11 @@ class ObservationCollection } ObservationCollection( - const std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > observationSetList ): - observationSetList_( createSortedObservationSetList< ObservationScalarType, TimeType >( observationSetList ) ) + const std::vector< + std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + observationSetList ) : + observationSetList_( createSortedObservationSetList< ObservationScalarType, TimeType >( + observationSetList ) ) { setObservationSetIndices( ); setConcatenatedObservationsAndTimes( ); @@ -1319,46 +1626,54 @@ class ObservationCollection return concatenatedObservations_; } - const Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >& getObservationVectorReference( ) + const Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >& + getObservationVectorReference( ) { return concatenatedObservations_; } - void setObservations( const Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >& newObservations ) + void setObservations( + const Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >& newObservations ) { - if ( newObservations.size( ) != totalObservableSize_ ) + if( newObservations.size( ) != totalObservableSize_ ) { - throw std::runtime_error( "Error when resetting observations in ObservationCollection, size of input observation vector is inconsistent." ); + throw std::runtime_error( + "Error when resetting observations in ObservationCollection, size of input " + "observation vector is inconsistent." ); } unsigned int startIndexObsSet = 0; - for ( auto observableIt : observationSetList_ ) + for( auto observableIt: observationSetList_ ) { - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( auto set : linkEndsIt.second ) + for( auto set: linkEndsIt.second ) { unsigned int sizeCurrentSet = set->getTotalObservationSetSize( ); - set->setObservations( newObservations.segment( startIndexObsSet, sizeCurrentSet ) ); + set->setObservations( + newObservations.segment( startIndexObsSet, sizeCurrentSet ) ); startIndexObsSet += sizeCurrentSet; } } } } - void setResiduals( const Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >& newResiduals ) + void setResiduals( + const Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >& newResiduals ) { - if ( newResiduals.size( ) != totalObservableSize_ ) + if( newResiduals.size( ) != totalObservableSize_ ) { - throw std::runtime_error( "Error when resetting observations in ObservationCollection, size of input observation vector is inconsistent." ); + throw std::runtime_error( + "Error when resetting observations in ObservationCollection, size of input " + "observation vector is inconsistent." ); } unsigned int startIndexObsSet = 0; - for ( auto observableIt : observationSetList_ ) + for( auto observableIt: observationSetList_ ) { - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( auto set : linkEndsIt.second ) + for( auto set: linkEndsIt.second ) { unsigned int sizeCurrentSet = set->getTotalObservationSetSize( ); set->setResiduals( newResiduals.segment( startIndexObsSet, sizeCurrentSet ) ); @@ -1372,77 +1687,89 @@ class ObservationCollection const Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >& observations, const std::shared_ptr< ObservationCollectionParser > observationParser ) { - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > singleObsSets = getSingleObservationSets( observationParser ); - if ( singleObsSets.empty( ) ) + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + singleObsSets = getSingleObservationSets( observationParser ); + if( singleObsSets.empty( ) ) { - std::cerr << "Warning when setting observations, no single observation set found for specified observation parser. Weights not set"; + std::cerr << "Warning when setting observations, no single observation set found for " + "specified observation parser. Weights not set"; } int totalSizeAllObsSets = 0; - for ( unsigned int k = 0 ; k < singleObsSets.size( ) ; k++ ) + for( unsigned int k = 0; k < singleObsSets.size( ); k++ ) { totalSizeAllObsSets += singleObsSets.at( k )->getTotalObservationSetSize( ); } - if ( observations.size( ) == totalSizeAllObsSets ) + if( observations.size( ) == totalSizeAllObsSets ) { unsigned int startObsSet = 0; - for ( auto obsSet : singleObsSets ) + for( auto obsSet: singleObsSets ) { - obsSet->setObservations( observations.segment( startObsSet, obsSet->getTotalObservationSetSize( ) ) ); + obsSet->setObservations( observations.segment( + startObsSet, obsSet->getTotalObservationSetSize( ) ) ); startObsSet += obsSet->getTotalObservationSetSize( ); } } else { - throw std::runtime_error( "Error when setting observations, the size of the input observation vector should be consistent with " - "the combined size of all required observation sets." ); + throw std::runtime_error( + "Error when setting observations, the size of the input observation vector " + "should be consistent with " + "the combined size of all required observation sets." ); } } void setObservations( - const std::map< std::shared_ptr< ObservationCollectionParser >, Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& observationsPerParser ) + const std::map< std::shared_ptr< ObservationCollectionParser >, + Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& + observationsPerParser ) { - for ( auto parserIt : observationsPerParser ) + for( auto parserIt: observationsPerParser ) { setObservations( parserIt.second, parserIt.first ); } } - void setResiduals( - const Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >& residuals, - const std::shared_ptr< ObservationCollectionParser > observationParser ) + void setResiduals( const Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >& residuals, + const std::shared_ptr< ObservationCollectionParser > observationParser ) { - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > singleObsSets = getSingleObservationSets( observationParser ); - if ( singleObsSets.empty( ) ) + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + singleObsSets = getSingleObservationSets( observationParser ); + if( singleObsSets.empty( ) ) { - std::cerr << "Warning when setting residuals, no single observation set found for specified observation parser. Weights not set"; + std::cerr << "Warning when setting residuals, no single observation set found for " + "specified observation parser. Weights not set"; } int totalSizeAllObsSets = 0; - for ( unsigned int k = 0 ; k < singleObsSets.size( ) ; k++ ) + for( unsigned int k = 0; k < singleObsSets.size( ); k++ ) { totalSizeAllObsSets += singleObsSets.at( k )->getTotalObservationSetSize( ); } - if ( residuals.size( ) == totalSizeAllObsSets ) + if( residuals.size( ) == totalSizeAllObsSets ) { unsigned int startObsSet = 0; - for ( auto obsSet : singleObsSets ) + for( auto obsSet: singleObsSets ) { - obsSet->setResiduals( residuals.segment( startObsSet, obsSet->getTotalObservationSetSize( ) ) ); + obsSet->setResiduals( + residuals.segment( startObsSet, obsSet->getTotalObservationSetSize( ) ) ); startObsSet += obsSet->getTotalObservationSetSize( ); } } else { - throw std::runtime_error( "Error when setting residuals, the size of the input residual vector should be consistent with " - "the combined size of all required observation sets." ); + throw std::runtime_error( + "Error when setting residuals, the size of the input residual vector should be " + "consistent with " + "the combined size of all required observation sets." ); } } - void setResiduals( - const std::map< std::shared_ptr< ObservationCollectionParser >, Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& residualsPerParser ) + void setResiduals( const std::map< std::shared_ptr< ObservationCollectionParser >, + Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& + residualsPerParser ) { - for ( auto parserIt : residualsPerParser ) + for( auto parserIt: residualsPerParser ) { setResiduals( parserIt.second, parserIt.first ); } @@ -1455,26 +1782,31 @@ class ObservationCollection std::vector< double > getConcatenatedDoubleTimeVector( ) { - return utilities::staticCastVector( concatenatedTimes_ ); + return utilities::staticCastVector< double, TimeType >( concatenatedTimes_ ); } std::pair< TimeType, TimeType > getTimeBounds( ) { - return std::make_pair ( *std::min_element( concatenatedTimes_.begin( ), concatenatedTimes_.end( ) ), - *std::max_element( concatenatedTimes_.begin( ), concatenatedTimes_.end( ) ) ); + return std::make_pair( + *std::min_element( concatenatedTimes_.begin( ), concatenatedTimes_.end( ) ), + *std::max_element( concatenatedTimes_.begin( ), concatenatedTimes_.end( ) ) ); } std::vector< std::pair< TimeType, TimeType > > getTimeBoundsPerSet( - std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) const { std::vector< std::pair< TimeType, TimeType > > timeBounds; - for ( auto observableIt : getSingleObservationSetsIndices( observationParser ) ) + for( auto observableIt: getSingleObservationSetsIndices( observationParser ) ) { - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( auto index : linkEndsIt.second ) + for( auto index: linkEndsIt.second ) { - timeBounds.push_back( observationSetList_.at( observableIt.first ).at( linkEndsIt.first ).at( index )->getTimeBounds( ) ); + timeBounds.push_back( observationSetList_.at( observableIt.first ) + .at( linkEndsIt.first ) + .at( index ) + ->getTimeBounds( ) ); } } } @@ -1486,20 +1818,27 @@ class ObservationCollection return concatenatedLinkEndIds_; } - std::vector< int > getConcatenatedLinkEndIds( std::shared_ptr< ObservationCollectionParser > observationParser ) + std::vector< int > getConcatenatedLinkEndIds( + std::shared_ptr< ObservationCollectionParser > observationParser ) { std::vector< int > subsetConcatenatedLinkEndIds; - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > observationSetsIndices = getSingleObservationSetsIndices( observationParser ); + std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > + observationSetsIndices = getSingleObservationSetsIndices( observationParser ); - for ( auto obsIt : observationSetsIndices ) + for( auto obsIt: observationSetsIndices ) { - for ( auto linkEndsIt : obsIt.second ) + for( auto linkEndsIt: obsIt.second ) { std::vector< unsigned int > listRelevantSetIndices = linkEndsIt.second; - for ( auto index : listRelevantSetIndices ) + for( auto index: listRelevantSetIndices ) { - std::pair< int, int > singleSetStartAndSize = observationSetStartAndSize_.at( obsIt.first ).at( linkEndsIt.first ).at( index ); - for ( unsigned int k = singleSetStartAndSize.first ; k < ( singleSetStartAndSize.first + singleSetStartAndSize.second ) ; k++ ) + std::pair< int, int > singleSetStartAndSize = + observationSetStartAndSize_.at( obsIt.first ) + .at( linkEndsIt.first ) + .at( index ); + for( unsigned int k = singleSetStartAndSize.first; + k < ( singleSetStartAndSize.first + singleSetStartAndSize.second ); + k++ ) { subsetConcatenatedLinkEndIds.push_back( concatenatedLinkEndIds_[ k ] ); } @@ -1519,12 +1858,14 @@ class ObservationCollection return inverseLinkEndIds_; } - std::map< ObservableType, std::map< LinkEnds, std::vector< std::pair< int, int > > > > getObservationSetStartAndSize( ) + std::map< ObservableType, std::map< LinkEnds, std::vector< std::pair< int, int > > > > + getObservationSetStartAndSize( ) { return observationSetStartAndSize_; } - std::map< ObservableType, std::map< LinkEnds, std::vector< std::pair< int, int > > > >& getObservationSetStartAndSizeReference( ) + std::map< ObservableType, std::map< LinkEnds, std::vector< std::pair< int, int > > > >& + getObservationSetStartAndSizeReference( ) { return observationSetStartAndSize_; } @@ -1534,18 +1875,18 @@ class ObservationCollection return concatenatedObservationSetStartAndSize_; } - std::map< ObservableType, std::map< LinkEnds, std::pair< int, int > > > getObservationTypeAndLinkEndStartAndSize( ) + std::map< ObservableType, std::map< LinkEnds, std::pair< int, int > > > + getObservationTypeAndLinkEndStartAndSize( ) { return observationTypeAndLinkEndStartAndSize_; } - std::map< ObservableType, std::map< int, std::vector< std::pair< int, int > > > > getObservationSetStartAndSizePerLinkEndIndex( ) + std::map< ObservableType, std::map< int, std::vector< std::pair< int, int > > > > + getObservationSetStartAndSizePerLinkEndIndex( ) { return observationSetStartAndSizePerLinkEndIndex_; } - - std::map< ObservableType, std::pair< int, int > > getObservationTypeStartAndSize( ) { return observationTypeStartAndSize_; @@ -1561,7 +1902,7 @@ class ObservationCollection return observationSetList_; } - const SortedObservationSets& getObservationsReference ( ) const + const SortedObservationSets& getObservationsReference( ) const { return observationSetList_; } @@ -1577,7 +1918,7 @@ class ObservationCollection } std::vector< LinkDefinition > getLinkDefinitionsForSingleObservable( - const ObservableType observableType ) + const ObservableType observableType ) { if( linkDefinitionsPerObservable_.count( observableType ) > 0 ) { @@ -1589,12 +1930,20 @@ class ObservationCollection } } - std::map< ObservableType, std::map< int, std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > > > getSortedObservationSets( ) + std::map< ObservableType, + std::map< int, + std::vector< std::shared_ptr< + SingleObservationSet< ObservationScalarType, TimeType > > > > > + getSortedObservationSets( ) { - std::map< ObservableType, std::map< int, std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > > > observationSetListIndexSorted; - for( auto it1 : observationSetList_ ) + std::map< ObservableType, + std::map< int, + std::vector< std::shared_ptr< + SingleObservationSet< ObservationScalarType, TimeType > > > > > + observationSetListIndexSorted; + for( auto it1: observationSetList_ ) { - for( auto it2 : it1.second ) + for( auto it2: it1.second ) { observationSetListIndexSorted[ it1.first ][ linkEndIds_[ it2.first ] ] = it2.second; } @@ -1602,36 +1951,42 @@ class ObservationCollection return observationSetListIndexSorted; } - std::map< ObservableType, std::map< int, std::vector< std::pair< double, double > > > > getSortedObservationSetsTimeBounds( ) + std::map< ObservableType, std::map< int, std::vector< std::pair< double, double > > > > + getSortedObservationSetsTimeBounds( ) { - std::map< ObservableType, std::map< int, std::vector< std::pair< double, double > > > > observationSetTimeBounds; - for( auto it1 : observationSetList_ ) + std::map< ObservableType, std::map< int, std::vector< std::pair< double, double > > > > + observationSetTimeBounds; + for( auto it1: observationSetList_ ) { - for( auto it2 : it1.second ) + for( auto it2: it1.second ) { int currentLinkEndId = linkEndIds_[ it2.first ]; for( unsigned int i = 0; i < it2.second.size( ); i++ ) { - std::pair< TimeType, TimeType > timeBounds = it2.second.at( i )->getTimeBounds( ); - observationSetTimeBounds[ it1.first ][ currentLinkEndId ].push_back( - { static_cast< double >( timeBounds.first ), static_cast< double >( timeBounds.second ) } ); + std::pair< TimeType, TimeType > timeBounds = + it2.second.at( i )->getTimeBounds( ); + observationSetTimeBounds[ it1.first ][ currentLinkEndId ].push_back( + { static_cast< double >( timeBounds.first ), + static_cast< double >( timeBounds.second ) } ); } } } return observationSetTimeBounds; } - std::map < ObservableType, std::vector< LinkEnds > > getLinkEndsPerObservableType( ) + std::map< ObservableType, std::vector< LinkEnds > > getLinkEndsPerObservableType( ) { - std::map < ObservableType, std::vector< LinkEnds > > linkEndsPerObservableType; + std::map< ObservableType, std::vector< LinkEnds > > linkEndsPerObservableType; - for ( auto observableTypeIt = observationSetList_.begin( ); observableTypeIt != observationSetList_.end( ); - ++observableTypeIt ) + for( auto observableTypeIt = observationSetList_.begin( ); + observableTypeIt != observationSetList_.end( ); + ++observableTypeIt ) { std::vector< LinkEnds > linkEndsVector; - for ( auto linkEndsIt = observableTypeIt->second.begin( ); linkEndsIt != observableTypeIt->second.end( ); - ++linkEndsIt ) + for( auto linkEndsIt = observableTypeIt->second.begin( ); + linkEndsIt != observableTypeIt->second.end( ); + ++linkEndsIt ) { linkEndsVector.push_back( linkEndsIt->first ); } @@ -1642,13 +1997,18 @@ class ObservationCollection return linkEndsPerObservableType; } - std::vector< ObservableType > getObservableTypes( std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const + std::vector< ObservableType > getObservableTypes( + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) const { - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > observationSetsIndices = getSingleObservationSetsIndices( observationParser ); + std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > + observationSetsIndices = getSingleObservationSetsIndices( observationParser ); std::vector< ObservableType > observableTypes; - for ( auto observableIt : observationSetsIndices ) + for( auto observableIt: observationSetsIndices ) { - if ( std::count( observableTypes.begin( ), observableTypes.end( ), observableIt.first ) == 0 ) + if( std::count( observableTypes.begin( ), + observableTypes.end( ), + observableIt.first ) == 0 ) { observableTypes.push_back( observableIt.first ); } @@ -1656,15 +2016,18 @@ class ObservationCollection return observableTypes; } - std::vector< LinkEnds > getLinkEnds( std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const + std::vector< LinkEnds > getLinkEnds( + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) const { - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > observationSetsIndices = getSingleObservationSetsIndices( observationParser ); + std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > + observationSetsIndices = getSingleObservationSetsIndices( observationParser ); std::vector< LinkEnds > linkEnds; - for ( auto observableIt : observationSetsIndices ) + for( auto observableIt: observationSetsIndices ) { - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - if ( std::count( linkEnds.begin( ), linkEnds.end( ), linkEndsIt.first ) == 0 ) + if( std::count( linkEnds.begin( ), linkEnds.end( ), linkEndsIt.first ) == 0 ) { linkEnds.push_back( linkEndsIt.first ); } @@ -1673,17 +2036,22 @@ class ObservationCollection return linkEnds; } - std::vector< std::string > getBodiesInLinkEnds( std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const + std::vector< std::string > getBodiesInLinkEnds( + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) const { - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > observationSetsIndices = getSingleObservationSetsIndices( observationParser ); + std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > + observationSetsIndices = getSingleObservationSetsIndices( observationParser ); std::vector< std::string > bodiesInLinkEnds; - for ( auto observableIt : observationSetsIndices ) + for( auto observableIt: observationSetsIndices ) { - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( auto it : linkEndsIt.first ) + for( auto it: linkEndsIt.first ) { - if ( std::count( bodiesInLinkEnds.begin( ), bodiesInLinkEnds.end( ), it.second.bodyName_ ) == 0 ) + if( std::count( bodiesInLinkEnds.begin( ), + bodiesInLinkEnds.end( ), + it.second.bodyName_ ) == 0 ) { bodiesInLinkEnds.push_back( it.second.bodyName_ ); } @@ -1693,17 +2061,23 @@ class ObservationCollection return bodiesInLinkEnds; } - std::vector< std::string > getReferencePointsInLinkEnds( std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const + std::vector< std::string > getReferencePointsInLinkEnds( + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) const { - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > observationSetsIndices = getSingleObservationSetsIndices( observationParser ); + std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > + observationSetsIndices = getSingleObservationSetsIndices( observationParser ); std::vector< std::string > referencePoints; - for ( auto observableIt : observationSetsIndices ) + for( auto observableIt: observationSetsIndices ) { - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( auto it : linkEndsIt.first ) + for( auto it: linkEndsIt.first ) { - if ( ( it.second.stationName_ != "" ) && ( std::count( referencePoints.begin( ), referencePoints.end( ), it.second.stationName_ ) == 0 ) ) + if( ( it.second.stationName_ != "" ) && + ( std::count( referencePoints.begin( ), + referencePoints.end( ), + it.second.stationName_ ) == 0 ) ) { referencePoints.push_back( it.second.stationName_ ); } @@ -1714,18 +2088,25 @@ class ObservationCollection } std::vector< std::pair< TimeType, TimeType > > getTimeBoundsList( - std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) const { - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > observationSetsIndices = getSingleObservationSetsIndices( observationParser ); + std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > + observationSetsIndices = getSingleObservationSetsIndices( observationParser ); std::vector< std::pair< TimeType, TimeType > > timeBounds; - for ( auto observableIt : observationSetsIndices ) + for( auto observableIt: observationSetsIndices ) { - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( auto index : linkEndsIt.second ) + for( auto index: linkEndsIt.second ) { - std::pair< TimeType, TimeType > currentTimeBounds = observationSetList_.at( observableIt.first ).at( linkEndsIt.first ).at( index )->getTimeBounds( ); - if ( std::count( timeBounds.begin( ), timeBounds.end( ), currentTimeBounds ) == 0 ) + std::pair< TimeType, TimeType > currentTimeBounds = + observationSetList_.at( observableIt.first ) + .at( linkEndsIt.first ) + .at( index ) + ->getTimeBounds( ); + if( std::count( timeBounds.begin( ), timeBounds.end( ), currentTimeBounds ) == + 0 ) { timeBounds.push_back( currentTimeBounds ); } @@ -1736,18 +2117,23 @@ class ObservationCollection } std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > getObservations( - std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) const { std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > observations; - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > observationSetsIndices = getSingleObservationSetsIndices( observationParser ); - for ( auto observableIt : observationSetsIndices ) + std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > + observationSetsIndices = getSingleObservationSetsIndices( observationParser ); + for( auto observableIt: observationSetsIndices ) { - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( auto index : linkEndsIt.second ) + for( auto index: linkEndsIt.second ) { - observations.push_back( observationSetList_.at( observableIt.first ).at( linkEndsIt.first ).at( index )->getObservationsVector( ) ); + observations.push_back( observationSetList_.at( observableIt.first ) + .at( linkEndsIt.first ) + .at( index ) + ->getObservationsVector( ) ); } } } @@ -1755,21 +2141,25 @@ class ObservationCollection } Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > getConcatenatedObservations( - std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) const { - std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > observations = getObservations( observationParser ); + std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > observations = + getObservations( observationParser ); unsigned int totalObsSize = 0; - for ( auto obs : observations ) + for( auto obs: observations ) { totalObsSize += obs.size( ); } - Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > concatenatedObservations = Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( totalObsSize, 1 ); + Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > concatenatedObservations = + Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( totalObsSize, 1 ); unsigned int obsIndex = 0; - for ( unsigned int k = 0 ; k < observations.size( ) ; k++ ) + for( unsigned int k = 0; k < observations.size( ); k++ ) { - concatenatedObservations.block( obsIndex, 0, observations.at( k ).size( ), 1 ) = observations.at( k ); + concatenatedObservations.block( obsIndex, 0, observations.at( k ).size( ), 1 ) = + observations.at( k ); obsIndex += observations.at( k ).size( ); } @@ -1777,18 +2167,23 @@ class ObservationCollection } std::vector< std::vector< TimeType > > getObservationTimes( - std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) const { std::vector< std::vector< TimeType > > observationsTimes; - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > observationSetsIndices = getSingleObservationSetsIndices( observationParser ); - for ( auto observableIt : observationSetsIndices ) + std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > + observationSetsIndices = getSingleObservationSetsIndices( observationParser ); + for( auto observableIt: observationSetsIndices ) { - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( auto index : linkEndsIt.second ) + for( auto index: linkEndsIt.second ) { - observationsTimes.push_back( observationSetList_.at( observableIt.first ).at( linkEndsIt.first ).at( index )->getObservationTimes( ) ); + observationsTimes.push_back( observationSetList_.at( observableIt.first ) + .at( linkEndsIt.first ) + .at( index ) + ->getObservationTimes( ) ); } } } @@ -1796,14 +2191,16 @@ class ObservationCollection } std::vector< TimeType > getConcatenatedObservationTimes( - std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) const { - std::vector< std::vector< TimeType > > observationTimes = getObservationTimes( observationParser ); + std::vector< std::vector< TimeType > > observationTimes = + getObservationTimes( observationParser ); std::vector< TimeType > concatenatedObservationsTimes; - for ( auto times : observationTimes ) + for( auto times: observationTimes ) { - for ( unsigned int k = 0 ; k < times.size( ) ; k++ ) + for( unsigned int k = 0; k < times.size( ); k++ ) { concatenatedObservationsTimes.push_back( times.at( k ) ); } @@ -1813,36 +2210,49 @@ class ObservationCollection } std::vector< double > getConcatenatedDoubleObservationTimes( - std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) { - return utilities::staticCastVector< double, TimeType >( getConcatenatedObservationTimes( observationParser ) ); + return utilities::staticCastVector< double, TimeType >( + getConcatenatedObservationTimes( observationParser ) ); } - std::pair< std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >, std::vector< std::vector< TimeType > > > getObservationsAndTimes( - std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) + std::pair< std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >, + std::vector< std::vector< TimeType > > > + getObservationsAndTimes( std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) { - return std::make_pair( getObservations( observationParser ), getObservationTimes( observationParser ) ); + return std::make_pair( getObservations( observationParser ), + getObservationTimes( observationParser ) ); } - std::pair< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >, std::vector< TimeType > > getConcatenatedObservationsAndTimes( - std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) + std::pair< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >, std::vector< TimeType > > + getConcatenatedObservationsAndTimes( + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) { - return std::make_pair( getConcatenatedObservations( observationParser ), getConcatenatedObservationTimes( observationParser ) ); + return std::make_pair( getConcatenatedObservations( observationParser ), + getConcatenatedObservationTimes( observationParser ) ); } std::vector< Eigen::VectorXd > getWeights( - std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) const { std::vector< Eigen::VectorXd > weights; - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > observationSetsIndices = getSingleObservationSetsIndices( observationParser ); - for ( auto observableIt : observationSetsIndices ) + std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > + observationSetsIndices = getSingleObservationSetsIndices( observationParser ); + for( auto observableIt: observationSetsIndices ) { - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( auto index : linkEndsIt.second ) + for( auto index: linkEndsIt.second ) { - weights.push_back( observationSetList_.at( observableIt.first ).at( linkEndsIt.first ).at( index )->getWeightsVector( ) ); + weights.push_back( observationSetList_.at( observableIt.first ) + .at( linkEndsIt.first ) + .at( index ) + ->getWeightsVector( ) ); } } } @@ -1850,19 +2260,20 @@ class ObservationCollection } Eigen::VectorXd getConcatenatedWeights( - std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) const { std::vector< Eigen::VectorXd > weights = getWeights( observationParser ); unsigned int totalObsSize = 0; - for ( auto weight : weights ) + for( auto weight: weights ) { totalObsSize += weight.size( ); } Eigen::VectorXd concatenatedWeights = Eigen::VectorXd::Zero( totalObsSize ); unsigned int obsIndex = 0; - for ( unsigned int k = 0 ; k < weights.size( ) ; k++ ) + for( unsigned int k = 0; k < weights.size( ); k++ ) { concatenatedWeights.block( obsIndex, 0, weights.at( k ).size( ), 1 ) = weights.at( k ); obsIndex += weights.at( k ).size( ); @@ -1877,18 +2288,23 @@ class ObservationCollection } std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > getResiduals( - std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) const { std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > residuals; - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > observationSetsIndices = getSingleObservationSetsIndices( observationParser ); - for ( auto observableIt : observationSetsIndices ) + std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > + observationSetsIndices = getSingleObservationSetsIndices( observationParser ); + for( auto observableIt: observationSetsIndices ) { - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( auto index : linkEndsIt.second ) + for( auto index: linkEndsIt.second ) { - residuals.push_back( observationSetList_.at( observableIt.first ).at( linkEndsIt.first ).at( index )->getResidualsVector( ) ); + residuals.push_back( observationSetList_.at( observableIt.first ) + .at( linkEndsIt.first ) + .at( index ) + ->getResidualsVector( ) ); } } } @@ -1896,21 +2312,25 @@ class ObservationCollection } Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > getConcatenatedResiduals( - std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) const { - std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > residuals = getResiduals( observationParser ); + std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > residuals = + getResiduals( observationParser ); unsigned int totalObsSize = 0; - for ( auto residual : residuals ) + for( auto residual: residuals ) { totalObsSize += residual.size( ); } - Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > concatenatedResiduals = Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( totalObsSize, 1 ); + Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > concatenatedResiduals = + Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( totalObsSize, 1 ); unsigned int obsIndex = 0; - for ( unsigned int k = 0 ; k < residuals.size( ) ; k++ ) + for( unsigned int k = 0; k < residuals.size( ); k++ ) { - concatenatedResiduals.block( obsIndex, 0, residuals.at( k ).size( ), 1 ) = residuals.at( k ); + concatenatedResiduals.block( obsIndex, 0, residuals.at( k ).size( ), 1 ) = + residuals.at( k ); obsIndex += residuals.at( k ).size( ); } @@ -1918,18 +2338,23 @@ class ObservationCollection } std::vector< Eigen::VectorXd > getRmsResiduals( - std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) const { std::vector< Eigen::VectorXd > rmsResiduals; - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > observationSetsIndices = getSingleObservationSetsIndices( observationParser ); - for ( auto observableIt : observationSetsIndices ) + std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > + observationSetsIndices = getSingleObservationSetsIndices( observationParser ); + for( auto observableIt: observationSetsIndices ) { - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( auto index : linkEndsIt.second ) + for( auto index: linkEndsIt.second ) { - rmsResiduals.push_back( observationSetList_.at( observableIt.first ).at( linkEndsIt.first ).at( index )->getRmsResiduals( ) ); + rmsResiduals.push_back( observationSetList_.at( observableIt.first ) + .at( linkEndsIt.first ) + .at( index ) + ->getRmsResiduals( ) ); } } } @@ -1937,37 +2362,48 @@ class ObservationCollection } std::vector< Eigen::VectorXd > getMeanResiduals( - std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) const { std::vector< Eigen::VectorXd > meanResiduals; - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > observationSetsIndices = getSingleObservationSetsIndices( observationParser ); - for ( auto observableIt : observationSetsIndices ) + std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > + observationSetsIndices = getSingleObservationSetsIndices( observationParser ); + for( auto observableIt: observationSetsIndices ) { - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( auto index : linkEndsIt.second ) + for( auto index: linkEndsIt.second ) { - meanResiduals.push_back( observationSetList_.at( observableIt.first ).at( linkEndsIt.first ).at( index )->getMeanResiduals( ) ); + meanResiduals.push_back( observationSetList_.at( observableIt.first ) + .at( linkEndsIt.first ) + .at( index ) + ->getMeanResiduals( ) ); } } } return meanResiduals; } - std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > getComputedObservations( - std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const + std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > + getComputedObservations( std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) const { - std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > computedObservations; + std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > + computedObservations; - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > observationSetsIndices = getSingleObservationSetsIndices( observationParser ); - for ( auto observableIt : observationSetsIndices ) + std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > + observationSetsIndices = getSingleObservationSetsIndices( observationParser ); + for( auto observableIt: observationSetsIndices ) { - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( auto index : linkEndsIt.second ) + for( auto index: linkEndsIt.second ) { - computedObservations.push_back( observationSetList_.at( observableIt.first ).at( linkEndsIt.first ).at( index )->getComputedObservationsVector( ) ); + computedObservations.push_back( observationSetList_.at( observableIt.first ) + .at( linkEndsIt.first ) + .at( index ) + ->getComputedObservationsVector( ) ); } } } @@ -1975,42 +2411,52 @@ class ObservationCollection } Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > getConcatenatedComputedObservations( - std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) const { - std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > computedObservations = getComputedObservations( observationParser ); + std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > + computedObservations = getComputedObservations( observationParser ); unsigned int totalObsSize = 0; - for ( auto obs : computedObservations ) + for( auto obs: computedObservations ) { totalObsSize += obs.size( ); } Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > concatenatedComputedObservations = - Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( totalObsSize ); + Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( totalObsSize ); unsigned int obsIndex = 0; - for ( unsigned int k = 0 ; k < computedObservations.size( ) ; k++ ) + for( unsigned int k = 0; k < computedObservations.size( ); k++ ) { - concatenatedComputedObservations.block( obsIndex, 0, computedObservations.at( k ).size( ), 1 ) = computedObservations.at( k ); + concatenatedComputedObservations.block( + obsIndex, 0, computedObservations.at( k ).size( ), 1 ) = + computedObservations.at( k ); obsIndex += computedObservations.at( k ).size( ); } return concatenatedComputedObservations; } - std::map< ObservableType, std::map< LinkEnds, std::vector< std::pair< int, int > > > > getObservationSetStartAndSize( + std::map< ObservableType, std::map< LinkEnds, std::vector< std::pair< int, int > > > > + getObservationSetStartAndSize( std::shared_ptr< ObservationCollectionParser > observationParser ) { - std::map< ObservableType, std::map< LinkEnds, std::vector< std::pair< int, int > > > > observationSetStartAndSize; + std::map< ObservableType, std::map< LinkEnds, std::vector< std::pair< int, int > > > > + observationSetStartAndSize; - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > observationSetsIndices = getSingleObservationSetsIndices( observationParser ); - for ( auto observableIt : observationSetsIndices ) + std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > + observationSetsIndices = getSingleObservationSetsIndices( observationParser ); + for( auto observableIt: observationSetsIndices ) { std::map< LinkEnds, std::vector< std::pair< int, int > > > currentObservableTypeIndices; - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( auto index : linkEndsIt.second ) + for( auto index: linkEndsIt.second ) { - currentObservableTypeIndices[ linkEndsIt.first ].push_back( observationSetStartAndSize_.at( observableIt.first ).at( linkEndsIt.first ).at( index ) ); + currentObservableTypeIndices[ linkEndsIt.first ].push_back( + observationSetStartAndSize_.at( observableIt.first ) + .at( linkEndsIt.first ) + .at( index ) ); } } observationSetStartAndSize[ observableIt.first ] = currentObservableTypeIndices; @@ -2018,20 +2464,21 @@ class ObservationCollection return observationSetStartAndSize; } - - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > getSingleLinkAndTypeObservationSets( - const ObservableType observableType, - const LinkDefinition linkEndsDefinition ) + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + getSingleLinkAndTypeObservationSets( const ObservableType observableType, + const LinkDefinition linkEndsDefinition ) { std::vector< std::shared_ptr< ObservationCollectionParser > > multiTypeParserList; multiTypeParserList.push_back( observationParser( observableType ) ); multiTypeParserList.push_back( observationParser( linkEndsDefinition.linkEnds_ ) ); - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > observationSets = - getSingleObservationSets( observationParser( multiTypeParserList, true ) ); - if ( observationSets.empty( ) ) + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + observationSets = + getSingleObservationSets( observationParser( multiTypeParserList, true ) ); + if( observationSets.empty( ) ) { - throw std::runtime_error( " Error when getting single link observation sets, not observations of type " - + std::to_string( observableType ) + " for given link ends." ); + throw std::runtime_error( + " Error when getting single link observation sets, not observations of type " + + std::to_string( observableType ) + " for given link ends." ); } return observationSets; } @@ -2043,92 +2490,104 @@ class ObservationCollection std::vector< std::shared_ptr< ObservationCollectionParser > > multiTypeParserList; multiTypeParserList.push_back( observationParser( observableType ) ); multiTypeParserList.push_back( observationParser( linkEndsDefinition.linkEnds_ ) ); - Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > observations = getConcatenatedObservations( observationParser( multiTypeParserList, true ) ); - if ( observations.size( ) == 0 ) + Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > observations = + getConcatenatedObservations( observationParser( multiTypeParserList, true ) ); + if( observations.size( ) == 0 ) { - throw std::runtime_error( " Error when getting single link observations, not observations of type " - + std::to_string( observableType ) + " for given link ends." ); + throw std::runtime_error( + " Error when getting single link observations, not observations of type " + + std::to_string( observableType ) + " for given link ends." ); } return observations; } - std::vector< TimeType > getSingleLinkTimes( - const ObservableType observableType, - const LinkDefinition& linkEndsDefinition ) + std::vector< TimeType > getSingleLinkTimes( const ObservableType observableType, + const LinkDefinition& linkEndsDefinition ) { std::vector< std::shared_ptr< ObservationCollectionParser > > multiTypeParserList; multiTypeParserList.push_back( observationParser( observableType ) ); multiTypeParserList.push_back( observationParser( linkEndsDefinition.linkEnds_ ) ); - std::vector< TimeType > observationTimes = getConcatenatedObservationTimes( observationParser( multiTypeParserList, true ) ); - if ( observationTimes.empty( ) ) + std::vector< TimeType > observationTimes = + getConcatenatedObservationTimes( observationParser( multiTypeParserList, true ) ); + if( observationTimes.empty( ) ) { - throw std::runtime_error( " Error when getting single link observation times, not observations of type " - + std::to_string( observableType ) + " for given link ends." ); + throw std::runtime_error( + " Error when getting single link observation times, not observations of type " + + std::to_string( observableType ) + " for given link ends." ); } return observationTimes; } - std::pair< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >, std::vector< TimeType > > getSingleLinkObservationsAndTimes( - const ObservableType observableType, - const LinkDefinition& linkEndsDefinition ) + std::pair< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >, std::vector< TimeType > > + getSingleLinkObservationsAndTimes( const ObservableType observableType, + const LinkDefinition& linkEndsDefinition ) { std::vector< std::shared_ptr< ObservationCollectionParser > > multiTypeParserList; multiTypeParserList.push_back( observationParser( observableType ) ); multiTypeParserList.push_back( observationParser( linkEndsDefinition.linkEnds_ ) ); - std::pair< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >, std::vector< TimeType > > observationsAndTimes = - getConcatenatedObservationsAndTimes( observationParser( multiTypeParserList, true ) ); - if ( observationsAndTimes.second.empty( ) ) + std::pair< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >, + std::vector< TimeType > > + observationsAndTimes = getConcatenatedObservationsAndTimes( + observationParser( multiTypeParserList, true ) ); + if( observationsAndTimes.second.empty( ) ) { - throw std::runtime_error( " Error when getting single link observations and times, not observation set of type " - + std::to_string( observableType ) + " for given link ends." ); + throw std::runtime_error( + " Error when getting single link observations and times, not observation set " + "of type " + + std::to_string( observableType ) + " for given link ends." ); } return observationsAndTimes; } - - void setConstantWeight( - const double weight = 1.0, - const std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) + void setConstantWeight( const double weight = 1.0, + const std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) { - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > singleObsSets = getSingleObservationSets( observationParser ); - if ( singleObsSets.empty( ) ) + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + singleObsSets = getSingleObservationSets( observationParser ); + if( singleObsSets.empty( ) ) { - std::cerr << "Warning when setting constant weights, no single observation set found for specified observation parser. Weights not set"; + std::cerr << "Warning when setting constant weights, no single observation set found " + "for specified observation parser. Weights not set"; } - for ( auto singleObsSet : singleObsSets ) + for( auto singleObsSet: singleObsSets ) { singleObsSet->setConstantWeight( weight ); } } - void setConstantWeight( - const Eigen::VectorXd weight, - const std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) + void setConstantWeight( const Eigen::VectorXd weight, + const std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) { - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > singleObsSets = getSingleObservationSets( observationParser ); - if ( singleObsSets.empty( ) ) + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + singleObsSets = getSingleObservationSets( observationParser ); + if( singleObsSets.empty( ) ) { - std::cerr << "Warning when setting constant weights, no single observation set found for specified observation parser. Weights not set"; + std::cerr << "Warning when setting constant weights, no single observation set found " + "for specified observation parser. Weights not set"; } - for ( auto singleObsSet : singleObsSets ) + for( auto singleObsSet: singleObsSets ) { singleObsSet->setConstantWeight( weight ); } } void setConstantWeightPerObservable( - const std::map< std::shared_ptr< ObservationCollectionParser >, double > weightsPerObservationParser ) + const std::map< std::shared_ptr< ObservationCollectionParser >, double > + weightsPerObservationParser ) { - for ( auto parserIt : weightsPerObservationParser ) + for( auto parserIt: weightsPerObservationParser ) { setConstantWeight( parserIt.second, parserIt.first ); } } void setConstantWeightPerObservable( - const std::map< std::shared_ptr< ObservationCollectionParser >, Eigen::VectorXd > weightsPerObservationParser ) + const std::map< std::shared_ptr< ObservationCollectionParser >, Eigen::VectorXd > + weightsPerObservationParser ) { - for ( auto parserIt : weightsPerObservationParser ) + for( auto parserIt: weightsPerObservationParser ) { setConstantWeight( parserIt.second, parserIt.first ); } @@ -2136,73 +2595,85 @@ class ObservationCollection void setTabulatedWeights( const Eigen::VectorXd tabulatedWeights, - const std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) + const std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) { - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > singleObsSets = getSingleObservationSets( observationParser ); - if ( singleObsSets.empty( ) ) + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + singleObsSets = getSingleObservationSets( observationParser ); + if( singleObsSets.empty( ) ) { - std::cerr << "Warning when setting tabulated weights, no single observation set found for specified observation parser. Weights not set"; + std::cerr << "Warning when setting tabulated weights, no single observation set found " + "for specified observation parser. Weights not set"; } bool areObsSetsSameSize = true; int totalSizeAllObsSets = singleObsSets.at( 0 )->getTotalObservationSetSize( ); - for ( unsigned int k = 1 ; k < singleObsSets.size( ) ; k++ ) + for( unsigned int k = 1; k < singleObsSets.size( ); k++ ) { unsigned int currentObsSetSize = singleObsSets.at( k )->getTotalObservationSetSize( ); totalSizeAllObsSets += currentObsSetSize; - if ( currentObsSetSize != singleObsSets.at( 0 )->getTotalObservationSetSize( ) ) + if( currentObsSetSize != singleObsSets.at( 0 )->getTotalObservationSetSize( ) ) { areObsSetsSameSize = false; } } unsigned int startObsSet = 0; - for ( auto obsSet : singleObsSets ) + for( auto obsSet: singleObsSets ) { - if ( tabulatedWeights.size( ) == totalSizeAllObsSets ) + if( tabulatedWeights.size( ) == totalSizeAllObsSets ) { - Eigen::VectorXd singleSetWeights = tabulatedWeights.segment( startObsSet, obsSet->getTotalObservationSetSize( ) ); + Eigen::VectorXd singleSetWeights = tabulatedWeights.segment( + startObsSet, obsSet->getTotalObservationSetSize( ) ); startObsSet += obsSet->getTotalObservationSetSize( ); obsSet->setTabulatedWeights( singleSetWeights ); } - else if ( areObsSetsSameSize && ( tabulatedWeights.size( ) == singleObsSets.at( 0 )->getTotalObservationSetSize( ) ) ) + else if( areObsSetsSameSize && + ( tabulatedWeights.size( ) == + singleObsSets.at( 0 )->getTotalObservationSetSize( ) ) ) { obsSet->setTabulatedWeights( tabulatedWeights ); } else { - throw std::runtime_error( "Error when setting tabulated weights, the size of the input weight vector should be consistent with " - "either the size of each individual observation set, or the combined size of all required observation sets." ); + throw std::runtime_error( + "Error when setting tabulated weights, the size of the input weight vector " + "should be consistent with " + "either the size of each individual observation set, or the combined size " + "of all required observation sets." ); } } } - void setTabulatedWeights( - const std::map< std::shared_ptr< ObservationCollectionParser >, Eigen::VectorXd > weightsPerObservationParser ) + void setTabulatedWeights( const std::map< std::shared_ptr< ObservationCollectionParser >, + Eigen::VectorXd > weightsPerObservationParser ) { - for ( auto parserIt : weightsPerObservationParser ) + for( auto parserIt: weightsPerObservationParser ) { setTabulatedWeights( parserIt.second, parserIt.first ); } } - void filterObservations( const std::map< std::shared_ptr< ObservationCollectionParser >, std::shared_ptr< ObservationFilterBase > >& observationFilters, - const bool saveFilteredObservations = true ) + void filterObservations( + const std::map< std::shared_ptr< ObservationCollectionParser >, + std::shared_ptr< ObservationFilterBase > >& observationFilters, + const bool saveFilteredObservations = true ) { // Parse all observation filters - for ( auto filterIt : observationFilters ) + for( auto filterIt: observationFilters ) { std::shared_ptr< ObservationCollectionParser > observationParser = filterIt.first; std::shared_ptr< ObservationFilterBase > filter = filterIt.second; // Retrieve single observation sets based on observation parser - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > singleObsSets = - getSingleObservationSets( observationParser ); + std::vector< + std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + singleObsSets = getSingleObservationSets( observationParser ); // Filter observations for all single observation sets - for ( auto obsSet : singleObsSets ) + for( auto obsSet: singleObsSets ) { - obsSet->filterObservations( filter, saveFilteredObservations ); + obsSet->filterObservations( filter, saveFilteredObservations ); } } @@ -2211,47 +2682,62 @@ class ObservationCollection setConcatenatedObservationsAndTimes( ); } - void filterObservations( std::shared_ptr< ObservationFilterBase > observationFilter, - std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ), - const bool saveFilteredObservations = true ) + void filterObservations( std::shared_ptr< ObservationFilterBase > observationFilter, + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ), + const bool saveFilteredObservations = true ) { - std::map< std::shared_ptr< ObservationCollectionParser >, std::shared_ptr< ObservationFilterBase > > observationFilters - { { observationParser, observationFilter } }; + std::map< std::shared_ptr< ObservationCollectionParser >, + std::shared_ptr< ObservationFilterBase > > + observationFilters{ { observationParser, observationFilter } }; filterObservations( observationFilters, saveFilteredObservations ); } void splitObservationSets( std::shared_ptr< ObservationSetSplitterBase > observationSetSplitter, - std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) + std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) { // Retrieve single observation sets based on observation parser - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > singleObsSetsIndices = getSingleObservationSetsIndices( observationParser ); + std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > + singleObsSetsIndices = getSingleObservationSetsIndices( observationParser ); - // Boolean denoting whether the warning about splitting observation sets which contain filtered observations has already been - // printed or not (this warning should only been thrown out once). + // Boolean denoting whether the warning about splitting observation sets which contain + // filtered observations has already been printed or not (this warning should only been + // thrown out once). bool warningPrinted = false; - for ( auto observableIt : singleObsSetsIndices ) + for( auto observableIt: singleObsSetsIndices ) { - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > singleObsSets = observationSetList_.at( observableIt.first ).at( linkEndsIt.first ); + std::vector< + std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + singleObsSets = + observationSetList_.at( observableIt.first ).at( linkEndsIt.first ); unsigned int obsSetCounter = 0; - for ( auto indexSetToSplit : linkEndsIt.second ) + for( auto indexSetToSplit: linkEndsIt.second ) { // Get new observation sets after splitting - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > newObsSets = - splitObservationSet( singleObsSets.at( indexSetToSplit + obsSetCounter ), observationSetSplitter, !warningPrinted ); + std::vector< std::shared_ptr< + SingleObservationSet< ObservationScalarType, TimeType > > > + newObsSets = splitObservationSet( + singleObsSets.at( indexSetToSplit + obsSetCounter ), + observationSetSplitter, + !warningPrinted ); warningPrinted = true; // Remove original observation set - singleObsSets.erase( singleObsSets.begin( ) + ( indexSetToSplit + obsSetCounter ) ); + singleObsSets.erase( singleObsSets.begin( ) + + ( indexSetToSplit + obsSetCounter ) ); // Replace by new sets - for ( auto newSet : newObsSets ) + for( auto newSet: newObsSets ) { - singleObsSets.insert( singleObsSets.begin( ) + ( indexSetToSplit + obsSetCounter ), newSet ); + singleObsSets.insert( + singleObsSets.begin( ) + ( indexSetToSplit + obsSetCounter ), + newSet ); obsSetCounter++; } @@ -2267,30 +2753,44 @@ class ObservationCollection setConcatenatedObservationsAndTimes( ); } - void replaceSingleObservationSet( const std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > >& newSet, - const unsigned int setIndex ) + void replaceSingleObservationSet( + const std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > >& + newSet, + const unsigned int setIndex ) { - if ( observationSetList_.count( newSet->getObservableType( ) ) == 0 ) + if( observationSetList_.count( newSet->getObservableType( ) ) == 0 ) { - throw std::runtime_error( "Error when replacing single observation set in observation collection, observable type not found." ); + throw std::runtime_error( + "Error when replacing single observation set in observation collection, " + "observable type not found." ); } - if ( observationSetList_.at( newSet->getObservableType( ) ).count( newSet->getLinkEnds( ).linkEnds_ ) == 0 ) + if( observationSetList_.at( newSet->getObservableType( ) ) + .count( newSet->getLinkEnds( ).linkEnds_ ) == 0 ) { - throw std::runtime_error( "Error when replacing single observation set in observation collection, link ends not found for given observable type." ); + throw std::runtime_error( + "Error when replacing single observation set in observation collection, link " + "ends not found for given observable type." ); } - if ( setIndex > observationSetList_.at( newSet->getObservableType( ) ).at( newSet->getLinkEnds( ).linkEnds_ ).size( ) - 1 ) + if( setIndex > observationSetList_.at( newSet->getObservableType( ) ) + .at( newSet->getLinkEnds( ).linkEnds_ ) + .size( ) - + 1 ) { - throw std::runtime_error( "Error when replacing single observation set in observation collection, " - "set index exceeds number of sets found for given observable and link ends." ); + throw std::runtime_error( + "Error when replacing single observation set in observation collection, " + "set index exceeds number of sets found for given observable and link ends." ); } - observationSetList_.at( newSet->getObservableType( ) ).at( newSet->getLinkEnds( ).linkEnds_ ).at( setIndex ) = newSet; + observationSetList_.at( newSet->getObservableType( ) ) + .at( newSet->getLinkEnds( ).linkEnds_ ) + .at( setIndex ) = newSet; // Reset observation set indices and concatenated observations and times setObservationSetIndices( ); setConcatenatedObservationsAndTimes( ); } - void removeSingleObservationSets( const std::shared_ptr< ObservationCollectionParser > observationParser ) + void removeSingleObservationSets( + const std::shared_ptr< ObservationCollectionParser > observationParser ) { // Retrieve single observation sets based on observation parser removeSingleObservationSets( getSingleObservationSetsIndices( observationParser ) ); @@ -2298,17 +2798,18 @@ class ObservationCollection void removeEmptySingleObservationSets( ) { - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > indicesSetsToRemove; + std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > + indicesSetsToRemove; - for ( auto observableIt : observationSetList_ ) + for( auto observableIt: observationSetList_ ) { std::map< LinkEnds, std::vector< unsigned int > > indicesToRemovePerLinkEnds; - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { // Identify empty observation sets - for ( unsigned int k = 0 ; k < linkEndsIt.second.size( ) ; k++ ) + for( unsigned int k = 0; k < linkEndsIt.second.size( ); k++ ) { - if ( linkEndsIt.second.at( k )->getNumberOfObservables( ) == 0 ) + if( linkEndsIt.second.at( k )->getNumberOfObservables( ) == 0 ) { indicesToRemovePerLinkEnds[ linkEndsIt.first ].push_back( k ); } @@ -2321,31 +2822,47 @@ class ObservationCollection removeSingleObservationSets( indicesSetsToRemove ); } - void removeSingleObservationSets( const std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > >& indicesSetsToRemove ) + void removeSingleObservationSets( + const std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > >& + indicesSetsToRemove ) { // Parse observation set list and remove selected sets - for ( auto observableIt : indicesSetsToRemove ) + for( auto observableIt: indicesSetsToRemove ) { - if ( observationSetList_.count( observableIt.first ) == 0 ) + if( observationSetList_.count( observableIt.first ) == 0 ) { - throw std::runtime_error( "Error when removing single observation sets, given observable type not found." ); + throw std::runtime_error( + "Error when removing single observation sets, given observable type not " + "found." ); } - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - if ( observationSetList_.at( observableIt.first ).count( linkEndsIt.first ) == 0 ) + if( observationSetList_.at( observableIt.first ).count( linkEndsIt.first ) == 0 ) { - throw std::runtime_error( "Error when removing single observation sets, link ends not found for given observable type." ); + throw std::runtime_error( + "Error when removing single observation sets, link ends not found for " + "given observable type." ); } unsigned int counterRemovedSets = 0; - for ( auto indexToRemove : linkEndsIt.second ) + for( auto indexToRemove: linkEndsIt.second ) { - if ( indexToRemove - counterRemovedSets > observationSetList_.at( observableIt.first ).at( linkEndsIt.first ).size( ) - 1 ) + if( indexToRemove - counterRemovedSets > + observationSetList_.at( observableIt.first ) + .at( linkEndsIt.first ) + .size( ) - + 1 ) { - throw std::runtime_error( "Error when removing single observation sets, set index exceeds number of observation sets" - " for given observation type and link ends." ); + throw std::runtime_error( + "Error when removing single observation sets, set index exceeds " + "number of observation sets" + " for given observation type and link ends." ); } - observationSetList_.at( observableIt.first ).at( linkEndsIt.first ).erase( - observationSetList_.at( observableIt.first ).at( linkEndsIt.first ).begin( ) + indexToRemove - counterRemovedSets ); + observationSetList_.at( observableIt.first ) + .at( linkEndsIt.first ) + .erase( observationSetList_.at( observableIt.first ) + .at( linkEndsIt.first ) + .begin( ) + + indexToRemove - counterRemovedSets ); counterRemovedSets += 1; } } @@ -2356,22 +2873,24 @@ class ObservationCollection setConcatenatedObservationsAndTimes( ); } - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > getSingleObservationSetsIndices( - const std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) const + std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > + getSingleObservationSetsIndices( + const std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) const { - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > observationSetsIndices; + std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > + observationSetsIndices; ObservationParserType parserType = observationParser->getObservationParserType( ); switch( parserType ) { - case empty_parser: - { - for ( auto observableIt : observationSetList_ ) + case empty_parser: { + for( auto observableIt: observationSetList_ ) { std::map< LinkEnds, std::vector< unsigned int > > indicesPerObservable; - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( unsigned int k = 0 ; k < linkEndsIt.second.size( ) ; k++ ) + for( unsigned int k = 0; k < linkEndsIt.second.size( ); k++ ) { indicesPerObservable[ linkEndsIt.first ].push_back( k ); } @@ -2380,18 +2899,26 @@ class ObservationCollection } break; } - case observable_type_parser: - { - std::vector< ObservableType > observableTypes = std::dynamic_pointer_cast< ObservationCollectionObservableTypeParser >( observationParser )->getObservableTypes( ); - for ( auto observableIt : observationSetList_ ) + case observable_type_parser: { + std::vector< ObservableType > observableTypes = + std::dynamic_pointer_cast< ObservationCollectionObservableTypeParser >( + observationParser ) + ->getObservableTypes( ); + for( auto observableIt: observationSetList_ ) { - if ( ( ( std::count( observableTypes.begin( ), observableTypes.end( ), observableIt.first ) ) && ( !observationParser->useOppositeCondition( ) ) ) || - ( ( std::count( observableTypes.begin( ), observableTypes.end( ), observableIt.first ) == 0 ) && ( observationParser->useOppositeCondition( ) ) ) ) + if( ( ( std::count( observableTypes.begin( ), + observableTypes.end( ), + observableIt.first ) ) && + ( !observationParser->useOppositeCondition( ) ) ) || + ( ( std::count( observableTypes.begin( ), + observableTypes.end( ), + observableIt.first ) == 0 ) && + ( observationParser->useOppositeCondition( ) ) ) ) { std::map< LinkEnds, std::vector< unsigned int > > indicesPerObservable; - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( unsigned int k = 0 ; k < linkEndsIt.second.size( ) ; k++ ) + for( unsigned int k = 0; k < linkEndsIt.second.size( ); k++ ) { indicesPerObservable[ linkEndsIt.first ].push_back( k ); } @@ -2402,31 +2929,41 @@ class ObservationCollection break; } - case link_ends_parser: - { - std::vector< LinkEnds > linkEndsVector = std::dynamic_pointer_cast< ObservationCollectionLinkEndsParser >( observationParser )->getLinkEndsVector( ); + case link_ends_parser: { + std::vector< LinkEnds > linkEndsVector = + std::dynamic_pointer_cast< ObservationCollectionLinkEndsParser >( + observationParser ) + ->getLinkEndsVector( ); std::vector< LinkEnds > allLinkEnds = getLinkEnds( ); - for ( auto linkEnds : allLinkEnds ) + for( auto linkEnds: allLinkEnds ) { - if ( ( ( std::count( linkEndsVector.begin( ), linkEndsVector.end( ), linkEnds ) ) && ( !observationParser->useOppositeCondition( ) ) ) || - ( ( std::count( linkEndsVector.begin( ), linkEndsVector.end( ), linkEnds ) == 0 ) && ( observationParser->useOppositeCondition( ) ) ) ) + if( ( ( std::count( + linkEndsVector.begin( ), linkEndsVector.end( ), linkEnds ) ) && + ( !observationParser->useOppositeCondition( ) ) ) || + ( ( std::count( linkEndsVector.begin( ), + linkEndsVector.end( ), + linkEnds ) == 0 ) && + ( observationParser->useOppositeCondition( ) ) ) ) { - for ( auto observableIt : observationSetList_ ) + for( auto observableIt: observationSetList_ ) { std::map< LinkEnds, std::vector< unsigned int > > indicesPerObservable; - if ( observationSetsIndices.count( observableIt.first ) ) + if( observationSetsIndices.count( observableIt.first ) ) { - indicesPerObservable = observationSetsIndices.at( observableIt.first ); + indicesPerObservable = + observationSetsIndices.at( observableIt.first ); } - if ( observableIt.second.count( linkEnds ) ) + if( observableIt.second.count( linkEnds ) ) { - for ( unsigned int k = 0 ; k < observableIt.second.at( linkEnds ).size( ) ; k++ ) + for( unsigned int k = 0; + k < observableIt.second.at( linkEnds ).size( ); + k++ ) { indicesPerObservable[ linkEnds ].push_back( k ); } } - if ( indicesPerObservable.size( ) > 0 ) + if( indicesPerObservable.size( ) > 0 ) { observationSetsIndices[ observableIt.first ] = indicesPerObservable; } @@ -2435,16 +2972,17 @@ class ObservationCollection } break; } - case link_end_string_parser: - { - std::shared_ptr< ObservationCollectionLinkEndStringParser > linkEndStringObservationParser = - std::dynamic_pointer_cast< ObservationCollectionLinkEndStringParser >( observationParser ); - std::vector< std::string > linkEndsNames = linkEndStringObservationParser->getLinkEndNames( ); + case link_end_string_parser: { + std::shared_ptr< ObservationCollectionLinkEndStringParser > + linkEndStringObservationParser = std::dynamic_pointer_cast< + ObservationCollectionLinkEndStringParser >( observationParser ); + std::vector< std::string > linkEndsNames = + linkEndStringObservationParser->getLinkEndNames( ); bool isReferencePoint = linkEndStringObservationParser->isReferencePoint( ); // Retrieve all body names std::vector< std::string > allLinkEndsNames; - if ( !isReferencePoint ) + if( !isReferencePoint ) { allLinkEndsNames = getBodiesInLinkEnds( ); } @@ -2453,46 +2991,52 @@ class ObservationCollection allLinkEndsNames = getReferencePointsInLinkEnds( ); } - for ( auto name : allLinkEndsNames ) + for( auto name: allLinkEndsNames ) { - if ( ( ( std::count( linkEndsNames.begin( ), linkEndsNames.end( ), name ) ) && ( !observationParser->useOppositeCondition( ) ) ) || - ( ( std::count( linkEndsNames.begin( ), linkEndsNames.end( ), name ) == 0 ) && ( observationParser->useOppositeCondition( ) ) ) ) + if( ( ( std::count( linkEndsNames.begin( ), linkEndsNames.end( ), name ) ) && + ( !observationParser->useOppositeCondition( ) ) ) || + ( ( std::count( linkEndsNames.begin( ), linkEndsNames.end( ), name ) == + 0 ) && + ( observationParser->useOppositeCondition( ) ) ) ) { - for ( auto observableIt : observationSetList_ ) + for( auto observableIt: observationSetList_ ) { std::map< LinkEnds, std::vector< unsigned int > > indicesPerObservable; - if ( observationSetsIndices.count( observableIt.first ) ) + if( observationSetsIndices.count( observableIt.first ) ) { - indicesPerObservable = observationSetsIndices.at( observableIt.first ); + indicesPerObservable = + observationSetsIndices.at( observableIt.first ); } - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { // FIX! bool isBodyInLinkEnds = false; bool isGroundStationInLinkEnds = false; - for ( auto it : linkEndsIt.first ) + for( auto it: linkEndsIt.first ) { - if ( it.second.bodyName_ == name ) + if( it.second.bodyName_ == name ) { isBodyInLinkEnds = true; } - if ( it.second.stationName_ == name ) + if( it.second.stationName_ == name ) { isGroundStationInLinkEnds = true; } } - if ( ( !isReferencePoint && isBodyInLinkEnds/*( linkEndsIt.first, name )*/ ) || - ( isReferencePoint && isGroundStationInLinkEnds/*( linkEndsIt.first, name )*/ ) ) + if( ( !isReferencePoint && + isBodyInLinkEnds /*( linkEndsIt.first, name )*/ ) || + ( isReferencePoint && + isGroundStationInLinkEnds /*( linkEndsIt.first, name )*/ ) ) { - for ( unsigned int k = 0 ; k < linkEndsIt.second.size( ) ; k++ ) + for( unsigned int k = 0; k < linkEndsIt.second.size( ); k++ ) { indicesPerObservable[ linkEndsIt.first ].push_back( k ); } } } - if ( indicesPerObservable.size( ) > 0 ) + if( indicesPerObservable.size( ) > 0 ) { observationSetsIndices[ observableIt.first ] = indicesPerObservable; } @@ -2501,46 +3045,48 @@ class ObservationCollection } break; } - case link_end_id_parser: - { + case link_end_id_parser: { std::shared_ptr< ObservationCollectionLinkEndIdParser > linkEndIdObservationParser = - std::dynamic_pointer_cast< ObservationCollectionLinkEndIdParser >( observationParser ); + std::dynamic_pointer_cast< ObservationCollectionLinkEndIdParser >( + observationParser ); std::vector< LinkEndId > linkEndIds = linkEndIdObservationParser->getLinkEndIds( ); - for ( auto linkEndId : linkEndIds ) + for( auto linkEndId: linkEndIds ) { - for ( auto observableIt : observationSetList_ ) + for( auto observableIt: observationSetList_ ) { std::map< LinkEnds, std::vector< unsigned int > > indicesPerObservable; - if ( observationSetsIndices.count( observableIt.first ) ) + if( observationSetsIndices.count( observableIt.first ) ) { indicesPerObservable = observationSetsIndices.at( observableIt.first ); } - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { LinkEnds linkEnds = linkEndsIt.first; // Check whether relevant link end id is present in linkEnds bool isLinkEndIdInLinkEnds = false; - for ( auto it : linkEnds ) + for( auto it: linkEnds ) { - if ( it.second == linkEndId ) + if( it.second == linkEndId ) { isLinkEndIdInLinkEnds = true; } } - if ( ( isLinkEndIdInLinkEnds && ( !observationParser->useOppositeCondition( ) ) ) || - ( !isLinkEndIdInLinkEnds && ( observationParser->useOppositeCondition( ) ) ) ) + if( ( isLinkEndIdInLinkEnds && + ( !observationParser->useOppositeCondition( ) ) ) || + ( !isLinkEndIdInLinkEnds && + ( observationParser->useOppositeCondition( ) ) ) ) { - for ( unsigned int k = 0 ; k < linkEndsIt.second.size( ) ; k++ ) + for( unsigned int k = 0; k < linkEndsIt.second.size( ); k++ ) { indicesPerObservable[ linkEndsIt.first ].push_back( k ); } } } - if ( indicesPerObservable.size( ) > 0 ) + if( indicesPerObservable.size( ) > 0 ) { observationSetsIndices[ observableIt.first ] = indicesPerObservable; } @@ -2548,46 +3094,50 @@ class ObservationCollection } break; } - case link_end_type_parser: - { - std::shared_ptr< ObservationCollectionLinkEndTypeParser > linkEndTypeObservationParser = - std::dynamic_pointer_cast< ObservationCollectionLinkEndTypeParser >( observationParser ); - std::vector< LinkEndType > linkEndTypes = linkEndTypeObservationParser->getLinkEndTypes( ); - - for ( auto linkEndType : linkEndTypes ) + case link_end_type_parser: { + std::shared_ptr< ObservationCollectionLinkEndTypeParser > + linkEndTypeObservationParser = + std::dynamic_pointer_cast< ObservationCollectionLinkEndTypeParser >( + observationParser ); + std::vector< LinkEndType > linkEndTypes = + linkEndTypeObservationParser->getLinkEndTypes( ); + + for( auto linkEndType: linkEndTypes ) { - for ( auto observableIt : observationSetList_ ) + for( auto observableIt: observationSetList_ ) { std::map< LinkEnds, std::vector< unsigned int > > indicesPerObservable; - if ( observationSetsIndices.count( observableIt.first ) ) + if( observationSetsIndices.count( observableIt.first ) ) { indicesPerObservable = observationSetsIndices.at( observableIt.first ); } - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { LinkEnds linkEnds = linkEndsIt.first; // Check whether relevant link end id is present in linkEnds bool isLinkEndTypeInLinkEnds = false; - for ( auto it : linkEnds ) + for( auto it: linkEnds ) { - if ( it.first == linkEndType ) + if( it.first == linkEndType ) { isLinkEndTypeInLinkEnds = true; } } - if ( ( isLinkEndTypeInLinkEnds && ( !observationParser->useOppositeCondition( ) ) ) || - ( !isLinkEndTypeInLinkEnds && ( observationParser->useOppositeCondition( ) ) ) ) + if( ( isLinkEndTypeInLinkEnds && + ( !observationParser->useOppositeCondition( ) ) ) || + ( !isLinkEndTypeInLinkEnds && + ( observationParser->useOppositeCondition( ) ) ) ) { - for ( unsigned int k = 0 ; k < linkEndsIt.second.size( ) ; k++ ) + for( unsigned int k = 0; k < linkEndsIt.second.size( ); k++ ) { indicesPerObservable[ linkEndsIt.first ].push_back( k ); } } } - if ( indicesPerObservable.size( ) > 0 ) + if( indicesPerObservable.size( ) > 0 ) { observationSetsIndices[ observableIt.first ] = indicesPerObservable; } @@ -2595,46 +3145,50 @@ class ObservationCollection } break; } - case single_link_end_parser: - { - std::shared_ptr< ObservationCollectionSingleLinkEndParser > singleLinkEndObservationParser = - std::dynamic_pointer_cast< ObservationCollectionSingleLinkEndParser >( observationParser ); - std::vector< std::pair< LinkEndType, LinkEndId > > singleLinkEnds = singleLinkEndObservationParser->getSingleLinkEnds( ); - - for ( auto singleLinkEnd : singleLinkEnds ) + case single_link_end_parser: { + std::shared_ptr< ObservationCollectionSingleLinkEndParser > + singleLinkEndObservationParser = std::dynamic_pointer_cast< + ObservationCollectionSingleLinkEndParser >( observationParser ); + std::vector< std::pair< LinkEndType, LinkEndId > > singleLinkEnds = + singleLinkEndObservationParser->getSingleLinkEnds( ); + + for( auto singleLinkEnd: singleLinkEnds ) { - for ( auto observableIt : observationSetList_ ) + for( auto observableIt: observationSetList_ ) { std::map< LinkEnds, std::vector< unsigned int > > indicesPerObservable; - if ( observationSetsIndices.count( observableIt.first ) ) + if( observationSetsIndices.count( observableIt.first ) ) { indicesPerObservable = observationSetsIndices.at( observableIt.first ); } - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { LinkEnds linkEnds = linkEndsIt.first; // Check whether relevant link end type is present in linkEnds bool isInLinkEnds = false; - for ( auto it : linkEnds ) + for( auto it: linkEnds ) { - if ( ( it.first == singleLinkEnd.first ) && ( it.second == singleLinkEnd.second ) ) + if( ( it.first == singleLinkEnd.first ) && + ( it.second == singleLinkEnd.second ) ) { isInLinkEnds = true; } } - if ( ( isInLinkEnds && ( !observationParser->useOppositeCondition( ) ) ) || - ( !isInLinkEnds && ( observationParser->useOppositeCondition( ) ) ) ) + if( ( isInLinkEnds && + ( !observationParser->useOppositeCondition( ) ) ) || + ( !isInLinkEnds && + ( observationParser->useOppositeCondition( ) ) ) ) { - for ( unsigned int k = 0 ; k < linkEndsIt.second.size( ) ; k++ ) + for( unsigned int k = 0; k < linkEndsIt.second.size( ); k++ ) { indicesPerObservable[ linkEndsIt.first ].push_back( k ); } } } - if ( indicesPerObservable.size( ) > 0 ) + if( indicesPerObservable.size( ) > 0 ) { observationSetsIndices[ observableIt.first ] = indicesPerObservable; } @@ -2642,38 +3196,44 @@ class ObservationCollection } break; } - case time_bounds_parser: - { - std::vector< std::pair< double, double > > timeBoundsVector = std::dynamic_pointer_cast< ObservationCollectionTimeBoundsParser >( observationParser )->getTimeBoundsVector( ); + case time_bounds_parser: { + std::vector< std::pair< double, double > > timeBoundsVector = + std::dynamic_pointer_cast< ObservationCollectionTimeBoundsParser >( + observationParser ) + ->getTimeBoundsVector( ); - for ( auto timeBounds : timeBoundsVector ) + for( auto timeBounds: timeBoundsVector ) { - for ( auto observableIt : observationSetList_ ) + for( auto observableIt: observationSetList_ ) { std::map< LinkEnds, std::vector< unsigned int > > indicesPerObservable; - if ( observationSetsIndices.count( observableIt.first ) ) + if( observationSetsIndices.count( observableIt.first ) ) { indicesPerObservable = observationSetsIndices.at( observableIt.first ); } - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( unsigned int k = 0 ; k < linkEndsIt.second.size( ) ; k++ ) + for( unsigned int k = 0; k < linkEndsIt.second.size( ); k++ ) { bool isInTimeBounds = false; - if ( ( linkEndsIt.second.at( k )->getTimeBounds( ).first >= timeBounds.first ) && - ( linkEndsIt.second.at( k )->getTimeBounds( ).second <= timeBounds.second ) ) + if( ( linkEndsIt.second.at( k )->getTimeBounds( ).first >= + timeBounds.first ) && + ( linkEndsIt.second.at( k )->getTimeBounds( ).second <= + timeBounds.second ) ) { isInTimeBounds = true; } - if ( ( isInTimeBounds && ( !observationParser->useOppositeCondition( ) ) ) || - ( !isInTimeBounds && ( observationParser->useOppositeCondition( ) ) ) ) + if( ( isInTimeBounds && + ( !observationParser->useOppositeCondition( ) ) ) || + ( !isInTimeBounds && + ( observationParser->useOppositeCondition( ) ) ) ) { indicesPerObservable[ linkEndsIt.first ].push_back( k ); } } } - if ( indicesPerObservable.size( ) > 0 ) + if( indicesPerObservable.size( ) > 0 ) { observationSetsIndices[ observableIt.first ] = indicesPerObservable; } @@ -2681,39 +3241,48 @@ class ObservationCollection } break; } - case ancillary_settings_parser: - { - std::vector< std::shared_ptr< ObservationAncilliarySimulationSettings > > ancillarySettings = - std::dynamic_pointer_cast< ObservationCollectionAncillarySettingsParser >( observationParser )->getAncillarySettings( ); - - for ( auto setting : ancillarySettings ) + case ancillary_settings_parser: { + std::vector< std::shared_ptr< ObservationAncilliarySimulationSettings > > + ancillarySettings = std::dynamic_pointer_cast< + ObservationCollectionAncillarySettingsParser >( + observationParser ) + ->getAncillarySettings( ); + + for( auto setting: ancillarySettings ) { - for ( auto observableIt : observationSetList_ ) + for( auto observableIt: observationSetList_ ) { std::map< LinkEnds, std::vector< unsigned int > > indicesPerObservable; - if ( observationSetsIndices.count( observableIt.first ) ) + if( observationSetsIndices.count( observableIt.first ) ) { indicesPerObservable = observationSetsIndices.at( observableIt.first ); } - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( unsigned int k = 0 ; k < linkEndsIt.second.size( ) ; k++ ) + for( unsigned int k = 0; k < linkEndsIt.second.size( ); k++ ) { bool identicalAncillarySettings = false; - if ( ( linkEndsIt.second.at( k )->getAncilliarySettings( )->getDoubleData( ) == setting->getDoubleData( ) ) && - ( linkEndsIt.second.at( k )->getAncilliarySettings( )->getDoubleVectorData( ) == setting->getDoubleVectorData( ) ) ) + if( ( linkEndsIt.second.at( k ) + ->getAncilliarySettings( ) + ->getDoubleData( ) == setting->getDoubleData( ) ) && + ( linkEndsIt.second.at( k ) + ->getAncilliarySettings( ) + ->getDoubleVectorData( ) == + setting->getDoubleVectorData( ) ) ) { identicalAncillarySettings = true; } - if ( ( identicalAncillarySettings && !observationParser->useOppositeCondition( ) ) || - ( !identicalAncillarySettings && observationParser->useOppositeCondition( ) ) ) + if( ( identicalAncillarySettings && + !observationParser->useOppositeCondition( ) ) || + ( !identicalAncillarySettings && + observationParser->useOppositeCondition( ) ) ) { indicesPerObservable[ linkEndsIt.first ].push_back( k ); } } } - if ( indicesPerObservable.size( ) > 0 ) + if( indicesPerObservable.size( ) > 0 ) { observationSetsIndices[ observableIt.first ] = indicesPerObservable; } @@ -2721,41 +3290,55 @@ class ObservationCollection } break; } - case multi_type_parser: - { - std::shared_ptr< ObservationCollectionMultiTypeParser > multiTypeParser = std::dynamic_pointer_cast< ObservationCollectionMultiTypeParser >( observationParser ); - std::vector< std::shared_ptr< ObservationCollectionParser > > observationParsers = multiTypeParser->getObservationParsers_( ); + case multi_type_parser: { + std::shared_ptr< ObservationCollectionMultiTypeParser > multiTypeParser = + std::dynamic_pointer_cast< ObservationCollectionMultiTypeParser >( + observationParser ); + std::vector< std::shared_ptr< ObservationCollectionParser > > observationParsers = + multiTypeParser->getObservationParsers_( ); bool areConditionsCombined = multiTypeParser->areConditionsCombined( ); - if ( !areConditionsCombined ) + if( !areConditionsCombined ) { - for ( auto parser : observationParsers ) + for( auto parser: observationParsers ) { - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > currentObservationSetsIndices = getSingleObservationSetsIndices( parser ); + std::map< ObservableType, + std::map< LinkEnds, std::vector< unsigned int > > > + currentObservationSetsIndices = + getSingleObservationSetsIndices( parser ); - for ( auto observableIt : currentObservationSetsIndices ) + for( auto observableIt: currentObservationSetsIndices ) { - if ( observationSetsIndices.count( observableIt.first ) == 0 ) + if( observationSetsIndices.count( observableIt.first ) == 0 ) { observationSetsIndices[ observableIt.first ] = observableIt.second; } else { - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - if ( observationSetsIndices.at( observableIt.first ).count( linkEndsIt.first ) == 0 ) + if( observationSetsIndices.at( observableIt.first ) + .count( linkEndsIt.first ) == 0 ) { - observationSetsIndices.at( observableIt.first )[ linkEndsIt.first ] = linkEndsIt.second; + observationSetsIndices.at( + observableIt.first )[ linkEndsIt.first ] = + linkEndsIt.second; } else { - std::vector< unsigned int > indices = observationSetsIndices.at( observableIt.first ).at( linkEndsIt.first ); - for ( auto index : linkEndsIt.second ) + std::vector< unsigned int > indices = + observationSetsIndices.at( observableIt.first ) + .at( linkEndsIt.first ); + for( auto index: linkEndsIt.second ) { - if ( std::count( indices.begin( ), indices.end( ), index ) == 0 ) + if( std::count( indices.begin( ), + indices.end( ), + index ) == 0 ) { - observationSetsIndices.at( observableIt.first ).at( linkEndsIt.first ).push_back( index ); + observationSetsIndices.at( observableIt.first ) + .at( linkEndsIt.first ) + .push_back( index ); } } } @@ -2768,51 +3351,78 @@ class ObservationCollection else { // First retrieve all observation sets - std::vector< std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > > allObservationSetsIndices; - for ( auto parser : observationParsers ) + std::vector< std::map< ObservableType, + std::map< LinkEnds, std::vector< unsigned int > > > > + allObservationSetsIndices; + for( auto parser: observationParsers ) { - allObservationSetsIndices.push_back( getSingleObservationSetsIndices( parser ) ); + allObservationSetsIndices.push_back( + getSingleObservationSetsIndices( parser ) ); } - if ( allObservationSetsIndices.size( ) > 0 ) + if( allObservationSetsIndices.size( ) > 0 ) { - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > originalSetsIndices = allObservationSetsIndices.at( 0 ); + std::map< ObservableType, + std::map< LinkEnds, std::vector< unsigned int > > > + originalSetsIndices = allObservationSetsIndices.at( 0 ); // Retrieve common observable types - std::vector< ObservableType > commonObsTypes = utilities::createVectorFromMapKeys( originalSetsIndices ); - for ( unsigned int k = 1 ; k < allObservationSetsIndices.size( ) ; k++ ) + std::vector< ObservableType > commonObsTypes = + utilities::createVectorFromMapKeys( originalSetsIndices ); + for( unsigned int k = 1; k < allObservationSetsIndices.size( ); k++ ) { - std::vector< ObservableType > currentObsTypes = utilities::createVectorFromMapKeys( allObservationSetsIndices.at( k ) ); + std::vector< ObservableType > currentObsTypes = + utilities::createVectorFromMapKeys( + allObservationSetsIndices.at( k ) ); std::vector< ObservableType > newCommonTypes; - std::set_intersection( commonObsTypes.begin( ), commonObsTypes.end( ), currentObsTypes.begin( ), currentObsTypes.end( ), + std::set_intersection( commonObsTypes.begin( ), + commonObsTypes.end( ), + currentObsTypes.begin( ), + currentObsTypes.end( ), std::back_inserter( newCommonTypes ) ); commonObsTypes = newCommonTypes; } - for ( auto obsType : commonObsTypes ) + for( auto obsType: commonObsTypes ) { std::map< LinkEnds, std::vector< unsigned int > > indicesPerObservable; // For given observable type, retrieve common link ends - std::vector< LinkEnds > commonLinkEndsList = utilities::createVectorFromMapKeys( originalSetsIndices.at( obsType ) ); - for ( unsigned int k = 1 ; k < allObservationSetsIndices.size( ) ; k++ ) + std::vector< LinkEnds > commonLinkEndsList = + utilities::createVectorFromMapKeys( + originalSetsIndices.at( obsType ) ); + for( unsigned int k = 1; k < allObservationSetsIndices.size( ); k++ ) { - std::vector< LinkEnds > currentLinkEndsList = utilities::createVectorFromMapKeys( allObservationSetsIndices.at( k ).at( obsType ) ); + std::vector< LinkEnds > currentLinkEndsList = + utilities::createVectorFromMapKeys( + allObservationSetsIndices.at( k ).at( obsType ) ); std::vector< LinkEnds > newCommonLinkEndsList; - std::set_intersection( commonLinkEndsList.begin( ), commonLinkEndsList.end( ), currentLinkEndsList.begin( ), currentLinkEndsList.end( ), - std::back_inserter( newCommonLinkEndsList ) ); + std::set_intersection( + commonLinkEndsList.begin( ), + commonLinkEndsList.end( ), + currentLinkEndsList.begin( ), + currentLinkEndsList.end( ), + std::back_inserter( newCommonLinkEndsList ) ); commonLinkEndsList = newCommonLinkEndsList; } - for ( auto linkEnds : commonLinkEndsList ) + for( auto linkEnds: commonLinkEndsList ) { - // For given observable type and link ends, retrieve common observation set indices - std::vector< unsigned int > commonIndices = originalSetsIndices.at( obsType ).at( linkEnds ); - for ( unsigned int k = 1 ; k < allObservationSetsIndices.size( ) ; k++ ) + // For given observable type and link ends, retrieve common + // observation set indices + std::vector< unsigned int > commonIndices = + originalSetsIndices.at( obsType ).at( linkEnds ); + for( unsigned int k = 1; k < allObservationSetsIndices.size( ); + k++ ) { - std::vector< unsigned int > currentIndices = allObservationSetsIndices.at( k ).at( obsType ).at( linkEnds ); + std::vector< unsigned int > currentIndices = + allObservationSetsIndices.at( k ).at( obsType ).at( + linkEnds ); std::vector< unsigned int > newCommonIndices; - std::set_intersection( commonIndices.begin( ), commonIndices.end( ), currentIndices.begin( ), currentIndices.end( ), + std::set_intersection( commonIndices.begin( ), + commonIndices.end( ), + currentIndices.begin( ), + currentIndices.end( ), std::back_inserter( newCommonIndices ) ); commonIndices = newCommonIndices; } @@ -2832,20 +3442,25 @@ class ObservationCollection return observationSetsIndices; } - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > getSingleObservationSets( - const std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + getSingleObservationSets( + const std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) { - std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > singleObservationSetsIndices = - getSingleObservationSetsIndices( observationParser ); + std::map< ObservableType, std::map< LinkEnds, std::vector< unsigned int > > > + singleObservationSetsIndices = getSingleObservationSetsIndices( observationParser ); - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > singleObservationSets; - for ( auto observableIt : singleObservationSetsIndices ) + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + singleObservationSets; + for( auto observableIt: singleObservationSetsIndices ) { - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( auto index : linkEndsIt.second ) + for( auto index: linkEndsIt.second ) { - singleObservationSets.push_back( observationSetList_.at( observableIt.first ).at( linkEndsIt.first ).at( index ) ); + singleObservationSets.push_back( observationSetList_.at( observableIt.first ) + .at( linkEndsIt.first ) + .at( index ) ); } } } @@ -2854,14 +3469,18 @@ class ObservationCollection void printObservationSetsStartAndSize( ) const { - std::cout << "Observation collection structure: start and size of individual observation sets" << std::endl; - for ( auto observableIt : observationSetStartAndSize_ ) + std::cout + << "Observation collection structure: start and size of individual observation sets" + << std::endl; + for( auto observableIt: observationSetStartAndSize_ ) { - for ( auto linkEndsIt : observableIt.second ) + for( auto linkEndsIt: observableIt.second ) { - for ( auto indices : linkEndsIt.second ) + for( auto indices: linkEndsIt.second ) { - std::cout << "start index " << indices.first << " - size " << indices.second << " : " << "type " << observableIt.first << ", link ends " << getLinkEndsString( linkEndsIt.first ) << std::endl; + std::cout << "start index " << indices.first << " - size " << indices.second + << " : " << "type " << observableIt.first << ", link ends " + << getLinkEndsString( linkEndsIt.first ) << std::endl; } } } @@ -2869,190 +3488,280 @@ class ObservationCollection } void setReferencePoint( simulation_setup::SystemOfBodies& bodies, - const Eigen::Vector3d& antennaPosition, - const std::string& antennaName, - const std::string& spacecraftName, - const LinkEndType linkEndType, - const std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) + const Eigen::Vector3d& antennaPosition, + const std::string& antennaName, + const std::string& spacecraftName, + const LinkEndType linkEndType, + const std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) { // Retrieve existing reference points with fixed position in the spacecraft-fixed frame - std::map< std::string, std::shared_ptr< ephemerides::ConstantEphemeris > > existingReferencePoints = bodies.at( spacecraftName )->getVehicleSystems( )->getFixedReferencePoints( ); + std::map< std::string, std::shared_ptr< ephemerides::ConstantEphemeris > > + existingReferencePoints = bodies.at( spacecraftName ) + ->getVehicleSystems( ) + ->getFixedReferencePoints( ); // Check if reference point exists and create missing antenna if necessary bool antennaDetected = false; - for ( auto refPointsIt : existingReferencePoints ) + for( auto refPointsIt: existingReferencePoints ) { - if ( ( refPointsIt.second->getCartesianState( ).segment( 0, 3 ) == antennaPosition ) && ( refPointsIt.first == antennaName ) ) + if( ( refPointsIt.second->getCartesianState( ).segment( 0, 3 ) == antennaPosition ) && + ( refPointsIt.first == antennaName ) ) { antennaDetected = true; } - else if ( ( refPointsIt.second->getCartesianState( ).segment( 0, 3 ) == antennaPosition ) && ( refPointsIt.first != antennaName ) ) + else if( ( refPointsIt.second->getCartesianState( ).segment( 0, 3 ) == + antennaPosition ) && + ( refPointsIt.first != antennaName ) ) { - throw std::runtime_error( "Error when setting reference point in observation collection, the required antenna position is already defined " - "as a reference point, but associated with different antenna name (" + antennaName + ")." ); + throw std::runtime_error( + "Error when setting reference point in observation collection, the " + "required antenna position is already defined " + "as a reference point, but associated with different antenna name (" + + antennaName + ")." ); } - else if ( ( refPointsIt.second->getCartesianState( ).segment( 0, 3 ) != antennaPosition ) && ( refPointsIt.first == antennaName ) ) + else if( ( refPointsIt.second->getCartesianState( ).segment( 0, 3 ) != + antennaPosition ) && + ( refPointsIt.first == antennaName ) ) { - throw std::runtime_error( "Error when setting reference point in observation collection, the required antenna name is already defined " - "as a reference point, but associated with different position." ); + throw std::runtime_error( + "Error when setting reference point in observation collection, the " + "required antenna name is already defined " + "as a reference point, but associated with different position." ); } } - if ( !antennaDetected ) + if( !antennaDetected ) { - bodies.at( spacecraftName )->getVehicleSystems( )->setReferencePointPosition( antennaName, antennaPosition ); + bodies.at( spacecraftName ) + ->getVehicleSystems( ) + ->setReferencePointPosition( antennaName, antennaPosition ); } - // Set reference point in observation collection - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > singleSets = getSingleObservationSets( observationParser ); - for ( auto set : singleSets ) + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + singleSets = getSingleObservationSets( observationParser ); + for( auto set: singleSets ) { std::map< LinkEndType, LinkEndId > newLinkEnds = set->getLinkEnds( ).linkEnds_; - newLinkEnds[ linkEndType ] = LinkEndId( std::make_pair( newLinkEnds[ linkEndType ].bodyName_, antennaName ) ); + newLinkEnds[ linkEndType ] = LinkEndId( + std::make_pair( newLinkEnds[ linkEndType ].bodyName_, antennaName ) ); LinkDefinition newLinkDefinition = LinkDefinition( newLinkEnds ); set->setLinkEnds( newLinkDefinition ); } - observationSetList_ = createSortedObservationSetList< ObservationScalarType, TimeType >( singleSets ); - setObservationSetIndices(); - setConcatenatedObservationsAndTimes(); + observationSetList_ = + createSortedObservationSetList< ObservationScalarType, TimeType >( singleSets ); + setObservationSetIndices( ); + setConcatenatedObservationsAndTimes( ); } - void setReferencePoints( simulation_setup::SystemOfBodies& bodies, - const std::map< double, Eigen::Vector3d >& antennaSwitchHistory, - const std::string& spacecraftName, - const LinkEndType linkEndType, - const std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) + void setReferencePoints( + simulation_setup::SystemOfBodies& bodies, + const std::map< double, Eigen::Vector3d >& antennaSwitchHistory, + const std::string& spacecraftName, + const LinkEndType linkEndType, + const std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) { // Check if reference points exist and create missing antenna if necessary std::map< double, std::string > antennaNames; unsigned int counter = 0; - for ( auto antennaIt : antennaSwitchHistory ) + for( auto antennaIt: antennaSwitchHistory ) { // Retrieve existing reference points with fixed position in the spacecraft-fixed frame - std::map< std::string, std::shared_ptr< ephemerides::ConstantEphemeris > > existingReferencePoints = bodies.at( spacecraftName )->getVehicleSystems( )->getFixedReferencePoints( ); + std::map< std::string, std::shared_ptr< ephemerides::ConstantEphemeris > > + existingReferencePoints = bodies.at( spacecraftName ) + ->getVehicleSystems( ) + ->getFixedReferencePoints( ); bool antennaDetected = false; - for ( auto refPointsIt : existingReferencePoints ) + for( auto refPointsIt: existingReferencePoints ) { - if ( refPointsIt.second->getCartesianState( ).segment( 0, 3 ) == antennaIt.second ) + if( refPointsIt.second->getCartesianState( ).segment( 0, 3 ) == antennaIt.second ) { antennaDetected = true; antennaNames[ antennaIt.first ] = refPointsIt.first; } } - if ( !antennaDetected ) + if( !antennaDetected ) { counter++; std::string newReferencePointName = "Antenna" + std::to_string( counter ); - bodies.at( spacecraftName )->getVehicleSystems( )->setReferencePointPosition( newReferencePointName, antennaIt.second ); + bodies.at( spacecraftName ) + ->getVehicleSystems( ) + ->setReferencePointPosition( newReferencePointName, antennaIt.second ); antennaNames[ antennaIt.first ] = newReferencePointName; } } // Retrieve switch times - std::vector< double > originalSwitchTimes = utilities::createVectorFromMapKeys( antennaSwitchHistory ); + std::vector< double > originalSwitchTimes = + utilities::createVectorFromMapKeys( antennaSwitchHistory ); std::pair< TimeType, TimeType > timeBounds = getTimeBounds( ); - if ( originalSwitchTimes.front( ) > timeBounds.first || originalSwitchTimes.back( ) < timeBounds.second ) + if( originalSwitchTimes.front( ) > timeBounds.first || + originalSwitchTimes.back( ) < timeBounds.second ) { - throw std::runtime_error( "Error when setting reference points in ObservationCollection, the antenna switch history does not cover the required observation time interval." ); + throw std::runtime_error( + "Error when setting reference points in ObservationCollection, the antenna " + "switch history does not cover the required observation time interval." ); } // Split single observation sets accordingly - splitObservationSets( observationSetSplitter( time_tags_splitter, originalSwitchTimes ), observationParser ); - + splitObservationSets( observationSetSplitter( time_tags_splitter, originalSwitchTimes ), + observationParser ); // Set reference points - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > singleSets = getSingleObservationSets( observationParser ); + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + singleSets = getSingleObservationSets( observationParser ); - for ( auto set : singleSets ) + for( auto set: singleSets ) { - TimeType setStartTime = set->getTimeBounds( ).first; - TimeType setEndTime = set->getTimeBounds( ).second; + TimeType setStartTime = set->getTimeBounds( ).first; + TimeType setEndTime = set->getTimeBounds( ).second; - for ( unsigned int k = 0 ; k < originalSwitchTimes.size( ) - 1 ; k++ ) + for( unsigned int k = 0; k < originalSwitchTimes.size( ) - 1; k++ ) { - if ( setStartTime >= originalSwitchTimes[ k ] && setEndTime <= originalSwitchTimes[ k+1 ] ) + if( setStartTime >= originalSwitchTimes[ k ] && + setEndTime <= originalSwitchTimes[ k + 1 ] ) { std::string currentAntenna = antennaNames[ originalSwitchTimes[ k ] ]; std::map< LinkEndType, LinkEndId > newLinkEnds = set->getLinkEnds( ).linkEnds_; - newLinkEnds[ linkEndType ] = LinkEndId( std::make_pair( newLinkEnds[ linkEndType ].bodyName_, currentAntenna ) ); + newLinkEnds[ linkEndType ] = LinkEndId( std::make_pair( + newLinkEnds[ linkEndType ].bodyName_, currentAntenna ) ); LinkDefinition newLinkDefinition = LinkDefinition( newLinkEnds ); set->setLinkEnds( newLinkDefinition ); } } } - observationSetList_ = createSortedObservationSetList< ObservationScalarType, TimeType >( singleSets ); - setObservationSetIndices(); - setConcatenatedObservationsAndTimes(); + observationSetList_ = + createSortedObservationSetList< ObservationScalarType, TimeType >( singleSets ); + setObservationSetIndices( ); + setConcatenatedObservationsAndTimes( ); } - - void setReferencePoint( simulation_setup::SystemOfBodies& bodies, - const std::shared_ptr< ephemerides::Ephemeris > antennaBodyFixedEphemeris, - const std::string& antennaName, - const std::string& spacecraftName, - const LinkEndType linkEndType, - const std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) + void setReferencePoint( + simulation_setup::SystemOfBodies& bodies, + const std::shared_ptr< ephemerides::Ephemeris > antennaBodyFixedEphemeris, + const std::string& antennaName, + const std::string& spacecraftName, + const LinkEndType linkEndType, + const std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) { // Retrieve existing reference points - std::map< std::string, std::shared_ptr< ephemerides::Ephemeris > > existingReferencePoints = bodies.at( spacecraftName )->getVehicleSystems( )->getReferencePoints( ); + std::map< std::string, std::shared_ptr< ephemerides::Ephemeris > > existingReferencePoints = + bodies.at( spacecraftName )->getVehicleSystems( )->getReferencePoints( ); // Check if ephemeris already defined for given reference point - if ( existingReferencePoints.count( antennaName ) > 0 ) + if( existingReferencePoints.count( antennaName ) > 0 ) { - std::cerr << "Warning when setting reference point (" << spacecraftName << ", " << antennaName << "), the reference point already exists. Its ephemeris will be overwritten."; + std::cerr + << "Warning when setting reference point (" << spacecraftName << ", " + << antennaName + << "), the reference point already exists. Its ephemeris will be overwritten."; } // Set reference point body-fixed ephemeris - bodies.at( spacecraftName )->getVehicleSystems( )->setReferencePointPosition( antennaName, antennaBodyFixedEphemeris ); + bodies.at( spacecraftName ) + ->getVehicleSystems( ) + ->setReferencePointPosition( antennaName, antennaBodyFixedEphemeris ); // Set reference point - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > singleSets = getSingleObservationSets( observationParser ); + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + singleSets = getSingleObservationSets( observationParser ); - for ( auto set : singleSets ) + for( auto set: singleSets ) { std::map< LinkEndType, LinkEndId > newLinkEnds = set->getLinkEnds( ).linkEnds_; - newLinkEnds[ linkEndType ] = LinkEndId( std::make_pair( newLinkEnds[ linkEndType ].bodyName_, antennaName ) ); + newLinkEnds[ linkEndType ] = LinkEndId( + std::make_pair( newLinkEnds[ linkEndType ].bodyName_, antennaName ) ); LinkDefinition newLinkDefinition = LinkDefinition( newLinkEnds ); set->setLinkEnds( newLinkDefinition ); } - observationSetList_ = createSortedObservationSetList< ObservationScalarType, TimeType >( singleSets ); - setObservationSetIndices(); - setConcatenatedObservationsAndTimes(); + observationSetList_ = + createSortedObservationSetList< ObservationScalarType, TimeType >( singleSets ); + setObservationSetIndices( ); + setConcatenatedObservationsAndTimes( ); } + void setTransponderDelay( + const std::string& spacecraftName, + const double transponderDelay, + const std::shared_ptr< ObservationCollectionParser > inputObservationParser = + std::make_shared< ObservationCollectionParser >( ) ) + { + // Create observation parser with the spacecraft name + std::shared_ptr< ObservationCollectionParser > spacecraftParser = + observationParser( spacecraftName ); - // Function to add an observation dependent variable to (a subset of) the single observation sets + // Create combined observation parser + std::shared_ptr< ObservationCollectionMultiTypeParser > jointParser = + std::make_shared< ObservationCollectionMultiTypeParser >( + std::vector< std::shared_ptr< ObservationCollectionParser > >( + { spacecraftParser, inputObservationParser } ), + true ); + + // Set transponder delay + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + singleSets = getSingleObservationSets( jointParser ); + + for( auto set: singleSets ) + { + std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > + ancilliarySettings = set->getAncilliarySettings( ); + + if( ancilliarySettings != nullptr ) + { + std::vector< double > linkEndsDelays_ = + ancilliarySettings->getAncilliaryDoubleVectorData( link_ends_delays, + false ); + linkEndsDelays_[ 1 ] = transponderDelay; + ancilliarySettings->setAncilliaryDoubleVectorData( link_ends_delays, + linkEndsDelays_ ); + } + } + } + + // Function to add an observation dependent variable to (a subset of) the single observation + // sets std::shared_ptr< ObservationCollectionParser > addDependentVariable( std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > settings, const simulation_setup::SystemOfBodies& bodies, - const std::shared_ptr< ObservationCollectionParser > parser = std::make_shared< ObservationCollectionParser >( ) ) + const std::shared_ptr< ObservationCollectionParser > parser = + std::make_shared< ObservationCollectionParser >( ) ) { - // Create observation collection parser corresponding to the required dependent variable settings - std::shared_ptr< ObservationCollectionParser > dependentVariablesParser = getObservationParserFromDependentVariableSettings( settings ); + // Create observation collection parser corresponding to the required dependent variable + // settings + std::shared_ptr< ObservationCollectionParser > dependentVariablesParser = + getObservationParserFromDependentVariableSettings( settings ); // Create combined observation parser - std::shared_ptr< ObservationCollectionMultiTypeParser > jointParser = std::make_shared< ObservationCollectionMultiTypeParser >( - std::vector< std::shared_ptr< ObservationCollectionParser > >( { dependentVariablesParser, parser } ), true ); + std::shared_ptr< ObservationCollectionMultiTypeParser > jointParser = + std::make_shared< ObservationCollectionMultiTypeParser >( + std::vector< std::shared_ptr< ObservationCollectionParser > >( + { dependentVariablesParser, parser } ), + true ); // Retrieve single observation sets from joint parser - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > singleObsSets = getSingleObservationSets( jointParser ); + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + singleObsSets = getSingleObservationSets( jointParser ); // Parse single observation sets - for ( auto set : singleObsSets ) + for( auto set: singleObsSets ) { // Retrieve single observation set observable type and link ends ObservableType observableType = set->getObservableType( ); LinkEnds linkEnds = set->getLinkEnds( ).linkEnds_; - // Retrieve complete list of all dependent variables settings compatible with the input settings (which might not be - // fully defined, i.e. with missing link ends information, etc.) - std::vector< std::shared_ptr< ObservationDependentVariableSettings > > allSettingsToCreate = - createAllCompatibleDependentVariableSettings( observableType, linkEnds, settings ); + // Retrieve complete list of all dependent variables settings compatible with the input + // settings (which might not be fully defined, i.e. with missing link ends information, + // etc.) + std::vector< std::shared_ptr< ObservationDependentVariableSettings > > + allSettingsToCreate = createAllCompatibleDependentVariableSettings( + observableType, linkEnds, settings ); // Add dependent variables for all compatible settings set->addDependentVariables( allSettingsToCreate, bodies ); @@ -3062,28 +3771,38 @@ class ObservationCollection return jointParser; } - - // Function to retrieve the values of a given dependent variable for (a subset of) the single observation sets, sorted per single observation set - std::pair< std::vector< Eigen::MatrixXd >, std::shared_ptr< ObservationCollectionParser > > getDependentVariables( - std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > dependentVariableSettings, - const bool returnFirstCompatibleSettings = false, - const std::shared_ptr< ObservationCollectionParser > parser = std::make_shared< ObservationCollectionParser >( ) ) - { - // Create observation collection parser corresponding to the required dependent variable settings - std::shared_ptr< ObservationCollectionParser > dependentVariablesParser = getObservationParserFromDependentVariableSettings( dependentVariableSettings ); - - // Create combined observation parser (for which the relevant dependent variable is also defined) - std::shared_ptr< ObservationCollectionMultiTypeParser > jointParser = std::make_shared< ObservationCollectionMultiTypeParser >( - std::vector< std::shared_ptr< ObservationCollectionParser > >( { dependentVariablesParser, parser } ), true ); + // Function to retrieve the values of a given dependent variable for (a subset of) the single + // observation sets, sorted per single observation set + std::pair< std::vector< Eigen::MatrixXd >, std::shared_ptr< ObservationCollectionParser > > + getDependentVariables( std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > + dependentVariableSettings, + const bool returnFirstCompatibleSettings = false, + const std::shared_ptr< ObservationCollectionParser > parser = + std::make_shared< ObservationCollectionParser >( ) ) + { + // Create observation collection parser corresponding to the required dependent variable + // settings + std::shared_ptr< ObservationCollectionParser > dependentVariablesParser = + getObservationParserFromDependentVariableSettings( dependentVariableSettings ); + + // Create combined observation parser (for which the relevant dependent variable is also + // defined) + std::shared_ptr< ObservationCollectionMultiTypeParser > jointParser = + std::make_shared< ObservationCollectionMultiTypeParser >( + std::vector< std::shared_ptr< ObservationCollectionParser > >( + { dependentVariablesParser, parser } ), + true ); // Retrieve single observation sets from joint parser - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > singleObsSets = getSingleObservationSets( jointParser ); + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + singleObsSets = getSingleObservationSets( jointParser ); // Retrieve dependent variables from the joint observation parser std::vector< Eigen::MatrixXd > dependentVariablesValues; - for ( auto set : singleObsSets ) + for( auto set: singleObsSets ) { - dependentVariablesValues.push_back( set->getSingleDependentVariable( dependentVariableSettings, returnFirstCompatibleSettings ) ); + dependentVariablesValues.push_back( set->getSingleDependentVariable( + dependentVariableSettings, returnFirstCompatibleSettings ) ); } // Return dependent variables values and joint parser @@ -3092,23 +3811,29 @@ class ObservationCollection //! Function that retrieves the time history of a given dependent variable, sorted per observation set. It must be noted that the reported epochs are the times at which the //! observations are computed/acquired, which might differ from the times at which the dependent variables are evaluated. - std::vector< std::map< TimeType, Eigen::VectorXd > > getDependentVariableHistoryPerObservationSet( - std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > dependentVariableSettings, + std::vector< std::map< TimeType, Eigen::VectorXd > > + getDependentVariableHistoryPerObservationSet( + std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > + dependentVariableSettings, const bool returnFirstCompatibleSettings = false, - const std::shared_ptr< ObservationCollectionParser > parser = std::make_shared< ObservationCollectionParser >( ) ) + const std::shared_ptr< ObservationCollectionParser > parser = + std::make_shared< ObservationCollectionParser >( ) ) { - std::pair< std::vector< Eigen::MatrixXd >, std::shared_ptr< ObservationCollectionParser > > dependentVariableResultList = - getDependentVariables( dependentVariableSettings, returnFirstCompatibleSettings, parser ); + std::pair< std::vector< Eigen::MatrixXd >, std::shared_ptr< ObservationCollectionParser > > + dependentVariableResultList = getDependentVariables( + dependentVariableSettings, returnFirstCompatibleSettings, parser ); // Retrieve single observation sets for which the dependent variable of interest is computed - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > singleObservationSets = - getSingleObservationSets( dependentVariableResultList.second ); + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + singleObservationSets = + getSingleObservationSets( dependentVariableResultList.second ); // Parse all relevant single observation sets std::vector< std::map< TimeType, Eigen::VectorXd > > dependentVariableResult; - for ( auto set : singleObservationSets ) + for( auto set: singleObservationSets ) { - dependentVariableResult.push_back( set->getSingleDependentVariableHistory( dependentVariableSettings, returnFirstCompatibleSettings ) ); + dependentVariableResult.push_back( set->getSingleDependentVariableHistory( + dependentVariableSettings, returnFirstCompatibleSettings ) ); } return dependentVariableResult; } @@ -3117,40 +3842,55 @@ class ObservationCollection //! It must be noted that the reported epochs are the times at which the observations are computed/acquired, which might //! differ from the times at which the dependent variables are evaluated. std::map< TimeType, Eigen::VectorXd > getDependentVariableHistory( - std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > dependentVariableSettings, + std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > + dependentVariableSettings, const bool returnFirstCompatibleSettings = false, - const std::shared_ptr< ObservationCollectionParser > parser = std::make_shared< ObservationCollectionParser >( ) ) + const std::shared_ptr< ObservationCollectionParser > parser = + std::make_shared< ObservationCollectionParser >( ) ) { - std::vector< std::map< TimeType, Eigen::VectorXd > > dependentVariableResultPerObservationSet = - getDependentVariableHistoryPerObservationSet( dependentVariableSettings, returnFirstCompatibleSettings, parser ); + std::vector< std::map< TimeType, Eigen::VectorXd > > + dependentVariableResultPerObservationSet = + getDependentVariableHistoryPerObservationSet( + dependentVariableSettings, returnFirstCompatibleSettings, parser ); return utilities::concatenateMaps( dependentVariableResultPerObservationSet ); } //! Function returning the list of all dependent variable settings compatible with the settings provided as inputs //! (which might not be fully defined, i.e. with missing link ends information, etc.). The inner vector of the first element of the pair //! contains the list of compatible settings, for each observation sets (outer vector). - std::pair< std::vector< std::vector< std::shared_ptr< ObservationDependentVariableSettings > > >, std::shared_ptr< ObservationCollectionParser > > - getCompatibleDependentVariablesSettingsList( - std::shared_ptr< ObservationDependentVariableSettings > dependentVariableSettings, - std::shared_ptr< ObservationCollectionParser > parser = std::make_shared< ObservationCollectionParser >( ) ) + std::pair< + std::vector< std::vector< std::shared_ptr< ObservationDependentVariableSettings > > >, + std::shared_ptr< ObservationCollectionParser > > + getCompatibleDependentVariablesSettingsList( + std::shared_ptr< ObservationDependentVariableSettings > dependentVariableSettings, + std::shared_ptr< ObservationCollectionParser > parser = + std::make_shared< ObservationCollectionParser >( ) ) { - // Create observation collection parser corresponding to the required dependent variable settings - std::shared_ptr< ObservationCollectionParser > dependentVariablesParser = getObservationParserFromDependentVariableSettings( dependentVariableSettings ); + // Create observation collection parser corresponding to the required dependent variable + // settings + std::shared_ptr< ObservationCollectionParser > dependentVariablesParser = + getObservationParserFromDependentVariableSettings( dependentVariableSettings ); // Define joint observation collection parser - std::shared_ptr< ObservationCollectionMultiTypeParser > jointParser = std::make_shared< ObservationCollectionMultiTypeParser >( - std::vector< std::shared_ptr< ObservationCollectionParser > >( { dependentVariablesParser, parser } ), true ); + std::shared_ptr< ObservationCollectionMultiTypeParser > jointParser = + std::make_shared< ObservationCollectionMultiTypeParser >( + std::vector< std::shared_ptr< ObservationCollectionParser > >( + { dependentVariablesParser, parser } ), + true ); // Retrieve relevant single observation sets - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > singleObservationSets = getSingleObservationSets( jointParser ); + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + singleObservationSets = getSingleObservationSets( jointParser ); // Parse all single observation sets - std::vector< std::vector< std::shared_ptr< ObservationDependentVariableSettings > > > dependentVariablesListPerSet; - for ( auto set : singleObservationSets ) - { - std::vector< std::shared_ptr< ObservationDependentVariableSettings > > currentVariableSettingsList = - set->getCompatibleDependentVariablesSettingsList( dependentVariableSettings ); - if ( currentVariableSettingsList.size( ) > 0 ) + std::vector< std::vector< std::shared_ptr< ObservationDependentVariableSettings > > > + dependentVariablesListPerSet; + for( auto set: singleObservationSets ) + { + std::vector< std::shared_ptr< ObservationDependentVariableSettings > > + currentVariableSettingsList = set->getCompatibleDependentVariablesSettingsList( + dependentVariableSettings ); + if( currentVariableSettingsList.size( ) > 0 ) { dependentVariablesListPerSet.push_back( currentVariableSettingsList ); } @@ -3162,26 +3902,36 @@ class ObservationCollection //! Function returning the values of all dependent variables compatible with the settings provided as input, sorted per observation sets (outer //! vector of the first element of the pair). The order in which the settings are provided in the inner vector matches the settings list output //! of the getCompatibleDependentVariablesSettingsList function - std::pair< std::vector< std::vector< Eigen::MatrixXd > >, std::shared_ptr< ObservationCollectionParser > > getAllCompatibleDependentVariables( + std::pair< std::vector< std::vector< Eigen::MatrixXd > >, + std::shared_ptr< ObservationCollectionParser > > + getAllCompatibleDependentVariables( std::shared_ptr< ObservationDependentVariableSettings > dependentVariableSettings, - std::shared_ptr< ObservationCollectionParser > parser = std::make_shared< ObservationCollectionParser >( ) ) + std::shared_ptr< ObservationCollectionParser > parser = + std::make_shared< ObservationCollectionParser >( ) ) { - // Create observation collection parser corresponding to the required dependent variable settings - std::shared_ptr< ObservationCollectionParser > dependentVariablesParser = getObservationParserFromDependentVariableSettings( dependentVariableSettings ); + // Create observation collection parser corresponding to the required dependent variable + // settings + std::shared_ptr< ObservationCollectionParser > dependentVariablesParser = + getObservationParserFromDependentVariableSettings( dependentVariableSettings ); // Define joint observation collection parser - std::shared_ptr< ObservationCollectionMultiTypeParser > jointParser = std::make_shared< ObservationCollectionMultiTypeParser >( - std::vector< std::shared_ptr< ObservationCollectionParser > >( { dependentVariablesParser, parser } ), true ); + std::shared_ptr< ObservationCollectionMultiTypeParser > jointParser = + std::make_shared< ObservationCollectionMultiTypeParser >( + std::vector< std::shared_ptr< ObservationCollectionParser > >( + { dependentVariablesParser, parser } ), + true ); // Retrieve relevant single observation sets - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > singleObservationSets = getSingleObservationSets( jointParser ); + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + singleObservationSets = getSingleObservationSets( jointParser ); // Parse all single observation sets std::vector< std::vector< Eigen::MatrixXd > > dependentVariablesListPerSet; - for ( auto set : singleObservationSets ) + for( auto set: singleObservationSets ) { - std::vector< Eigen::MatrixXd > currentVariablesList = set->getAllCompatibleDependentVariables( dependentVariableSettings ); - if ( currentVariablesList.size( ) > 0 ) + std::vector< Eigen::MatrixXd > currentVariablesList = + set->getAllCompatibleDependentVariables( dependentVariableSettings ); + if( currentVariablesList.size( ) > 0 ) { dependentVariablesListPerSet.push_back( currentVariablesList ); } @@ -3190,54 +3940,56 @@ class ObservationCollection return std::make_pair( dependentVariablesListPerSet, jointParser ); } - - // Function to retrieve the values of a given dependent variable for (a subset of) the single observation sets, concatenated over all - // relevant single observation sets - std::pair< Eigen::MatrixXd, std::shared_ptr< ObservationCollectionParser > > getConcatenatedDependentVariables( - std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > dependentVariableSettings, + // Function to retrieve the values of a given dependent variable for (a subset of) the single + // observation sets, concatenated over all relevant single observation sets + std::pair< Eigen::MatrixXd, std::shared_ptr< ObservationCollectionParser > > + getConcatenatedDependentVariables( + std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > + dependentVariableSettings, const bool returnFirstCompatibleSettings = false, - const std::shared_ptr< ObservationCollectionParser > parser = std::make_shared< ObservationCollectionParser >( ) ) + const std::shared_ptr< ObservationCollectionParser > parser = + std::make_shared< ObservationCollectionParser >( ) ) { // Retrieve dependent variables stored per single observation sets - std::pair< std::vector< Eigen::MatrixXd >, std::shared_ptr< ObservationCollectionParser > > dependentVariables = - getDependentVariables( dependentVariableSettings, returnFirstCompatibleSettings, parser ); + std::pair< std::vector< Eigen::MatrixXd >, std::shared_ptr< ObservationCollectionParser > > + dependentVariables = getDependentVariables( + dependentVariableSettings, returnFirstCompatibleSettings, parser ); // Compute size concatenated dependent variables matrix unsigned int dependentVariableSize = 0; - if ( dependentVariables.first.size( ) > 0 ) + if( dependentVariables.first.size( ) > 0 ) { dependentVariableSize = dependentVariables.first[ 0 ].cols( ); } unsigned int dependentVariablesRows = 0; - for ( auto it : dependentVariables.first ) + for( auto it: dependentVariables.first ) { - if ( it.cols( ) != dependentVariableSize ) + if( it.cols( ) != dependentVariableSize ) { - throw std::runtime_error( "Error when concatenated dependent variable over obs collection, dependent variable size is inconsistent between single observation sets." ); + throw std::runtime_error( + "Error when concatenated dependent variable over obs collection, dependent " + "variable size is inconsistent between single observation sets." ); } dependentVariablesRows += it.rows( ); } // Concatenate dependent variables over all single observation sets - Eigen::MatrixXd concatenatedDependentVariables = Eigen::MatrixXd::Zero( dependentVariablesRows, dependentVariableSize ); + Eigen::MatrixXd concatenatedDependentVariables = + Eigen::MatrixXd::Zero( dependentVariablesRows, dependentVariableSize ); unsigned int index = 0; - for ( unsigned int k = 0 ; k < dependentVariables.first.size( ) ; k++ ) + for( unsigned int k = 0; k < dependentVariables.first.size( ); k++ ) { - concatenatedDependentVariables.block( index, 0, dependentVariables.first.at( k ).rows( ), dependentVariableSize ) = dependentVariables.first.at( k ); + concatenatedDependentVariables.block( + index, 0, dependentVariables.first.at( k ).rows( ), dependentVariableSize ) = + dependentVariables.first.at( k ); index += dependentVariables.first.at( k ).rows( ); } return std::make_pair( concatenatedDependentVariables, dependentVariables.second ); } - - - - - -private: - + private: void setObservationSetIndices( ) { int currentStartIndex = 0; @@ -3250,28 +4002,31 @@ class ObservationCollection observationTypeAndLinkEndStartAndSize_.clear( ); observationTypeStartAndSize_.clear( ); - for( auto observationIterator : observationSetList_ ) + for( auto observationIterator: observationSetList_ ) { ObservableType currentObservableType = observationIterator.first; currentTypeStartIndex = currentStartIndex; - int observableSize = getObservableSize( currentObservableType ); + int observableSize = getObservableSize( currentObservableType ); int currentObservableTypeSize = 0; - for( auto linkEndIterator : observationIterator.second ) + for( auto linkEndIterator: observationIterator.second ) { LinkEnds currentLinkEnds = linkEndIterator.first; int currentLinkEndStartIndex = currentStartIndex; int currentLinkEndSize = 0; for( unsigned int i = 0; i < linkEndIterator.second.size( ); i++ ) { - int currentNumberOfObservables = linkEndIterator.second.at( i )->getNumberOfObservables( ); + int currentNumberOfObservables = + linkEndIterator.second.at( i )->getNumberOfObservables( ); int currentObservableVectorSize = currentNumberOfObservables * observableSize; - observationSetStartAndSize_[ currentObservableType ][ currentLinkEnds ].push_back( - std::make_pair( currentStartIndex, currentObservableVectorSize ) ); - concatenatedObservationSetStartAndSize_.push_back( std::make_pair( currentStartIndex, currentObservableVectorSize ) ); + observationSetStartAndSize_[ currentObservableType ][ currentLinkEnds ] + .push_back( std::make_pair( currentStartIndex, + currentObservableVectorSize ) ); + concatenatedObservationSetStartAndSize_.push_back( + std::make_pair( currentStartIndex, currentObservableVectorSize ) ); currentStartIndex += currentObservableVectorSize; currentObservableTypeSize += currentObservableVectorSize; currentLinkEndSize += currentObservableVectorSize; @@ -3279,17 +4034,18 @@ class ObservationCollection totalObservableSize_ += currentObservableVectorSize; totalNumberOfObservables_ += currentNumberOfObservables; } - observationTypeAndLinkEndStartAndSize_[ currentObservableType ][ currentLinkEnds ] = std::make_pair( - currentLinkEndStartIndex, currentLinkEndSize ); + observationTypeAndLinkEndStartAndSize_[ currentObservableType ][ currentLinkEnds ] = + std::make_pair( currentLinkEndStartIndex, currentLinkEndSize ); } - observationTypeStartAndSize_[ currentObservableType ] = std::make_pair( - currentTypeStartIndex, currentObservableTypeSize ); + observationTypeStartAndSize_[ currentObservableType ] = + std::make_pair( currentTypeStartIndex, currentObservableTypeSize ); } } void setConcatenatedObservationsAndTimes( ) { - concatenatedObservations_ = Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( totalObservableSize_ ); + concatenatedObservations_ = Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( + totalObservableSize_ ); concatenatedTimes_.resize( totalObservableSize_ ); concatenatedLinkEndIds_.resize( totalObservableSize_ ); concatenatedLinkEndIdNames_.resize( totalObservableSize_ ); @@ -3303,12 +4059,12 @@ class ObservationCollection int maximumStationId = 0; int currentStationId; - for( auto observationIterator : observationSetList_ ) + for( auto observationIterator: observationSetList_ ) { ObservableType currentObservableType = observationIterator.first; int observableSize = getObservableSize( currentObservableType ); - for( auto linkEndIterator : observationIterator.second ) + for( auto linkEndIterator: observationIterator.second ) { LinkEnds currentLinkEnds = linkEndIterator.first; LinkDefinition firstLinkDefinition; @@ -3316,7 +4072,8 @@ class ObservationCollection if( linkEndIterator.second.size( ) > 0 ) { firstLinkDefinition = linkEndIterator.second.at( 0 )->getLinkEnds( ); - linkDefinitionsPerObservable_[ currentObservableType].push_back( firstLinkDefinition ); + linkDefinitionsPerObservable_[ currentObservableType ].push_back( + firstLinkDefinition ); } if( linkEndIds_.count( currentLinkEnds ) == 0 ) @@ -3333,140 +4090,173 @@ class ObservationCollection for( unsigned int i = 0; i < linkEndIterator.second.size( ); i++ ) { - LinkDefinition currentLinkDefinition = linkEndIterator.second.at( i )->getLinkEnds( ); + LinkDefinition currentLinkDefinition = + linkEndIterator.second.at( i )->getLinkEnds( ); if( !( currentLinkDefinition == firstLinkDefinition ) ) { - throw std::runtime_error( "Error when creating ObservationCollection, link definitions of same link ends are not equal " ); + throw std::runtime_error( + "Error when creating ObservationCollection, link definitions of " + "same link ends are not equal " ); } std::pair< int, int > startAndSize = - observationSetStartAndSize_.at( currentObservableType ).at( currentLinkEnds ).at( i ); + observationSetStartAndSize_.at( currentObservableType ) + .at( currentLinkEnds ) + .at( i ); Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > currentObservables = - Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( startAndSize.second ); + Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 >::Zero( + startAndSize.second ); - std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > currentObservationSet = - linkEndIterator.second.at( i )->getObservations( ); + std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > + currentObservationSet = + linkEndIterator.second.at( i )->getObservations( ); std::vector< TimeType > currentObservationTimes = linkEndIterator.second.at( i )->getObservationTimes( ); for( unsigned int j = 0; j < currentObservationSet.size( ); j++ ) { - currentObservables.segment( j * observableSize, observableSize ) = currentObservationSet.at( j ); + currentObservables.segment( j * observableSize, observableSize ) = + currentObservationSet.at( j ); for( int k = 0; k < observableSize; k++ ) { - concatenatedTimes_[ observationCounter ] = currentObservationTimes.at( j ); + concatenatedTimes_[ observationCounter ] = + currentObservationTimes.at( j ); concatenatedLinkEndIds_[ observationCounter ] = currentStationId; concatenatedLinkEndIdNames_[ observationCounter ] = currentLinkEnds; observationCounter++; } } - concatenatedObservations_.segment( startAndSize.first, startAndSize.second ) = currentObservables; + concatenatedObservations_.segment( startAndSize.first, startAndSize.second ) = + currentObservables; } } } - for( auto it1 : observationSetStartAndSize_ ) + for( auto it1: observationSetStartAndSize_ ) { - for( auto it2 : it1.second ) + for( auto it2: it1.second ) { - observationSetStartAndSizePerLinkEndIndex_[ it1.first ][ linkEndIds_[ it2.first ] ] = it2.second; + observationSetStartAndSizePerLinkEndIndex_[ it1.first ] + [ linkEndIds_[ it2.first ] ] = it2.second; } } } - std::shared_ptr< ObservationCollectionParser > getObservationParserFromDependentVariableSettings( - const std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > dependentVariableSettings ) + std::shared_ptr< ObservationCollectionParser > + getObservationParserFromDependentVariableSettings( + const std::shared_ptr< simulation_setup::ObservationDependentVariableSettings > + dependentVariableSettings ) { std::shared_ptr< ObservationCollectionParser > observationParser; - if ( !isObservationDependentVariableAncilliarySetting( dependentVariableSettings->variableType_ ) ) + if( !isObservationDependentVariableAncilliarySetting( + dependentVariableSettings->variableType_ ) ) { std::vector< std::shared_ptr< ObservationCollectionParser > > parserList; // Check if relevant link end id and type are both specified - if ( ( dependentVariableSettings->linkEndId_ != LinkEndId( "", "" ) ) && ( dependentVariableSettings->linkEndType_ != unidentified_link_end ) ) + if( ( dependentVariableSettings->linkEndId_ != LinkEndId( "", "" ) ) && + ( dependentVariableSettings->linkEndType_ != unidentified_link_end ) ) { - parserList.push_back( std::make_shared< ObservationCollectionSingleLinkEndParser >( std::make_pair( dependentVariableSettings->linkEndType_, dependentVariableSettings->linkEndId_ ) ) ); + parserList.push_back( std::make_shared< ObservationCollectionSingleLinkEndParser >( + std::make_pair( dependentVariableSettings->linkEndType_, + dependentVariableSettings->linkEndId_ ) ) ); } - // if only relevant link end id is specified - else if ( dependentVariableSettings->linkEndId_ != LinkEndId( "", "" ) ) + // if only relevant link end id is specified + else if( dependentVariableSettings->linkEndId_ != LinkEndId( "", "" ) ) { - parserList.push_back( std::make_shared< ObservationCollectionLinkEndIdParser >( dependentVariableSettings->linkEndId_ ) ); + parserList.push_back( std::make_shared< ObservationCollectionLinkEndIdParser >( + dependentVariableSettings->linkEndId_ ) ); } - // if only relevant link end type is specified - else if ( dependentVariableSettings->linkEndType_ != unidentified_link_end ) + // if only relevant link end type is specified + else if( dependentVariableSettings->linkEndType_ != unidentified_link_end ) { - parserList.push_back( std::make_shared< ObservationCollectionLinkEndTypeParser >( dependentVariableSettings->linkEndType_ ) ); + parserList.push_back( std::make_shared< ObservationCollectionLinkEndTypeParser >( + dependentVariableSettings->linkEndType_ ) ); } // Check if originating link end id and type are both specified - if ( ( dependentVariableSettings->originatingLinkEndId_ != LinkEndId( "", "" ) ) && ( dependentVariableSettings->originatingLinkEndType_ != unidentified_link_end ) ) + if( ( dependentVariableSettings->originatingLinkEndId_ != LinkEndId( "", "" ) ) && + ( dependentVariableSettings->originatingLinkEndType_ != unidentified_link_end ) ) { - parserList.push_back( std::make_shared< ObservationCollectionSingleLinkEndParser >( std::make_pair( dependentVariableSettings->originatingLinkEndType_, dependentVariableSettings->originatingLinkEndId_ ) ) ); + parserList.push_back( std::make_shared< ObservationCollectionSingleLinkEndParser >( + std::make_pair( dependentVariableSettings->originatingLinkEndType_, + dependentVariableSettings->originatingLinkEndId_ ) ) ); } - // if only originating link end id is specified - else if ( dependentVariableSettings->originatingLinkEndId_ != LinkEndId( "", "" ) ) + // if only originating link end id is specified + else if( dependentVariableSettings->originatingLinkEndId_ != LinkEndId( "", "" ) ) { - parserList.push_back( std::make_shared< ObservationCollectionLinkEndIdParser >( dependentVariableSettings->originatingLinkEndId_ ) ); + parserList.push_back( std::make_shared< ObservationCollectionLinkEndIdParser >( + dependentVariableSettings->originatingLinkEndId_ ) ); } - // if only originating link end type is specified - else if ( dependentVariableSettings->originatingLinkEndType_ != unidentified_link_end ) + // if only originating link end type is specified + else if( dependentVariableSettings->originatingLinkEndType_ != unidentified_link_end ) { - parserList.push_back( std::make_shared< ObservationCollectionLinkEndTypeParser >( dependentVariableSettings->originatingLinkEndType_ ) ); + parserList.push_back( std::make_shared< ObservationCollectionLinkEndTypeParser >( + dependentVariableSettings->originatingLinkEndType_ ) ); } // Create multi-type observation collection parser - if ( parserList.size( ) > 0 ) + if( parserList.size( ) > 0 ) { - observationParser = std::make_shared< ObservationCollectionMultiTypeParser >( parserList, true ); + observationParser = std::make_shared< ObservationCollectionMultiTypeParser >( + parserList, true ); } else { observationParser = std::make_shared< ObservationCollectionParser >( ); } - } else { std::vector< std::shared_ptr< ObservationCollectionParser > > parserList; // Check if indirect condition on observable type via ancillary settings - if ( std::dynamic_pointer_cast< simulation_setup::AncillaryObservationDependentVariableSettings >( dependentVariableSettings ) != nullptr ) + if( std::dynamic_pointer_cast< + simulation_setup::AncillaryObservationDependentVariableSettings >( + dependentVariableSettings ) != nullptr ) { - std::shared_ptr< simulation_setup::AncillaryObservationDependentVariableSettings > ancillaryDependentVariables = - std::dynamic_pointer_cast< simulation_setup::AncillaryObservationDependentVariableSettings >( dependentVariableSettings ); - if ( ancillaryDependentVariables->observableType_ != undefined_observation_model ) + std::shared_ptr< simulation_setup::AncillaryObservationDependentVariableSettings > + ancillaryDependentVariables = std::dynamic_pointer_cast< + simulation_setup::AncillaryObservationDependentVariableSettings >( + dependentVariableSettings ); + if( ancillaryDependentVariables->observableType_ != undefined_observation_model ) { - parserList.push_back( std::make_shared< ObservationCollectionObservableTypeParser >( ancillaryDependentVariables->observableType_ ) ); + parserList.push_back( + std::make_shared< ObservationCollectionObservableTypeParser >( + ancillaryDependentVariables->observableType_ ) ); } else { - std::vector< ObservableType > allObservableTypes = utilities::createVectorFromMapKeys( observationSetList_ ); - for ( auto observableIt : allObservableTypes ) + std::vector< ObservableType > allObservableTypes = + utilities::createVectorFromMapKeys( observationSetList_ ); + for( auto observableIt: allObservableTypes ) { - if ( ancillaryDependentVariables->isObservableTypeCompatible_( observableIt ) ) + if( ancillaryDependentVariables->isObservableTypeCompatible_( + observableIt ) ) { - parserList.push_back( std::make_shared< ObservationCollectionObservableTypeParser >( observableIt ) ); + parserList.push_back( + std::make_shared< ObservationCollectionObservableTypeParser >( + observableIt ) ); } } } } // Create multi-type observation collection parser - if ( parserList.size( ) > 0 ) + if( parserList.size( ) > 0 ) { - observationParser = std::make_shared< ObservationCollectionMultiTypeParser >( parserList, false ); + observationParser = std::make_shared< ObservationCollectionMultiTypeParser >( + parserList, false ); } else { observationParser = std::make_shared< ObservationCollectionParser >( ); } - } return observationParser; } - SortedObservationSets observationSetList_; Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > concatenatedObservations_; @@ -3485,13 +4275,16 @@ class ObservationCollection std::map< int, observation_models::LinkEnds > inverseLinkEndIds_; - std::map< ObservableType, std::map< LinkEnds, std::vector< std::pair< int, int > > > > observationSetStartAndSize_; + std::map< ObservableType, std::map< LinkEnds, std::vector< std::pair< int, int > > > > + observationSetStartAndSize_; std::vector< std::pair< int, int > > concatenatedObservationSetStartAndSize_; - std::map< ObservableType, std::map< int, std::vector< std::pair< int, int > > > > observationSetStartAndSizePerLinkEndIndex_; + std::map< ObservableType, std::map< int, std::vector< std::pair< int, int > > > > + observationSetStartAndSizePerLinkEndIndex_; - std::map< ObservableType, std::map< LinkEnds, std::pair< int, int > > > observationTypeAndLinkEndStartAndSize_; + std::map< ObservableType, std::map< LinkEnds, std::pair< int, int > > > + observationTypeAndLinkEndStartAndSize_; std::map< ObservableType, std::pair< int, int > > observationTypeStartAndSize_; @@ -3500,39 +4293,59 @@ class ObservationCollection int totalNumberOfObservables_; }; - -template< typename ObservationScalarType = double, typename TimeType = double, - typename std::enable_if< is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, int >::type = 0 > -std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > createNewObservationCollection( - const std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > observationCollection, - const std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) +template< typename ObservationScalarType = double, + typename TimeType = double, + typename std::enable_if< + is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, + int >::type = 0 > +std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > +createNewObservationCollection( + const std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > + observationCollection, + const std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) { - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > oldObservationSets = - observationCollection->getSingleObservationSets( observationParser ); - - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > newSingleObservationSets; - for ( auto oldObsSet : oldObservationSets ) - { - std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > newObsSet = std::make_shared< SingleObservationSet< ObservationScalarType, TimeType > >( - oldObsSet->getObservableType( ), oldObsSet->getLinkEnds( ), oldObsSet->getObservations( ), oldObsSet->getObservationTimes( ), oldObsSet->getReferenceLinkEnd( ), - oldObsSet->getObservationsDependentVariablesReference( ), oldObsSet->getDependentVariableCalculator( ), oldObsSet->getAncilliarySettings( ) ); + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + oldObservationSets = + observationCollection->getSingleObservationSets( observationParser ); + + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + newSingleObservationSets; + for( auto oldObsSet: oldObservationSets ) + { + std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > newObsSet = + std::make_shared< SingleObservationSet< ObservationScalarType, TimeType > >( + oldObsSet->getObservableType( ), + oldObsSet->getLinkEnds( ), + oldObsSet->getObservations( ), + oldObsSet->getObservationTimes( ), + oldObsSet->getReferenceLinkEnd( ), + oldObsSet->getObservationsDependentVariablesReference( ), + oldObsSet->getDependentVariableCalculator( ), + oldObsSet->getAncilliarySettings( ) ); newObsSet->setTabulatedWeights( oldObsSet->getWeightsVector( ) ); newObsSet->setResiduals( oldObsSet->getResiduals( ) ); newSingleObservationSets.push_back( newObsSet ); } - return std::make_shared< ObservationCollection< ObservationScalarType, TimeType > >( newSingleObservationSets ); + return std::make_shared< ObservationCollection< ObservationScalarType, TimeType > >( + newSingleObservationSets ); } - -template< typename ObservationScalarType = double, typename TimeType = double, - typename std::enable_if< is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, int >::type = 0 > +template< typename ObservationScalarType = double, + typename TimeType = double, + typename std::enable_if< + is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, + int >::type = 0 > std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > filterObservations( - const std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > observationCollection, - const std::map< std::shared_ptr< ObservationCollectionParser >, std::shared_ptr< ObservationFilterBase > >& observationFilters ) + const std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > + observationCollection, + const std::map< std::shared_ptr< ObservationCollectionParser >, + std::shared_ptr< ObservationFilterBase > >& observationFilters ) { // Create new observation collection - std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > newObservationCollection = createNewObservationCollection( observationCollection ); + std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > + newObservationCollection = createNewObservationCollection( observationCollection ); // Apply filtering to observation collection newObservationCollection->filterObservations( observationFilters, false ); @@ -3540,15 +4353,21 @@ std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > filt return newObservationCollection; } -template< typename ObservationScalarType = double, typename TimeType = double, - typename std::enable_if< is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, int >::type = 0 > +template< typename ObservationScalarType = double, + typename TimeType = double, + typename std::enable_if< + is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, + int >::type = 0 > std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > filterObservations( - const std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > observationCollection, + const std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > + observationCollection, const std::shared_ptr< ObservationFilterBase > observationFilter, - const std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) + const std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) { // Create new observation collection - std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > newObservationCollection = createNewObservationCollection( observationCollection ); + std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > + newObservationCollection = createNewObservationCollection( observationCollection ); // Apply filtering to observation collection newObservationCollection->filterObservations( observationFilter, observationParser, false ); @@ -3556,15 +4375,21 @@ std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > filt return newObservationCollection; } -template< typename ObservationScalarType = double, typename TimeType = double, - typename std::enable_if< is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, int >::type = 0 > +template< typename ObservationScalarType = double, + typename TimeType = double, + typename std::enable_if< + is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, + int >::type = 0 > std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > splitObservationSets( - const std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > observationCollection, + const std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > + observationCollection, const std::shared_ptr< ObservationSetSplitterBase > observationSetSplitter, - const std::shared_ptr< ObservationCollectionParser > observationParser = std::make_shared< ObservationCollectionParser >( ) ) + const std::shared_ptr< ObservationCollectionParser > observationParser = + std::make_shared< ObservationCollectionParser >( ) ) { // Create new observation collection - std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > newObservationCollection = createNewObservationCollection( observationCollection ); + std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > + newObservationCollection = createNewObservationCollection( observationCollection ); // Split observation sets within new observation collection newObservationCollection->splitObservationSets( observationSetSplitter, observationParser ); @@ -3572,68 +4397,101 @@ std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > spli return newObservationCollection; } - -template< typename ObservationScalarType = double, typename TimeType = double, - typename std::enable_if< is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, int >::type = 0 > -inline std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > createSingleObservationSet( +template< typename ObservationScalarType = double, + typename TimeType = double, + typename std::enable_if< + is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, + int >::type = 0 > +inline std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > +createSingleObservationSet( const ObservableType observableType, const LinkEnds& linkEnds, - const std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& observations, + const std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& + observations, const std::vector< TimeType > observationTimes, const LinkEndType referenceLinkEnd, - const std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > ancilliarySettings ) + const std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > + ancilliarySettings ) { return std::make_shared< SingleObservationSet< ObservationScalarType, TimeType > >( - observableType, linkEnds, observations, observationTimes, referenceLinkEnd, - std::vector< Eigen::VectorXd >( ), nullptr, ancilliarySettings ); + observableType, + linkEnds, + observations, + observationTimes, + referenceLinkEnd, + std::vector< Eigen::VectorXd >( ), + nullptr, + ancilliarySettings ); } -//template< typename ObservationScalarType = double, typename TimeType = double, -// typename std::enable_if< is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, int >::type = 0 > -//inline std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > createManualObservationCollection( -// const std::map< ObservableType, std::map< LinkEnds, std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > > >& observationSetList ) +// template< typename ObservationScalarType = double, typename TimeType = double, +// typename std::enable_if< is_state_scalar_and_time_type< ObservationScalarType, TimeType +// >::value, int >::type = 0 > +// inline std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > +// createManualObservationCollection( +// const std::map< ObservableType, std::map< LinkEnds, std::vector< std::shared_ptr< +// SingleObservationSet< ObservationScalarType, TimeType > > > > >& observationSetList ) //{ -// return std::make_shared< ObservationCollection< ObservationScalarType, TimeType > >( observationSetList ); -//} - -//template< typename ObservationScalarType = double, typename TimeType = double, -// typename std::enable_if< is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, int >::type = 0 > -//inline std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > createManualObservationCollection( -// const ObservableType observableType, -// const LinkEnds& linkEnds, -// const std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > >& observationSetList ) +// return std::make_shared< ObservationCollection< ObservationScalarType, TimeType > >( +// observationSetList ); +// } + +// template< typename ObservationScalarType = double, typename TimeType = double, +// typename std::enable_if< is_state_scalar_and_time_type< ObservationScalarType, TimeType +// >::value, int >::type = 0 > +// inline std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > +// createManualObservationCollection( +// const ObservableType observableType, +// const LinkEnds& linkEnds, +// const std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > >& +// observationSetList ) //{ -// return std::make_shared< ObservationCollection< ObservationScalarType, TimeType > >( observationSetList ); -//} +// return std::make_shared< ObservationCollection< ObservationScalarType, TimeType > >( +// observationSetList ); +// } template< typename ObservationScalarType = double, typename TimeType = double > -inline std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > createManualObservationCollection( +inline std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > +createManualObservationCollection( const ObservableType observableType, const LinkDefinition& linkEnds, - const std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& observations, + const std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > >& + observations, const std::vector< TimeType > observationTimes, const LinkEndType referenceLinkEnd, - const std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > ancilliarySettings = nullptr ) + const std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > + ancilliarySettings = nullptr ) { - std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > singleObservationSet = - createSingleObservationSet( observableType, linkEnds.linkEnds_, observations, observationTimes, referenceLinkEnd, - ancilliarySettings ); - - std::map< ObservableType, std::map< LinkEnds, std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > > > observationSetList; + std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > + singleObservationSet = createSingleObservationSet( observableType, + linkEnds.linkEnds_, + observations, + observationTimes, + referenceLinkEnd, + ancilliarySettings ); + + std::map< ObservableType, + std::map< LinkEnds, + std::vector< std::shared_ptr< + SingleObservationSet< ObservationScalarType, TimeType > > > > > + observationSetList; observationSetList[ observableType ][ linkEnds.linkEnds_ ].push_back( singleObservationSet ); - return std::make_shared< ObservationCollection< ObservationScalarType, TimeType > >( observationSetList ); + return std::make_shared< ObservationCollection< ObservationScalarType, TimeType > >( + observationSetList ); } template< typename ObservationScalarType = double, typename TimeType = double > -inline std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > createManualObservationCollection( - std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > singleObservationSets ) +inline std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > +createManualObservationCollection( + std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > + singleObservationSets ) { - return std::make_shared< ObservationCollection< ObservationScalarType, TimeType > >( singleObservationSets ); + return std::make_shared< ObservationCollection< ObservationScalarType, TimeType > >( + singleObservationSets ); } +} // namespace observation_models -} // namespace observation_models - -} // namespace tudat +} // namespace tudat -#endif // TUDAT_OBSERVATIONS_H +#endif // TUDAT_OBSERVATIONS_H diff --git a/include/tudat/simulation/estimation_setup/processOdfFile.h b/include/tudat/simulation/estimation_setup/processOdfFile.h index 66904b4134..a26d26571c 100644 --- a/include/tudat/simulation/estimation_setup/processOdfFile.h +++ b/include/tudat/simulation/estimation_setup/processOdfFile.h @@ -1296,6 +1296,34 @@ observation_models::ObservationAncilliarySimulationSettings createOdfAncillarySe observation_models::sequential_range_lowest_ranging_component, sequentialRangeDataBlock->lowestRangingComponent_.at( dataIndex ) ); + // Define the conversion factor based on the value of the uplink frequency band + FrequencyBands uplinkBand = frequencyBands.at( 0 ); + + double conversionFactor; + if( uplinkBand == 0 ) // S-band + { + conversionFactor = 0.5; + } + else if( uplinkBand == 1 ) // X-band + { + conversionFactor = 221.0 / ( 749.0 * 2.0 ); + } + else if( uplinkBand == 2 ) // Ka-band + { + conversionFactor = 221 / ( 3599 * 2.0 ); + } + else + { + throw std::runtime_error( + "Unsupported uplink frequency band to compute the range conversion factor" ); + } + + ancillarySettings.setAncilliaryDoubleData( + observation_models::range_conversion_factor, + physical_constants::SPEED_OF_LIGHT / + ( sequentialRangeDataBlock->referenceFrequency_.at( dataIndex ) * + conversionFactor ) ); + ancillarySettings.setAncilliaryDoubleVectorData( observation_models::link_ends_delays, std::vector< double >{ @@ -1513,7 +1541,6 @@ createOdfObservedObservationCollection( sortedObservationSets ); } - template< typename ObservationScalarType = double, typename TimeType = double > std::shared_ptr< observation_models::SingleObservationSet< ObservationScalarType, TimeType > > compressDopplerData( @@ -1536,14 +1563,16 @@ compressDopplerData( earth_orientation::TerrestrialTimeScaleConverter timeScaleConverter = earth_orientation::TerrestrialTimeScaleConverter( ); - Eigen::Vector3d stationPosition = simulation_setup::getCombinedApproximateGroundStationPositions( ).at( - originalDopplerData->getLinkEnds( ).at( receiver ).stationName_ ); + Eigen::Vector3d stationPosition = + simulation_setup::getCombinedApproximateGroundStationPositions( ).at( + originalDopplerData->getLinkEnds( ).at( receiver ).stationName_ ); std::vector< TimeType > originalObservationTimesUtc = - timeScaleConverter.getCurrentTimesFromSinglePosition< TimeType >( basic_astrodynamics::tdb_scale, - basic_astrodynamics::utc_scale, - originalObservationTimesTdb, - stationPosition ); + timeScaleConverter.getCurrentTimesFromSinglePosition< TimeType >( + basic_astrodynamics::tdb_scale, + basic_astrodynamics::utc_scale, + originalObservationTimesTdb, + stationPosition ); std::vector< Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > compressedObservations; std::vector< TimeType > compressedObservationTimesUtc; @@ -1766,32 +1795,34 @@ void setOdfInformationInBodies( } template< typename ObservationScalarType = double, typename TimeType = Time > -std::shared_ptr< observation_models::ObservationCollection< ObservationScalarType, TimeType > > createOdfObservedObservationCollectionFromFile( - simulation_setup::SystemOfBodies& bodies, - const std::vector< std::string >& odfFileNames, - const std::string& targetName, - const bool verboseOutput = true, - const std::map< std::string, Eigen::Vector3d >& earthFixedGroundStationPositions = - simulation_setup::getApproximateDsnGroundStationPositions( ) ) +std::shared_ptr< observation_models::ObservationCollection< ObservationScalarType, TimeType > > +createOdfObservedObservationCollectionFromFile( + simulation_setup::SystemOfBodies &bodies, + const std::vector< std::string > &odfFileNames, + const std::string &targetName, + const bool verboseOutput = true, + const std::map< std::string, Eigen::Vector3d > &earthFixedGroundStationPositions = + simulation_setup::getApproximateDsnGroundStationPositions( ) ) { - std::vector< std::shared_ptr< input_output::OdfRawFileContents > > rawOdfDataVector; - for( std::string odfFileName : odfFileNames ) + for( std::string odfFileName: odfFileNames ) { - rawOdfDataVector.push_back( std::make_shared< input_output::OdfRawFileContents>( odfFileName ) ); + rawOdfDataVector.push_back( + std::make_shared< input_output::OdfRawFileContents >( odfFileName ) ); } std::shared_ptr< ProcessedOdfFileContents< TimeType > > processedOdfFileContents = - std::make_shared< ProcessedOdfFileContents< TimeType > >( rawOdfDataVector, targetName, verboseOutput, earthFixedGroundStationPositions ); + std::make_shared< ProcessedOdfFileContents< TimeType > >( + rawOdfDataVector, targetName, verboseOutput, earthFixedGroundStationPositions ); observation_models::setOdfInformationInBodies( processedOdfFileContents, bodies ); // Create observed observation collection - return observation_models::createOdfObservedObservationCollection< ObservationScalarType, TimeType >( - processedOdfFileContents ); + return observation_models::createOdfObservedObservationCollection< ObservationScalarType, + TimeType >( + processedOdfFileContents ); } -} // namespace observation_models - +} // namespace observation_models } // namespace tudat From d4e1db927135d427b3cfa6017a7046362e6bf927 Mon Sep 17 00:00:00 2001 From: Valerio Date: Thu, 12 Dec 2024 13:18:26 +0100 Subject: [PATCH 8/9] Remove unitTestTransmittingFrequencies from the test list --- tests/src/astro/ground_stations/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tests/src/astro/ground_stations/CMakeLists.txt b/tests/src/astro/ground_stations/CMakeLists.txt index 5bb4f2df63..62c0350a49 100644 --- a/tests/src/astro/ground_stations/CMakeLists.txt +++ b/tests/src/astro/ground_stations/CMakeLists.txt @@ -31,6 +31,6 @@ TUDAT_ADD_TEST_CASE(PoleTideDisplacement ${Tudat_PROPAGATION_LIBRARIES} ) -TUDAT_ADD_TEST_CASE(TransmittingFrequencies - PRIVATE_LINKS - ${Tudat_ESTIMATION_LIBRARIES}) \ No newline at end of file +# TUDAT_ADD_TEST_CASE(TransmittingFrequencies +# PRIVATE_LINKS +# ${Tudat_ESTIMATION_LIBRARIES}) \ No newline at end of file From 7cd102bbaf96642268efba4db120ce83913a89b3 Mon Sep 17 00:00:00 2001 From: Valerio Date: Thu, 12 Dec 2024 13:18:57 +0100 Subject: [PATCH 9/9] Add comments to the CSPICE error handling functions --- .../tudat/interface/spice/spiceInterface.h | 127 +++++++++++++++++- 1 file changed, 126 insertions(+), 1 deletion(-) diff --git a/include/tudat/interface/spice/spiceInterface.h b/include/tudat/interface/spice/spiceInterface.h index 83d45fca34..47c0057652 100644 --- a/include/tudat/interface/spice/spiceInterface.h +++ b/include/tudat/interface/spice/spiceInterface.h @@ -135,15 +135,140 @@ std::vector getStandardSpiceKernels(const std::vector //! @get_docstring(load_standard_kernels) void loadStandardSpiceKernels(const std::vector alternativeEphemerisKernels = std::vector()); - +/** + * @brief Sets the default error handling action of the CSPICE toolkit to "RETURN". + * + * This function uses the CSPICE function `erract_c` to set the error handling mode + * to "RETURN". In this mode, CSPICE routines do not abort when an error is detected. + * Instead, they output error messages (if enabled) and immediately return upon entry. + * + * This mode allows for safe handling of errors in programs where continued execution + * without program termination is desired. It is especially useful when multiple + * CSPICE routines are called sequentially, as it prevents unexpected crashes and + * ensures error messages are retained for later retrieval. + * + * @note The CSPICE toolkit provides several error handling modes. By using this + * function, the error handling is explicitly set to "RETURN". Ensure this behavior + * aligns with the error handling strategy of your program. + * + * Example Usage: + * @code + * toggleErrorReturn(); + * // Call CSPICE routines here. Errors will cause immediate returns instead of aborting. + * @endcode + * + * @see erract_c + */ void toggleErrorReturn( ); +/** + * @brief Sets the CSPICE error output device to "ABORT". + * + * This function uses the CSPICE function `errdev_c` to configure the error output device + * such that errors cause the program to terminate execution immediately. When set to + * "ABORT", CSPICE routines will output error messages (if enabled) and then stop execution. + * + * This setting is useful for non-interactive programs or applications where errors should + * result in immediate termination to avoid undefined behavior or further erroneous operations. + * + * @note The CSPICE toolkit provides multiple error output options. By using this function, + * the error output behavior is explicitly set to "ABORT". + * + * Example Usage: + * @code + * toggleErrorAbort(); + * // CSPICE routines will now terminate the program if errors occur. + * @endcode + * + * @see errdev_c + */ void toggleErrorAbort( ); +/** + * @brief Suppresses all CSPICE error output. + * + * This function uses the CSPICE function `errdev_c` to configure the error output device + * to "NULL". When set to "NULL", no error messages will be output, effectively silencing + * all CSPICE error reporting. + * + * This setting is useful in scenarios where error messages are not desired, such as when + * testing or debugging, or when errors are handled programmatically without needing + * console or file outputs. + * + * @warning Suppressing error output can make debugging difficult, as errors will not be + * displayed. Use this setting with caution. + * + * Example Usage: + * @code + * suppressErrorOutput(); + * // CSPICE routines will no longer output error messages. + * @endcode + * + * @see errdev_c + */ void suppressErrorOutput( ); +/** + * @brief Retrieves the current CSPICE long error message if an error condition exists. + * + * This function checks for an active error condition using the CSPICE function `failed_c`. + * If an error is detected (i.e., `failed_c` returns `SPICETRUE`), it retrieves the detailed + * long error message using `getmsg_c` and returns it as a standard C++ string. If no error + * condition exists (`failed_c` returns `SPICEFALSE`), the function returns an empty string. + * + * The long error message provides a detailed explanation of the error, including potentially + * specific data about the error's occurrence. This function is useful for debugging and + * understanding error causes in CSPICE-based programs. + * + * @return A string containing the CSPICE long error message if an error exists, or an empty + * string if no error condition is present. + * + * Example Usage: + * @code + * std::string errorMessage = getErrorMessage(); + * if (!errorMessage.empty()) { + * std::cerr << "Error: " << errorMessage << std::endl; + * } + * @endcode + * + * @note This function does not alter the CSPICE error state. If error handling needs to + * continue, ensure that the CSPICE environment is appropriately managed after retrieving + * the error message. + * + * @see failed_c + * @see getmsg_c + */ std::string getErrorMessage( ); +/** + * @brief Checks for a CSPICE error condition and resets the error status if one exists. + * + * This function uses `failed_c` to check whether a CSPICE error condition has been signaled + * (i.e., an error was detected). If an error condition exists (`failed_c` returns `SPICETRUE`), + * the function resets the error status using `reset_c` and returns `true`. If no error condition + * exists (`failed_c` returns `SPICEFALSE`), the function returns `false`. + * + * Resetting the error status clears the error messages and re-enables the setting of long + * error messages, allowing CSPICE routines to continue normal execution. This function is + * particularly useful in applications that use the "RETURN" error action mode, where CSPICE + * routines return immediately upon entry if an error condition exists. + * + * @return `true` if an error condition exists and has been reset, otherwise `false`. + * + * Example Usage: + * @code + * if (checkFailure()) { + * std::cerr << "An error occurred and was reset." << std::endl; + * } + * @endcode + * + * @note Use this function to handle and reset CSPICE error conditions in cases where + * continued program execution is desired. Ensure that appropriate diagnostic + * actions are taken before calling this function if required. + * + * @see failed_c + * @see reset_c + */ bool checkFailure( );