Added orbit fit function

include/tudat/simulation/estimation_setup/createObservationModel.h

@@ -2423,7 +2423,8 @@ std::shared_ptr< ObservationSimulator< ObservationSize, ObservationScalarType, T
  *  \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(
+std::vector< std::shared_ptr< ObservationSimulatorBase< ObservationScalarType, TimeType > > >
+    createObservationSimulators(
         const std::vector< std::shared_ptr< ObservationModelSettings > >& observationSettingsList,
         const simulation_setup::SystemOfBodies& bodies )
 {

include/tudat/simulation/estimation_setup/fitOrbitToEphemeris.h

@@ -0,0 +1,121 @@
+/*    Copyright (c) 2010-2019, Delft University of Technology
+ *    All rigths reserved
+ *
+ *    This file is part of the Tudat. Redistribution and use in source and
+ *    binary forms, with or without modification, are permitted exclusively
+ *    under the terms of the Modified BSD license. You should have received
+ *    a copy of the license with this file. If not, please or visit:
+ *    http://tudat.tudelft.nl/LICENSE.
+ */
+
+#ifndef TUDAT_FITORBITTOEPHEMERIS_H
+#define TUDAT_FITORBITTOEPHEMERIS_H
+
+#include <vector>
+
+#include <memory>
+#include <functional>
+
+#include <Eigen/Core>
+
+#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/orbitDeterminationManager.h"
+#include "tudat/simulation/estimation_setup/observationSimulationSettings.h"
+#include "tudat/simulation/estimation_setup/simulateObservations.h"
+
+namespace tudat
+{
+
+namespace simulation_setup
+{
+
+template< typename TimeType = double, typename StateScalarType = double >
+std::shared_ptr< EstimationOutput< > > createBestFitToCurrentEphemeris(
+    const SystemOfBodies& bodies,
+    const basic_astrodynamics::AccelerationMap& accelerationModelMap,
+    const std::vector< std::string >& bodiesToPropagate,
+    const std::vector< std::string >& centralBodies,
+    const std::shared_ptr< numerical_integrators::IntegratorSettings< TimeType > >& integratorSettings,
+    const TimeType initialTime,
+    const TimeType finalTime,
+    const TimeType dataPointInterval,
+    const std::vector< std::shared_ptr< estimatable_parameters::EstimatableParameterSettings > > additionalParameterNames =
+    std::vector< std::shared_ptr< estimatable_parameters::EstimatableParameterSettings > >( ),
+    const int numberOfIterations = 2 )
+{
+    using namespace observation_models;
+    using namespace estimatable_parameters;
+    using namespace propagators;
+
+    double initialPropagationTime = initialTime;
+
+   Eigen::VectorXd initialState = getInitialStatesOfBodies( bodiesToPropagate, centralBodies, bodies, initialPropagationTime );
+
+    std::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings =
+        std::make_shared< TranslationalStatePropagatorSettings< double > >
+        ( centralBodies, accelerationModelMap, bodiesToPropagate, initialState, initialPropagationTime, integratorSettings,
+            std::make_shared< PropagationTimeTerminationSettings >( finalTime ) );
+
+    std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames =
+        getInitialStateParameterSettings< double >( propagatorSettings, bodies );
+    parameterNames.insert(parameterNames.end(), additionalParameterNames.begin(), additionalParameterNames.end());
+
+    std::shared_ptr< estimatable_parameters::EstimatableParameterSet< StateScalarType > > parametersToEstimate =
+        createParametersToEstimate< StateScalarType, TimeType >( parameterNames, bodies, propagatorSettings );
+
+    std::vector< TimeType > observationTimes;
+    double currentTime = initialTime;
+    while( currentTime < finalTime )
+    {
+        observationTimes.push_back( currentTime );
+        currentTime += dataPointInterval;
+    }
+
+
+    std::vector< std::shared_ptr< observation_models::ObservationModelSettings > > observationModelSettingsList;
+    std::vector< std::shared_ptr< ObservationSimulationSettings< TimeType > > > measurementSimulationInput;
+
+    for( unsigned int i = 0; i < bodiesToPropagate.size( ); i++ )
+    {
+        // Create link ends
+        LinkEnds linkEnds;
+        linkEnds[ observed_body ] = bodiesToPropagate.at( i );
+        linkEnds[ observer ] = centralBodies.at( i );
+
+        // Create observation model settings
+        observationModelSettingsList.push_back( relativePositionObservableSettings( linkEnds ) );
+
+        measurementSimulationInput.push_back(
+            std::make_shared< TabulatedObservationSimulationSettings< TimeType > >(
+                relative_position_observable, linkEnds, observationTimes, observed_body ) );
+    }
+
+    std::shared_ptr< ObservationSimulatorBase<double, double> > observationSimulator =
+        createObservationSimulators( observationModelSettingsList, bodies ).at( 0 );
+
+    std::shared_ptr< observation_models::ObservationCollection< > > observationCollection =
+        simulateObservations( measurementSimulationInput, { observationSimulator }, bodies );
+
+    OrbitDeterminationManager< > orbitDeterminationManager = OrbitDeterminationManager< >(
+        bodies, parametersToEstimate, observationModelSettingsList, propagatorSettings );
+
+    std::shared_ptr< EstimationInput< > > estimationInput = std::make_shared< EstimationInput< > >(
+        observationCollection );
+    estimationInput->setConvergenceChecker( std::make_shared< EstimationConvergenceChecker >( numberOfIterations ) );
+    estimationInput->defineEstimationSettings(
+        0, 1, 0, 1, 1, 1 );
+    return  orbitDeterminationManager.estimateParameters( estimationInput );
+
+}
+
+} // namespace observation_models
+
+} // namespace tudat
+
+#endif // TUDAT_FITORBITTOEPHEMERIS_H

tests/src/astro/observation_models/unitTestDsnOdfMro.cpp

@@ -217,7 +217,7 @@ int main( )
     std::shared_ptr< observation_models::ObservationCollection< long double, Time > > filteredResidualObservationCollection =
         createResidualCollection( filteredObservedObservationCollection, filteredComputedObservationCollection );
 
-
+SystemOfB
 
     {
         Eigen::VectorXd residuals = filteredResidualObservationCollection->getObservationVector( ).template cast< double >( );

tests/src/astro/orbit_determination/CMakeLists.txt

@@ -112,6 +112,11 @@ TUDAT_ADD_TEST_CASE(NonSequentialStateEstimation
          ${Tudat_ESTIMATION_LIBRARIES}
          )
 
+ TUDAT_ADD_TEST_CASE(FitToSpice
+         PRIVATE_LINKS
+         ${Tudat_ESTIMATION_LIBRARIES}
+ )
+
 if( TUDAT_BUILD_WITH_EXTENDED_PRECISION_PROPAGATION_TOOLS )
 
     TUDAT_ADD_TEST_CASE(EstimationFromIdealDataDoubleLongDouble

tests/src/astro/orbit_determination/unitTestFitToSpice.cpp

@@ -0,0 +1,145 @@
+/*    Copyright (c) 2010-2019, Delft University of Technology
+ *    All rigths reserved
+ *
+ *    This file is part of the Tudat. Redistribution and use in source and
+ *    binary forms, with or without modification, are permitted exclusively
+ *    under the terms of the Modified BSD license. You should have received
+ *    a copy of the license with this file. If not, please or visit:
+ *    http://tudat.tudelft.nl/LICENSE.
+ */
+//
+//#define BOOST_TEST_DYN_LINK
+//#define BOOST_TEST_MAIN
+
+#include <string>
+#include <thread>
+
+#include <limits>
+
+#include <boost/test/unit_test.hpp>
+
+#include "tudat/simulation/estimation_setup/fitOrbitToEphemeris.h"
+//
+//
+//namespace tudat
+//{
+//namespace unit_tests
+//{
+//BOOST_AUTO_TEST_SUITE( test_fit_to_spice )
+
+//Using declarations.
+using namespace tudat;
+using namespace tudat::observation_models;
+using namespace tudat::orbit_determination;
+using namespace tudat::estimatable_parameters;
+using namespace tudat::simulation_setup;
+using namespace tudat::basic_astrodynamics;
+//
+//BOOST_AUTO_TEST_CASE( test_FitToSpice )
+//{
+int main( )
+{
+    //Load spice kernels.
+    spice_interface::loadStandardSpiceKernels( );
+
+    spice_interface::loadSpiceKernelInTudat(
+        tudat::paths::get_spice_kernels_path( )  + "/mgs_map1_ipng_mgs95j.bsp" );
+
+    double initialTime = DateTime( 1999, 3, 10, 0, 0, 0.0 ).epoch< double >( );
+    double finalTime = DateTime( 1999, 3, 12, 0, 0, 0.0 ).epoch< double >( );
+
+    std::vector< std::string > bodyNames;
+    bodyNames.push_back( "Earth" );
+    bodyNames.push_back( "Mars" );
+    bodyNames.push_back( "Sun" );
+    bodyNames.push_back( "Moon" );
+    bodyNames.push_back( "Jupiter" );
+    bodyNames.push_back( "Saturn" );
+
+    BodyListSettings bodySettings =
+        getDefaultBodySettings( bodyNames, "Mars" );
+    bodySettings.addSettings( "MGS" );
+    bodySettings.at( "MGS" )->ephemerisSettings = std::make_shared< InterpolatedSpiceEphemerisSettings >(
+        initialTime - 3600.0, finalTime + 3600.0, 30.0, "Mars" );
+
+    for( unsigned int i = 0; i < 2; i++ )
+    {
+
+        // Create bodies needed in simulation
+        SystemOfBodies bodies = createSystemOfBodies( bodySettings );
+
+        // Create radiation pressure settings
+        double referenceAreaRadiation = 4.0;
+        double radiationPressureCoefficient = 1.2;
+        std::vector<std::string> occultingBodies;
+        occultingBodies.push_back( "Mars" );
+        std::shared_ptr<RadiationPressureInterfaceSettings> asterixRadiationPressureSettings =
+            std::make_shared<CannonBallRadiationPressureInterfaceSettings>(
+                "Sun", referenceAreaRadiation, radiationPressureCoefficient, occultingBodies );
+
+        // Create and set radiation pressure settings
+        bodies.at( "MGS" )->setRadiationPressureInterface(
+            "Sun", createRadiationPressureInterface(
+                asterixRadiationPressureSettings, "MGS", bodies ));
+        bodies.at( "MGS" )->setConstantBodyMass( 2000.0 );
+
+        // Set accelerations between bodies that are to be taken into account.
+        SelectedAccelerationMap accelerationMap;
+        std::map<std::string, std::vector<std::shared_ptr<AccelerationSettings> > > accelerationsOfGrail;
+        accelerationsOfGrail[ "Sun" ].push_back( std::make_shared<AccelerationSettings>( point_mass_gravity ));
+        accelerationsOfGrail[ "Sun" ].push_back(
+            std::make_shared<AccelerationSettings>( cannon_ball_radiation_pressure ));
+        accelerationsOfGrail[ "Earth" ].push_back( std::make_shared<AccelerationSettings>( point_mass_gravity ));
+        accelerationsOfGrail[ "Mars" ].push_back( std::make_shared<SphericalHarmonicAccelerationSettings>( 64, 64 ));
+        accelerationsOfGrail[ "Mars" ].push_back( empiricalAcceleration( ));
+        accelerationsOfGrail[ "Moon" ].push_back( std::make_shared<AccelerationSettings>( point_mass_gravity ));
+        accelerationsOfGrail[ "Jupiter" ].push_back( std::make_shared<AccelerationSettings>( point_mass_gravity ));
+        accelerationsOfGrail[ "Saturn" ].push_back( std::make_shared<AccelerationSettings>( point_mass_gravity ));
+
+        accelerationMap[ "MGS" ] = accelerationsOfGrail;
+
+        // Set bodies for which initial state is to be estimated and integrated.
+        std::vector<std::string> bodiesToEstimate = { "MGS" };
+        std::vector<std::string> centralBodies = { "Mars" };
+
+        AccelerationMap accelerationModelMap = createAccelerationModelsMap(
+            bodies, accelerationMap, bodiesToEstimate, centralBodies );
+
+        std::vector<std::shared_ptr<estimatable_parameters::EstimatableParameterSettings> > additionalParameterNames;
+        if( i > 0 )
+        {
+            additionalParameterNames.push_back( estimatable_parameters::radiationPressureCoefficient( "MGS" ));
+
+            std::map<basic_astrodynamics::EmpiricalAccelerationComponents,
+                std::vector<basic_astrodynamics::EmpiricalAccelerationFunctionalShapes> > empiricalComponentsToEstimate;
+            empiricalComponentsToEstimate[ radial_empirical_acceleration_component ].push_back( constant_empirical );
+            empiricalComponentsToEstimate[ radial_empirical_acceleration_component ].push_back( sine_empirical );
+            empiricalComponentsToEstimate[ radial_empirical_acceleration_component ].push_back( cosine_empirical );
+
+            empiricalComponentsToEstimate[ along_track_empirical_acceleration_component ].push_back( constant_empirical );
+            empiricalComponentsToEstimate[ along_track_empirical_acceleration_component ].push_back( sine_empirical );
+            empiricalComponentsToEstimate[ along_track_empirical_acceleration_component ].push_back( cosine_empirical );
+
+            empiricalComponentsToEstimate[ across_track_empirical_acceleration_component ].push_back( constant_empirical );
+            empiricalComponentsToEstimate[ across_track_empirical_acceleration_component ].push_back( sine_empirical );
+            empiricalComponentsToEstimate[ across_track_empirical_acceleration_component ].push_back( cosine_empirical );
+
+            additionalParameterNames.push_back( std::make_shared<EmpiricalAccelerationEstimatableParameterSettings>(
+                "MGS", "Mars", empiricalComponentsToEstimate ));
+        }
+
+        std::shared_ptr<EstimationOutput<> > estimationOutput =
+            createBestFitToCurrentEphemeris( bodies, accelerationModelMap, bodiesToEstimate, centralBodies,
+                                             numerical_integrators::rungeKuttaFixedStepSettings( 120.0,
+                                                                                                 numerical_integrators::rungeKuttaFehlberg78 ),
+                                             initialTime, finalTime, 600.0, additionalParameterNames );
+    }
+
+
+}
+//
+//BOOST_AUTO_TEST_SUITE_END( )
+//
+//}
+//
+//}