Merge pull request #270 from tudat-team/feature/doppler_extension

Feature/doppler extension

include/tudat/astro/electromagnetism/radiationSourceModel.h

@@ -26,12 +26,10 @@
 #include <utility>
 #include <vector>
 #include <map>
-#include <utility>
 #include <memory>
 #include <set>
 
 #include <Eigen/Core>
-#include <Eigen/Geometry>
 
 #include "tudat/astro/electromagnetism/luminosityModel.h"
 #include "tudat/astro/electromagnetism/sourcePanelRadiosityModel.h"

include/tudat/astro/observation_models/lightTimeSolution.h

@@ -633,23 +633,33 @@ class LightTimeCalculator
      *  \param receiverTime Time at reiver.
      *  \param isPartialWrtReceiver If partial is to be calculated w.r.t. receiver or transmitter.
      */
-    Eigen::Matrix< ObservationScalarType, 1, 3 > getPartialOfLightTimeWrtLinkEndPosition(
-            const StateType& transmitterState,
-            const StateType& receiverState,
-            const TimeType transmitterTime,
-            const TimeType receiverTime,
+    Eigen::Matrix< ObservationScalarType, 1, 3 >  getPartialOfLightTimeWrtLinkEndPosition(
+            const Eigen::Vector6d& transmitterState,
+            const Eigen::Vector6d& receiverState,
+            const double transmitterTime,
+            const double receiverTime,
             const bool isPartialWrtReceiver,
             const std::shared_ptr< observation_models::ObservationAncilliarySimulationSettings > ancillarySettings = nullptr )
     {
-        setTotalLightTimeCorrection( transmitterState, receiverState, transmitterTime, receiverTime, ancillarySettings );
+        setTotalLightTimeCorrection(
+            transmitterState.template cast< ObservationScalarType >( ),
+                receiverState.template cast< ObservationScalarType >( ), transmitterTime, receiverTime, ancillarySettings );
 
         Eigen::Matrix< ObservationScalarType, 3, 1 > relativePosition =
-                receiverState.segment( 0, 3 ) - transmitterState.segment( 0, 3 );
-        return ( relativePosition.normalized( ) ).transpose( ) *
-//                ( mathematical_constants::getFloatingInteger< ObservationScalarType >( 1 ) +
-//                  currentCorrection_ / relativePosition.norm( ) ) *
-                ( isPartialWrtReceiver ? mathematical_constants::getFloatingInteger< ObservationScalarType >( 1 ) :
+            ( receiverState.segment( 0, 3 ) - transmitterState.segment( 0, 3 ) ).template cast< ObservationScalarType >( );
+        Eigen::Matrix< ObservationScalarType, 1, 3 > partialWrtLinkEndPosition =
+            ( relativePosition.normalized( ) ).transpose( ) * ( isPartialWrtReceiver ? mathematical_constants::getFloatingInteger< ObservationScalarType >( 1 ) :
                                          mathematical_constants::getFloatingInteger< ObservationScalarType >( -1 ) );
+
+        for( unsigned int i = 0; i < correctionFunctions_.size( ); i++ )
+        {
+            partialWrtLinkEndPosition +=
+                  correctionFunctions_.at( i )->calculateLightTimeCorrectionPartialDerivativeWrtLinkEndPosition(
+                transmitterState.template cast< double >( ),
+                    receiverState.template cast< double >( ),
+                        transmitterTime, receiverTime, isPartialWrtReceiver ? receiver : transmitter ).template cast< ObservationScalarType >( ) * physical_constants::SPEED_OF_LIGHT;
+        }
+        return partialWrtLinkEndPosition;
     }
 
     //! Function to get list of light-time correction functions
@@ -1022,6 +1032,7 @@ class MultiLegLightTimeCalculator
             const std::shared_ptr< ObservationAncilliarySimulationSettings > ancillarySettings = nullptr,
             bool computeLightTimeCorrections = true )
     {
+
         // Define objects to keep light times
         ObservationScalarType totalLightTime = 0.0;
         ObservationScalarType currentLightTime;

include/tudat/astro/observation_models/twoWayDopplerObservationModel.h

@@ -46,17 +46,19 @@ class TwoWayDopplerObservationModel: public ObservationModel< 1, ObservationScal
      */
     TwoWayDopplerObservationModel(
             const LinkEnds& linkEnds,
+            const std::shared_ptr< observation_models::MultiLegLightTimeCalculator< ObservationScalarType, TimeType > >
+            multiLegLightTimeCalculator,
             const std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > >
             uplinkDopplerCalculator,
             const std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > >
             downlinkDopplerCalculator,
             const std::shared_ptr< ObservationBias< 1 > > observationBiasCalculator = nullptr,
             const bool normalizeWithSpeedOfLight = false ):
         ObservationModel< 1, ObservationScalarType, TimeType >( two_way_doppler, linkEnds, observationBiasCalculator ),
-       uplinkDopplerCalculator_( uplinkDopplerCalculator ),
-       downlinkDopplerCalculator_( downlinkDopplerCalculator )
+        multiLegLightTimeCalculator_( multiLegLightTimeCalculator ),
+        uplinkDopplerCalculator_( uplinkDopplerCalculator ),
+        downlinkDopplerCalculator_( downlinkDopplerCalculator )
     {
-//        std::cout<<"Normalize: "<<normalizeWithSpeedOfLight<<std::endl;
         setNormalizeWithSpeedOfLight( normalizeWithSpeedOfLight );
         uplinkDopplerCalculator_->setNormalizeWithSpeedOfLight( true );
         downlinkDopplerCalculator_->setNormalizeWithSpeedOfLight( true );
@@ -87,58 +89,44 @@ class TwoWayDopplerObservationModel: public ObservationModel< 1, ObservationScal
             std::vector< Eigen::Matrix< double, 6, 1 > >& linkEndStates,
             const std::shared_ptr< ObservationAncilliarySimulationSettings > ancilliarySetings = nullptr )
     {
-        std::vector< double > uplinkLinkEndTimes;
-        std::vector< Eigen::Matrix< double, 6, 1 > > uplinkLinkEndStates;
+        linkEndTimes.clear( );
+        linkEndStates.clear( );
+        multiLegLightTimeCalculator_->calculateLightTimeWithLinkEndsStates(
+            time, linkEndAssociatedWithTime, linkEndTimes, linkEndStates, ancilliarySetings );
 
-        std::vector< double > downlinkLinkEndTimes;
-        std::vector< Eigen::Matrix< double, 6, 1 > > downlinkLinkEndStates;
+        std::vector< double > uplinkLinkEndTimes = { linkEndTimes.at( 0 ), linkEndTimes.at( 1 ) };
+        std::vector< Eigen::Matrix< double, 6, 1 > > uplinkLinkEndStates = { linkEndStates.at( 0 ), linkEndStates.at( 1 ) };
+
+        std::vector< double > downlinkLinkEndTimes = { linkEndTimes.at( 2 ), linkEndTimes.at( 3 ) };
+        std::vector< Eigen::Matrix< double, 6, 1 > > downlinkLinkEndStates = { linkEndStates.at( 2 ), linkEndStates.at( 3 ) };
 
         Eigen::Matrix< ObservationScalarType, 1, 1 > uplinkDoppler, downlinkDoppler;
 
+        LinkEndType uplinkReferenceLinkEnd, downlinkReferenceLinkEnd;
         switch( linkEndAssociatedWithTime )
         {
         case receiver:
-
-            downlinkDoppler = downlinkDopplerCalculator_->computeIdealObservationsWithLinkEndData(
-                        time, receiver, downlinkLinkEndTimes, downlinkLinkEndStates );
-            uplinkDoppler = uplinkDopplerCalculator_->computeIdealObservationsWithLinkEndData(
-                        downlinkLinkEndTimes.at( 0 ), receiver, uplinkLinkEndTimes, uplinkLinkEndStates );
-
+            uplinkReferenceLinkEnd = receiver;
+            downlinkReferenceLinkEnd = receiver;
             break;
         case reflector1:
-
-            uplinkDoppler = uplinkDopplerCalculator_->computeIdealObservationsWithLinkEndData(
-                        time, receiver, uplinkLinkEndTimes, uplinkLinkEndStates );
-            downlinkDoppler = downlinkDopplerCalculator_->computeIdealObservationsWithLinkEndData(
-                        time, transmitter, downlinkLinkEndTimes, downlinkLinkEndStates );
-
+            uplinkReferenceLinkEnd = receiver;
+            downlinkReferenceLinkEnd = transmitter;
             break;
         case transmitter:
-            uplinkDoppler = uplinkDopplerCalculator_->computeIdealObservationsWithLinkEndData(
-                        time, transmitter, uplinkLinkEndTimes, uplinkLinkEndStates );
-            downlinkDoppler = downlinkDopplerCalculator_->computeIdealObservationsWithLinkEndData(
-                        uplinkLinkEndTimes.at( 1 ), transmitter, downlinkLinkEndTimes, downlinkLinkEndStates );
+            uplinkReferenceLinkEnd = transmitter;
+            downlinkReferenceLinkEnd = transmitter;
             break;
         default:
             throw std::runtime_error(
                         "Error when calculating two way Doppler observation, link end is not transmitter or receiver" );
         }
 
-        linkEndTimes.clear( );
-        linkEndStates.clear( );
-
-        linkEndTimes.resize( 4 );
-        linkEndStates.resize( 4 );
-
-        linkEndTimes[ 0 ] = uplinkLinkEndTimes.at( 0 );
-        linkEndTimes[ 1 ] = uplinkLinkEndTimes.at( 1 );
-        linkEndTimes[ 2 ] = downlinkLinkEndTimes.at( 0 );
-        linkEndTimes[ 3 ] = downlinkLinkEndTimes.at( 1 );
+        uplinkDoppler = uplinkDopplerCalculator_->computeIdealDopplerWithLinkEndData(
+            uplinkReferenceLinkEnd, uplinkLinkEndTimes, uplinkLinkEndStates );
+        downlinkDoppler = downlinkDopplerCalculator_->computeIdealDopplerWithLinkEndData(
+            downlinkReferenceLinkEnd, downlinkLinkEndTimes, downlinkLinkEndStates );
 
-        linkEndStates[ 0 ] = uplinkLinkEndStates.at( 0 );
-        linkEndStates[ 1 ] = uplinkLinkEndStates.at( 1 );
-        linkEndStates[ 2 ] = downlinkLinkEndStates.at( 0 );
-        linkEndStates[ 3 ] = downlinkLinkEndStates.at( 1 );
 
         return multiplicationTerm_ * ( Eigen::Matrix< ObservationScalarType, 1, 1 >( ) << downlinkDoppler( 0 ) * uplinkDoppler( 0 ) +
                  downlinkDoppler( 0 ) + uplinkDoppler( 0 ) ).finished( );
@@ -185,13 +173,13 @@ class TwoWayDopplerObservationModel: public ObservationModel< 1, ObservationScal
 
 private:
 
+    std::shared_ptr< observation_models::MultiLegLightTimeCalculator< ObservationScalarType, TimeType > > multiLegLightTimeCalculator_;
+
     //! Object that computes the one-way Doppler observable for the uplink
-    std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > >
-    uplinkDopplerCalculator_;
+    std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > uplinkDopplerCalculator_;
 
     //! Object that computes the one-way Doppler observable for the downlink
-    std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > >
-    downlinkDopplerCalculator_;
+    std::shared_ptr< observation_models::OneWayDopplerObservationModel< ObservationScalarType, TimeType > > downlinkDopplerCalculator_;
 
     ObservationScalarType multiplicationTerm_;
 

include/tudat/astro/orbit_determination/observation_partials/directObservationPartial.h

@@ -28,6 +28,7 @@ class DirectObservationPartial: public ObservationPartial< ObservationSize >
     typedef std::vector< std::pair< Eigen::Matrix< double, ObservationSize, Eigen::Dynamic >, double > > ObservationPartialReturnType;
     typedef std::pair< Eigen::Matrix< double, ObservationSize, Eigen::Dynamic >, double > SingleObservationPartialReturnType;
     typedef std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > SingleLightTimePartialReturnType;
+    typedef std::pair< Eigen::Matrix< double, 3, Eigen::Dynamic >, double > SingleLightTimeGradientPartialReturnType;
 
     //! Constructor
     /*!
@@ -51,6 +52,8 @@ class DirectObservationPartial: public ObservationPartial< ObservationSize >
 
         std::pair< std::function< SingleLightTimePartialReturnType(
                     const std::vector< Eigen::Vector6d >&, const std::vector< double >& ) >, bool > lightTimeCorrectionPartial;
+        std::pair< std::function< SingleLightTimeGradientPartialReturnType(
+            const std::vector< Eigen::Vector6d >&, const std::vector< double >&, const observation_models::LinkEndType ) >, bool > lightTimeCorrectionGradientPartial;
 
         // Create light time correction partial functions
         for( unsigned int i = 0; i < lighTimeCorrectionPartials.size( ); i++ )
@@ -61,7 +64,17 @@ class DirectObservationPartial: public ObservationPartial< ObservationSize >
             {
                 lighTimeCorrectionPartialsFunctions_.push_back( lightTimeCorrectionPartial.first );
             }
-        }
+
+            if( positionPartialScaler->useLightTimeGradientPartials( ) )
+            {
+                lightTimeCorrectionGradientPartial = getLightTimeGradientParameterPartialFunction(
+                    parameterIdentifier, lighTimeCorrectionPartials.at( i ) );
+                if( lightTimeCorrectionGradientPartial.second != 0 )
+                {
+                    lighTimeCorrectionGradientPartialsFunctions_.push_back( lightTimeCorrectionGradientPartial.first );
+                }
+            }
+       }
     }
 
     //! Destructor.
@@ -161,6 +174,22 @@ class DirectObservationPartial: public ObservationPartial< ObservationSize >
                                         currentLinkTimeCorrectionPartial_.second ) );
         }
 
+        // Add scaled light-time gradient correcion partials.
+        for( unsigned int i = 0; i < lighTimeCorrectionGradientPartialsFunctions_.size( ); i++ )
+        {
+            currentLinkTimeCorrectionGradientPartial_ = lighTimeCorrectionGradientPartialsFunctions_.at( i )( states, times, observation_models::transmitter );
+            returnPartial.push_back(
+                std::make_pair( positionPartialScaler_->getLightTimeGradientPartialScalingFactor( observation_models::transmitter ) * physical_constants::SPEED_OF_LIGHT *
+                                currentLinkTimeCorrectionGradientPartial_.first,
+                                currentLinkTimeCorrectionGradientPartial_.second ) );
+            currentLinkTimeCorrectionGradientPartial_ = lighTimeCorrectionGradientPartialsFunctions_.at( i )( states, times, observation_models::receiver );
+            returnPartial.push_back(
+                std::make_pair( positionPartialScaler_->getLightTimeGradientPartialScalingFactor( observation_models::receiver ) * physical_constants::SPEED_OF_LIGHT *
+                                currentLinkTimeCorrectionGradientPartial_.first,
+                                currentLinkTimeCorrectionGradientPartial_.second ) );
+
+        }
+
         if( useLinkIndependentPartials( ) )
         {
             std::vector< std::pair< Eigen::Matrix< double, ObservationSize, Eigen::Dynamic >, double > > additionalPartials =
@@ -226,6 +255,10 @@ class DirectObservationPartial: public ObservationPartial< ObservationSize >
             const std::vector< Eigen::Vector6d >&, const std::vector< double >& ) > >
     lighTimeCorrectionPartialsFunctions_;
 
+    std::vector< std::function< SingleLightTimeGradientPartialReturnType(
+        const std::vector< Eigen::Vector6d >&, const std::vector< double >&, const observation_models::LinkEndType ) > >
+        lighTimeCorrectionGradientPartialsFunctions_;
+
     //! List of light-time correction partial objects.
     std::vector< std::shared_ptr< observation_partials::LightTimeCorrectionPartial > > lighTimeCorrectionPartials_;
 
@@ -237,6 +270,8 @@ class DirectObservationPartial: public ObservationPartial< ObservationSize >
 
     std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > currentLinkTimeCorrectionPartial_;
 
+    std::pair< Eigen::Matrix< double, 3, Eigen::Dynamic >, double > currentLinkTimeCorrectionGradientPartial_;
+
     std::map< observation_models::LinkEndType, int > stateEntryIndices_;
 
     bool fixLinkEndTime_ = false;

include/tudat/astro/orbit_determination/observation_partials/firstOrderRelativisticPartial.h

@@ -107,6 +107,15 @@ class FirstOrderRelativisticLightTimeCorrectionPartial: public LightTimeCorrecti
                     ( times[ 0 ] + times[ 1 ] ) / 2.0 );
     }
 
+    SingleOneWayRangeGradientPartialReturnType gradientWrtPpnParameterGamma(
+        const std::vector< Eigen::Vector6d >& states, const std::vector< double >& times, const observation_models::LinkEndType gradientLinkEnd )
+    {
+        return std::make_pair( correctionCalculator_->calculateLightTimeCorrectionPartialDerivativeWrtLinkEndPosition(
+            states.at( 0 ), states.at( 1 ), times.at( 0 ), times.at( 1 ), gradientLinkEnd ) /
+            ( correctionCalculator_->getPpnParameterGammaFunction_( )( ) + 1.0 ),
+                               gradientLinkEnd == observation_models::receiver ? times.at( 1 ) : times.at( 0 ) );
+    }
+
     //! Function to get the names of bodies causing light-time correction.
     /*!
      * Function to get the names of bodies causing light-time correction.

include/tudat/astro/orbit_determination/observation_partials/lightTimeCorrectionPartial.h

@@ -41,6 +41,8 @@ class LightTimeCorrectionPartial
     //! Typedef for return argument for light time partial (partial with vector and associated time)
     typedef std::pair< Eigen::Matrix< double, 1, Eigen::Dynamic >, double > SingleOneWayRangePartialReturnType;
 
+    typedef std::pair< Eigen::Matrix< double, 3, Eigen::Dynamic >, double > SingleOneWayRangeGradientPartialReturnType;
+
     //! Constructor
     /*!
      * Constructor
@@ -83,6 +85,12 @@ getLightTimeParameterPartialFunction(
         const estimatable_parameters::EstimatebleParameterIdentifier parameterId,
         const std::shared_ptr< LightTimeCorrectionPartial > lightTimeCorrectionPartial );
 
+std::pair< std::function< LightTimeCorrectionPartial::SingleOneWayRangeGradientPartialReturnType(
+    const std::vector< Eigen::Vector6d >&, const std::vector< double >&, const observation_models::LinkEndType ) >, bool >
+getLightTimeGradientParameterPartialFunction(
+    const estimatable_parameters::EstimatebleParameterIdentifier parameterId,
+    const std::shared_ptr< LightTimeCorrectionPartial > lightTimeCorrectionPartial );
+
 }
 
 }

include/tudat/astro/orbit_determination/observation_partials/observationPartial.h

@@ -125,11 +125,24 @@ class DirectPositionPartialScaling: public PositionPartialScaling
         }
     }
 
+    virtual Eigen::Matrix< double, ObservationSize, 3 > getLightTimeGradientPartialScalingFactor( const observation_models::LinkEndType linkEndType )
+    {
+        throw std::runtime_error( "Error when getting light time gradient partials of observable type " +
+                                  observation_models::getObservableName( observableType_) +
+                                  ", derived class model is not implemented." );
+    }
+
+
     virtual bool useLinkIndependentPartials( )
     {
         return false;
     }
 
+    virtual bool useLightTimeGradientPartials( )
+    {
+        return false;
+    }
+
 protected:
 
     observation_models::ObservableType observableType_;