Feature/clock noise models

CMakeLists.txt

@@ -74,6 +74,8 @@ option(TUDAT_BUILD_WITH_SOFA_INTERFACE "Build Tudat with sofa interface." ON)
 # Build json interface.
 option(TUDAT_BUILD_WITH_JSON_INTERFACE "Build Tudat with json interface." OFF)
 
+option(TUDAT_BUILD_WITH_FFTW3 "Build Tudat with FFTW3." OFF)
+
 # Build pagmo-dependent code
 option(TUDAT_BUILD_WITH_PAGMO "Build Tudat with pagmo." OFF)
 if(CMAKE_CXX_SIMULATE_ID MATCHES "MSVC")
@@ -116,12 +118,14 @@ message(STATUS "TUDAT_BUILD_TUDAT_TUTORIALS                           ${TUDAT_BU
 message(STATUS "TUDAT_BUILD_STATIC_LIBRARY                            ${TUDAT_BUILD_STATIC_LIBRARY}")
 message(STATUS "TUDAT_BUILD_WITH_FILTERS                              ${TUDAT_BUILD_WITH_FILTERS}")
 message(STATUS "TUDAT_BUILD_WITH_SOFA_INTERFACE                       ${TUDAT_BUILD_WITH_SOFA_INTERFACE}")
+message(STATUS "TUDAT_BUILD_WITH_FFTW3                                ${TUDAT_BUILD_WITH_FFTW3}")
 message(STATUS "TUDAT_BUILD_WITH_JSON_INTERFACE                       ${TUDAT_BUILD_WITH_JSON_INTERFACE}")
 message(STATUS "TUDAT_BUILD_WITH_EXTENDED_PRECISION_PROPAGATION_TOOLS ${TUDAT_BUILD_WITH_EXTENDED_PRECISION_PROPAGATION_TOOLS}")
 message(STATUS "TUDAT_DOWNLOAD_AND_BUILD_BOOST                        ${TUDAT_DOWNLOAD_AND_BUILD_BOOST}")
 
 set(Tudat_DEFINITIONS "${Tudat_DEFINITIONS} -DTUDAT_BUILD_WITH_FILTERS=${TUDAT_BUILD_WITH_FILTERS}")
 set(Tudat_DEFINITIONS "${Tudat_DEFINITIONS} -DTUDAT_BUILD_WITH_SOFA_INTERFACE=${TUDAT_BUILD_WITH_SOFA_INTERFACE}")
+set(Tudat_DEFINITIONS "${Tudat_DEFINITIONS} -DTUDAT_BUILD_WITH_FFTW3=${TUDAT_BUILD_WITH_FFTW3}")
 set(Tudat_DEFINITIONS "${Tudat_DEFINITIONS} -DTUDAT_BUILD_WITH_JSON_INTERFACE=${TUDAT_BUILD_WITH_JSON_INTERFACE}")
 set(Tudat_DEFINITIONS "${Tudat_DEFINITIONS} -DTUDAT_BUILD_WITH_EXTENDED_PRECISION_PROPAGATION_TOOLS=${TUDAT_BUILD_WITH_EXTENDED_PRECISION_PROPAGATION_TOOLS}")
 # +============================================================================
@@ -326,6 +330,28 @@ find_package(nrlmsise00 REQUIRED 0.1)
 message(STATUS ${NRLMSISE00_LIBRARIES})
 add_definitions(-DTUDAT_BUILD_WITH_NRLMSISE=1)
 
+if (TUDAT_BUILD_WITH_FFTW3)
+    if(NOT APPLE)
+        find_package(FFTW3)
+        message(STATUS "FFTW " ${FFTW3_LIBRARIES})
+        include_directories(SYSTEM AFTER "${FFTW3_INCLUDE_DIRS}")
+        if(WIN32)
+            set (FFTW3_LIBRARIES "${FFTW3_LIBRARY_DIRS}/fftw3.lib")
+        else()
+            set (FFTW3_LIBRARIES "${FFTW3_LIBRARY_DIRS}/libfftw3.so")
+        endif()
+        add_definitions(-DTUDAT_BUILD_WITH_FFTW3=1)
+    else( )
+        find_package(FFTW3)
+        message(STATUS ${FFTW3_LIBRARIES})
+        include_directories(SYSTEM AFTER "${FFTW3_INCLUDE_DIRS}")
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -isystem \"${FFTW3_INCLUDE_DIR}\"")
+        set (FFTW3_LIBRARIES "${FFTW3_LIBRARY_DIRS}/libfftw3.dylib")
+        add_definitions(-DTUDAT_BUILD_WITH_FFTW3=1)
+    endif( )
+else( )
+    add_definitions(-DTUDAT_BUILD_WITH_FFTW3=0)
+endif( )
 
 # JSON
 if (TUDAT_BUILD_WITH_JSON_INTERFACE)

