Merge remote-tracking branch 'upstream/develop' into feature/radiatio…

…n_pressure_modeling
tudat-team · Sep 25, 2023 · b424b73 · b424b73
2 parents a7be1f6 + 49e6c96
commit b424b73
Show file tree

Hide file tree

Showing 27 changed files with 2,068 additions and 1,457 deletions.
diff --git a/.bumpversion.cfg b/.bumpversion.cfg
@@ -1,5 +1,5 @@
 [bumpversion]
-current_version = 2.12.1.dev24
+current_version = 2.12.1.dev30
 commit = True
 tag = True
 parse = (?P<major>\d+)\.(?P<minor>\d+)\.(?P<patch>\d+)(\.(?P<release>[a-z]+)(?P<dev>\d+))?

diff --git a/include/tudat/astro/observation_models/observationManager.h b/include/tudat/astro/observation_models/observationManager.h
@@ -450,9 +450,6 @@ class ObservationManager: public ObservationManagerBase< ObservationScalarType,
                     // Partial w.r.t. observation time property
                     if ( isParameterObservationLinkTimeProperty( partialIterator->second->getParameterIdentifier( ).first ) )
                     {
-//                        partialMatrix.block( 0, currentIndexInfo.first, observationSize, currentIndexInfo.second ) +=
-//                                observationPartialWrtTimeProperty * singlePartialSet[ i ].first;
-
                         // Iterate (again) over all observation partials to retrieve those associated with given link ends states.
                         for( auto itr : currentLinkEndPartials )
                         {
@@ -489,7 +486,9 @@ class ObservationManager: public ObservationManagerBase< ObservationScalarType,
                                     {
                                         throw std::runtime_error( "Error, required dependent variable interfaces, but none found when computing partials." );
                                     }
-                                    Eigen::VectorXd acceleration = dependentVariablesInterface_->getSingleDependentVariable( totalAccelerationVariable, times.at( indexLinkEndType ) );
+
+                                    Eigen::VectorXd acceleration = dependentVariablesInterface_->getSingleDependentVariable(
+                                        totalAccelerationVariable, times.at( indexLinkEndType ) );
                                     Eigen::Vector6d stateDerivativeVector = Eigen::Vector6d::Zero( );
                                     stateDerivativeVector.segment( 0, 3 ) = states.at( indexLinkEndType ).segment( 3, 3 );
                                     stateDerivativeVector.segment( 3, 3 ) = acceleration;

diff --git a/include/tudat/astro/orbit_determination/podInputOutputTypes.h b/include/tudat/astro/orbit_determination/podInputOutputTypes.h
diff --git a/include/tudat/astro/propagators/stateTransitionMatrixInterface.h b/include/tudat/astro/propagators/stateTransitionMatrixInterface.h
@@ -1009,7 +1009,6 @@ class HybridArcCombinedStateTransitionAndSensitivityMatrixInterface: public Comb
                         currentArc.second, false, arcDefiningBodies );
 
             // Set coupled block
-            std::cout << multiArcStateTransition.block( singleArcStateSize_, 0, originalMultiArcStateSize, singleArcStateSize_ ) << "\n\n";
             combinedStateTransitionMatrix.block( singleArcStateSize_, 0, originalMultiArcStateSize, singleArcStateSize_ ) =
                     multiArcStateTransition.block( singleArcStateSize_, 0, originalMultiArcStateSize, singleArcStateSize_ )
                     * singleArcStateTransitionAtArcStart.block( 0, 0, singleArcStateSize_, singleArcStateSize_ );

diff --git a/include/tudat/basics/utilities.h b/include/tudat/basics/utilities.h
@@ -11,6 +11,7 @@
 #ifndef TUDAT_UTILITIES_H
 #define TUDAT_UTILITIES_H
 
+#include <unordered_map>
 #include <map>
 #include <vector>
 #include <iostream>
@@ -92,6 +93,25 @@ std::vector< VectorArgument > createVectorFromMapValues( const std::map< KeyType
     return outputVector;
 }
 
+template< typename VectorArgument, typename KeyType >
+std::vector< KeyType > createVectorFromUnorderedMapKeys( const std::unordered_map< KeyType, VectorArgument >& inputMap )
+{
+    // Create and size return vector.
+    std::vector< KeyType > outputVector;
+    outputVector.resize( inputMap.size( ) );
+
+    // Iterate over all map entries and create vector
+    int currentIndex = 0;
+    for( typename std::unordered_map< KeyType, VectorArgument >::const_iterator mapIterator = inputMap.begin( );
+         mapIterator != inputMap.end( ); mapIterator++, currentIndex++ )
+    {
+        outputVector[ currentIndex ] = mapIterator->first;
+    }
+
+    return outputVector;
+}
+
+
 //! Function to create a vector from the keys of a map
 /*!
  *  Function to create a vector from the keys of a map. The output vector is in the order of the map entries, i.e. as provided by a forward iterator.
@@ -845,6 +865,21 @@ std::string to_string_with_precision(const T a_value, const int n = 6)
     return std::move(out).str();
 }
 
+template <typename T>
+Eigen::Matrix< T, Eigen::Dynamic, 1 > getSuccesivelyConcatenatedVector(
+    const Eigen::Matrix< T, Eigen::Dynamic, 1 > baseVector, const unsigned int numberOfConcatenations )
+{
+    int singleSize = baseVector.rows( );
+    Eigen::Matrix< T, Eigen::Dynamic, 1 > concatenatedVector = Eigen::Matrix< T, Eigen::Dynamic, 1 >::Zero(
+        numberOfConcatenations * singleSize );
+    for( unsigned int i = 0; i < numberOfConcatenations; i++ )
+    {
+        concatenatedVector.segment( i * singleSize, singleSize ) = baseVector;
+    }
+    return concatenatedVector;
+}
+
+
 } // namespace utilities
 
 } // namespace tudat

diff --git a/include/tudat/math/interpolators/oneDimensionalInterpolator.h b/include/tudat/math/interpolators/oneDimensionalInterpolator.h
@@ -75,7 +75,7 @@ class OneDimensionalInterpolator : public Interpolator< IndependentVariableType,
     { }
 
     //! Constructor.
-    /*!
+    /*!throw_exception_at_boundary
      *  Constructor.
      *  \param boundaryHandling Boundary handling method in case independent variable is outside the
      *      specified range.
@@ -185,6 +185,11 @@ class OneDimensionalInterpolator : public Interpolator< IndependentVariableType,
         return boundaryHandling_;
     }
 
+    void resetBoundaryHandling( const BoundaryInterpolationType boundaryHandling )
+    {
+        boundaryHandling_ = boundaryHandling;
+    }
+
     std::pair< DependentVariableType, DependentVariableType > getDefaultExtrapolationValue( )
     {
         return defaultExtrapolationValue_;

diff --git a/include/tudat/simulation/environment_setup/body.h b/include/tudat/simulation/environment_setup/body.h
@@ -2105,6 +2105,16 @@ class SystemOfBodies
         return bodyMap_.count( bodyName );
     }
 
+    bool doesBodyExist( const std::string& bodyName ) const
+    {
+        return ( this->count( bodyName ) != 0 );
+    }
+
+    std::vector< std::string > getListOfBodies( ) const
+    {
+        return utilities::createVectorFromUnorderedMapKeys( bodyMap_ );
+    }
+
     int getNumberOfBodies( ) const
     {
         return bodyMap_.size( );

diff --git a/include/tudat/simulation/estimation_setup/observations.h b/include/tudat/simulation/estimation_setup/observations.h
@@ -248,6 +248,11 @@ class ObservationCollection
         return observationSetStartAndSize_;
     }
 
+    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( )
     {
         return observationSetStartAndSizePerLinkEndIndex_;
@@ -399,6 +404,8 @@ class ObservationCollection
             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( );
@@ -408,10 +415,13 @@ class ObservationCollection
                                 std::make_pair( currentStartIndex, currentObservableVectorSize ) );
                     currentStartIndex += currentObservableVectorSize;
                     currentObservableTypeSize += currentObservableVectorSize;
+                    currentLinkEndSize += currentObservableVectorSize;
 
                     totalObservableSize_ += currentObservableVectorSize;
                     totalNumberOfObservables_ += currentNumberOfObservables;
                 }
+                observationTypeAndLinkEndStartAndSize_[ currentObservableType ][ currentLinkEnds ] = std::make_pair(
+                    currentLinkEndStartIndex, currentLinkEndSize );
             }
             observationTypeStartAndSize_[ currentObservableType ] = std::make_pair(
                         currentTypeStartIndex, currentObservableTypeSize );
@@ -504,6 +514,8 @@ class ObservationCollection
 
     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::pair< int, int > > observationTypeStartAndSize_;
 
     int totalObservableSize_;

diff --git a/include/tudat/simulation/estimation_setup/orbitDeterminationManager.h b/include/tudat/simulation/estimation_setup/orbitDeterminationManager.h
@@ -1101,7 +1101,7 @@ class OrbitDeterminationManager
             integrateAndEstimateOrbit_ = false;
         }
 
-        propagatorSettings->getOutputSettingsBase( )->setCreateDependentVariablesInterface( true );
+        propagatorSettings->getOutputSettingsBase( )->setUpdateDependentVariableInterpolator( true );
         if( integrateAndEstimateOrbit_ )
         {
             variationalEquationsSolver_ = simulation_setup::createVariationalEquationsSolver< ObservationScalarType, TimeType >(

diff --git a/include/tudat/simulation/estimation_setup/variationalEquationsSolver.h b/include/tudat/simulation/estimation_setup/variationalEquationsSolver.h
@@ -795,7 +795,8 @@ class SingleArcVariationalEquationsSolver: public VariationalEquationsSolver< St
 
         // Create object that will contain and process the propagation results
         variationalPropagationResults_ = std::make_shared< SingleArcVariationalSimulationResults< StateScalarType, TimeType>>(
-                dynamicsSimulator_->getSingleArcPropagationResults( ), this->stateTransitionMatrixSize_, this->parameterVectorSize_ - this->stateTransitionMatrixSize_ );
+                dynamicsSimulator_->getSingleArcPropagationResults( ),
+                this->stateTransitionMatrixSize_, this->parameterVectorSize_ - this->stateTransitionMatrixSize_ );
 
         // Integrate variational equations from initial state estimate.
         if( integrateEquationsOnCreation )
@@ -1219,7 +1220,8 @@ class MultiArcVariationalEquationsSolver: public VariationalEquationsSolver< Sta
                     arcWiseStateTransitionMatrixSize_.at( i ), arcWiseParameterVectorSize_.at( i ) - arcWiseStateTransitionMatrixSize_.at( i ) ) );
         }
         variationalPropagationResults_ = std::make_shared< MultiArcSimulationResults< SingleArcVariationalSimulationResults, StateScalarType, TimeType > >(
-                singleArcVariationalResults );
+                singleArcVariationalResults,
+                dynamicsSimulator_->getMultiArcPropagationResults( )->getMultiArcDependentVariablesInterface( ) );
 
 
         for( unsigned int i = 0; i < singleArcDynamicsSimulators.size( ); i++ )
@@ -1356,7 +1358,8 @@ class MultiArcVariationalEquationsSolver: public VariationalEquationsSolver< Sta
     {
 
         // Propagate variational equations and equations of motion concurrently
-        if( integrateEquationsConcurrently ) {
+        if( integrateEquationsConcurrently )
+        {
             // Propagate dynamics and variational equations and store results in variationalPropagationResults_ object
             dynamicsSimulator_->template integrateEquationsOfMotion<MultiArcVariationalResults>(
                     variationalPropagationResults_, getInitialStateProvider( initialStateEstimate ));
@@ -1816,6 +1819,9 @@ class HybridArcVariationalEquationsSolver: public VariationalEquationsSolver< St
     void integrateVariationalAndDynamicalEquations(
             const VectorType& initialStateEstimate, const bool integrateEquationsConcurrently )
     {
+        // TODO: do process depdendent variables in original multi-arc solver, do not process dependent variables in
+        // extended solver. Also add dependent variables to original multi-arc solver.
+
         // Reset initial time and propagate multi-arc equations
         singleArcSolver_->integrateVariationalAndDynamicalEquations(
                     initialStateEstimate.block( 0, 0, singleArcDynamicsSize_, 1 ),
@@ -1841,8 +1847,15 @@ class HybridArcVariationalEquationsSolver: public VariationalEquationsSolver< St
         removeSingleArcBodiesFromMultiArcSolultion( numericalMultiArcSolution );
 
         originalMultiArcSolver_->getDynamicsSimulator( )->getMultiArcPropagationResults( )->restartPropagation();
+
         // Reset original multi-arc bodies' dynamics
-        originalMultiArcSolver_->getDynamicsSimulator( )->getMultiArcPropagationResults( )->manuallySetPropagationResults( numericalMultiArcSolution );
+        originalMultiArcSolver_->getDynamicsSimulator( )->getMultiArcPropagationResults( )->manuallySetPropagationResults(
+            numericalMultiArcSolution );
+        originalMultiArcSolver_->getDynamicsSimulator( )->getMultiArcPropagationResults( )->manuallySetSecondaryData(
+            multiArcSolver_->getDynamicsSimulator( )->getMultiArcPropagationResults( ) );
+
+
+
         originalMultiArcSolver_->getDynamicsSimulator( )->processNumericalEquationsOfMotionSolution( );
 
         // Create state transition matrix if not yet created.
@@ -1903,6 +1916,8 @@ class HybridArcVariationalEquationsSolver: public VariationalEquationsSolver< St
 
         // Reset original multi-arc bodies' dynamics
         originalMultiArcSolver_->getDynamicsSimulator( )->getMultiArcPropagationResults( )->manuallySetPropagationResults( numericalMultiArcSolution );
+        originalMultiArcSolver_->getDynamicsSimulator( )->getMultiArcPropagationResults( )->manuallySetSecondaryData(
+            multiArcSolver_->getDynamicsSimulator( )->getMultiArcPropagationResults( ) );
         originalMultiArcSolver_->getDynamicsSimulator( )->processNumericalEquationsOfMotionSolution( );
     }