cmake_modules/TudatLinkLibraries.cmake

@@ -25,6 +25,9 @@ endif ()
 
 list(APPEND TUDAT_EXTERNAL_INTERFACE_LIBRARIES ${NRLMSISE00_LIBRARIES})
 
+if(TUDAT_BUILD_WITH_FFTW3)
+    list(APPEND TUDAT_EXTERNAL_INTERFACE_LIBRARIES ${FFTW3_LIBRARIES})
+endif( )
 
 if (TUDAT_BUILD_WITH_JSON_INTERFACE)
     list(APPEND TUDAT_EXTERNAL_LIBRARIES ${nlohmann_json_LIBRARIES})
@@ -67,15 +70,15 @@ list(APPEND Tudat_PROPAGATION_LIBRARIES
         Tudat::tudat_statistics
         Tudat::tudat_propagators
         ${TUDAT_EXTERNAL_INTERFACE_LIBRARIES}
-        Tudat::tudat_basic_astrodynamics        
+        Tudat::tudat_basic_astrodynamics
         Tudat::tudat_numerical_quadrature
         Tudat::tudat_interpolators
         Tudat::tudat_root_finders
         Tudat::tudat_basic_mathematics
         Tudat::tudat_input_output
         Tudat::tudat_basics
         Tudat::tudat_data
-#        ${TUDAT_EXTERNAL_LIBRARIES}
+        #        ${TUDAT_EXTERNAL_LIBRARIES}
         )
 
 if (TUDAT_BUILD_WITH_ESTIMATION_TOOLS)

environment.yml

@@ -8,4 +8,5 @@ dependencies:
   - tudat-resources
   - sofa-cmake
   - nrlmsise-00
-  - cspice-cmake
+  - cspice-cmake
+  - fftw

include/tudat/astro/ground_stations/groundStation.h

@@ -18,6 +18,7 @@
 
 #include "tudat/astro/ground_stations/groundStationState.h"
 #include "tudat/astro/ground_stations/pointingAnglesCalculator.h"
+#include "tudat/astro/system_models/timingSystem.h"
 #include "tudat/astro/ground_stations/transmittingFrequencies.h"
 #include "tudat/astro/system_models/vehicleSystems.h"
 
@@ -96,6 +97,16 @@ class GroundStation
         return pointingAnglesCalculator_;
     }
 
+    std::shared_ptr< system_models::TimingSystem > getTimingSystem( )
+    {
+        return timingSystem_;
+    }
+
+    void setTimingSystem( const std::shared_ptr< system_models::TimingSystem > timingSystem )
+    {
+        timingSystem_ = timingSystem;
+    }
+
     //! Function to return the object used to compute the ground station's transmitting frequency at a given time
     std::shared_ptr< StationFrequencyInterpolator > getTransmittingFrequencyCalculator( )
     {
@@ -218,6 +229,8 @@ class GroundStation
     //! Object used to computed pointing angles (elevation, azimuth) to a given target from this ground station.
     std::shared_ptr< PointingAnglesCalculator > pointingAnglesCalculator_;
 
+    std::shared_ptr< system_models::TimingSystem > timingSystem_;
+
     //! Name of the ground station
     std::string stationId_;
 

include/tudat/astro/observation_models/observableTypes.h

@@ -158,7 +158,7 @@ std::vector< int > getLinkEndIndicesForLinkEndTypeAtObservable(
 //! Function to retrieve the link end indices in link end states/times that are to be used in viability calculation
 /*!
  * Function to retrieve the link end indices in link end states/times that are to be used in viability calculation.
- * Return variable is a vector of pairs, where each the first entry denotes the index of the point at which the link is to be
+ * Return variable is a vector of pairs, where eacgetLinkEndTypesForGivenLinkEndIdh the first entry denotes the index of the point at which the link is to be
  * checkd. The second entry denotes the index for the opposite end of the link.
  * \param linkEnds Complete set of link ends for which check is to be performed
  * \param observableType Observable type for which check is to be performed
@@ -174,6 +174,8 @@ std::vector< LinkEndType > getLinkEndTypesForGivenLinkEndId(
         const LinkEnds& linkEnds,
         const LinkEndId linkEndToCheck );
 
+std::vector< int > getLinkEndIndicesForLinkEndIdAtObservable(
+        const ObservableType observableType, const LinkEnds& linkEnds, const LinkEndId linkEndToCheck );
 
 static const std::map< LinkEndType, int > oneWayLinkStateEntries = {
     { transmitter, 0 },

include/tudat/astro/observation_models/observationBias.h

@@ -22,10 +22,12 @@
 
 #include "tudat/astro/basic_astro/physicalConstants.h"
 #include "tudat/basics/basicTypedefs.h"
+#include "tudat/astro/basic_astro/physicalConstants.h"
 #include "tudat/astro/earth_orientation/terrestrialTimeScaleConverter.h"
 #include "tudat/astro/ground_stations/groundStationState.h"
 #include "tudat/astro/observation_models/linkTypeDefs.h"
 #include "tudat/astro/observation_models/observableTypes.h"
+#include "tudat/astro/system_models/timingSystem.h"
 #include "tudat/math/interpolators/lookupScheme.h"
 
 
@@ -47,6 +49,7 @@ enum ObservationBiasTypes
     arc_wise_time_drift_bias,
     constant_time_bias,
     arc_wise_time_bias,
+    clock_induced_bias,
     two_way_range_time_scale_bias
 };
 
@@ -205,7 +208,6 @@ class ConstantObservationBias: public ObservationBias< ObservationSize >
 
     //! Constant (entry-wise) observation bias.
     Eigen::Matrix< double, ObservationSize, 1 > observationBias_;
-
 };
 
 //! Class for an arc-wise constant absolute observation bias of a given size
@@ -1217,6 +1219,70 @@ class ArcWiseTimeBias: public ObservationBias< ObservationSize >
 };
 
 
+template< int ObservationSize = 1 >
+class ClockInducedRangeBias: public ObservationBias< ObservationSize >
+{
+public:
+
+    ClockInducedRangeBias(
+        const std::shared_ptr<system_models::TimingSystem> timingSystem,
+        const std::vector<int> linkEndIndicesForTime,
+        const observation_models::LinkEndId linkEndId ) :
+        timingSystem_( timingSystem ), linkEndIndicesForTime_( linkEndIndicesForTime ),
+        linkEndId_( linkEndId ), onesVector_( Eigen::Matrix<double, ObservationSize, 1>::Ones( ))
+    {
+        for ( unsigned int i = 0; i < linkEndIndicesForTime_.size( ); i++ )
+        {
+            signMultipliers_.push_back(( linkEndIndicesForTime_.at( i ) % 2 == 1 ? 1.0 : -1.0 ));
+        }
+    }
+
+    //! Destructor
+    ~ClockInducedRangeBias( )
+    {
+    }
+
+    Eigen::Matrix<double, ObservationSize, 1> getObservationBias(
+        const std::vector<double> &linkEndTimes,
+        const std::vector<Eigen::Matrix<double, 6, 1> > &linkEndStates,
+        const Eigen::Matrix<double, ObservationSize, 1> &currentObservableValue =
+        ( Eigen::Matrix<double, ObservationSize, 1>( ) << TUDAT_NAN ).finished( ))
+    {
+        double currentBias = 0.0;
+        for ( unsigned int i = 0; i < linkEndIndicesForTime_.size( ); i++ )
+        {
+            currentBias += signMultipliers_.at( i ) * timingSystem_->getCompleteClockError(
+                linkEndTimes.at( linkEndIndicesForTime_.at( i )));
+        }
+        return currentBias * onesVector_ * physical_constants::SPEED_OF_LIGHT;
+    }
+
+    std::shared_ptr<system_models::TimingSystem> getTimingSystem( )
+    {
+        return timingSystem_;
+    }
+
+    LinkEndId getLinkEndId( )
+    {
+        return linkEndId_;
+
+    }
+
+private:
+
+    std::shared_ptr<system_models::TimingSystem> timingSystem_;
+
+    //! Link end index from which the 'current time' is determined (e.g. entry from linkEndTimes used in getObservationBias
+    //! function.
+    std::vector<int> linkEndIndicesForTime_;
+
+    std::vector<double> signMultipliers_;
+
+    LinkEndId linkEndId_;
+
+    Eigen::Matrix<double, ObservationSize, 1> onesVector_;
+};
+
 //! Class for a constant time observation bias of a given size
 /*!
  *  Class for a constant time observation bias of a given size. For unbiases observation h and time bias c, the biased observation
@@ -1331,6 +1397,15 @@ ObservationBiasTypes getObservationBiasType(
     {
         biasType = arc_wise_time_bias;
     }
+    else if( std::dynamic_pointer_cast< ClockInducedRangeBias< ObservationSize > >( biasObject ) != nullptr )
+    {
+        biasType = clock_induced_bias;
+    }
+    else if( biasObject == nullptr )
+    {
+        std::string errorMessage = "Error, found nullptr when retrieving bias type";
+        throw std::runtime_error( errorMessage );
+    }
     else
     {
         std::string errorMessage = "Error, did not recognize observation bias when retrieving bias type";
@@ -1339,6 +1414,34 @@ ObservationBiasTypes getObservationBiasType(
     return biasType;
 }
 
+template< int ObservationSize = 1 >
+std::vector< std::shared_ptr< ObservationBias< ObservationSize > > > getClockInducedBiases(
+        const std::shared_ptr< ObservationBias< ObservationSize > > fullBias )
+{
+    std::vector< std::shared_ptr< ObservationBias< ObservationSize > > > clockInducedBiases;
+    if( fullBias != nullptr )
+    {
+        if( getObservationBiasType( fullBias ) == clock_induced_bias )
+        {
+            clockInducedBiases.push_back( fullBias );
+        }
+        else if( getObservationBiasType( fullBias ) == multiple_observation_biases )
+        {
+            std::shared_ptr< MultiTypeObservationBias< ObservationSize > > combinedBias =
+                    std::dynamic_pointer_cast< MultiTypeObservationBias< ObservationSize > >( fullBias );
+            for( unsigned int i = 0; i < combinedBias->getBiasList( ).size( ); i++ )
+            {
+                if( getObservationBiasType( combinedBias->getBiasList( ).at( i ) ) == clock_induced_bias )
+                {
+                    clockInducedBiases.push_back( combinedBias->getBiasList( ).at( i ) );
+                }
+            }
+        }
+    }
+    return clockInducedBiases;
+}
+
+
 } // namespace observation_models
 
 } // namespace tudat

include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h

@@ -74,6 +74,8 @@ enum EstimatebleParametersEnum
     arc_wise_time_drift_observation_bias,
     constant_time_observation_bias,
     arc_wise_time_observation_bias,
+    global_polynomial_clock_corrections,
+    arc_wise_polynomial_clock_corrections,
     inverse_tidal_quality_factor,
     yarkovsky_parameter,
     custom_estimated_parameter,
@@ -147,6 +149,8 @@ bool isParameterNonTidalGravityFieldVariationProperty( const EstimatebleParamete
  */
 bool isParameterArcWiseInitialStateProperty( const EstimatebleParametersEnum parameterType );
 
+bool isParameterClockProperty( const EstimatebleParametersEnum parameterType );
+
 //! Typedef for full parameter identifier.
 typedef std::pair< EstimatebleParametersEnum, std::pair< std::string, std::string > > EstimatebleParameterIdentifier;