From 37a588d2795460b20f0ea2768e9cd4de6a345f1c Mon Sep 17 00:00:00 2001 From: RivaAlkahal Date: Tue, 20 Aug 2024 19:36:33 +0200 Subject: [PATCH] tests of estimation functions --- cmake_modules/TudatFindBoost.cmake | 10 +- tests_riva/CMakeLists.txt | 24 + tests_riva/multiarc-observ.cpp | 749 ++++++++++++++++++++++++ tests_riva/multiarc-trial2.cpp | 470 +++++++++++++++ tests_riva/multiarc-trial3.cpp | 679 +++++++++++++++++++++ tests_riva/newtest.cpp | 300 ++++++++++ tests_riva/oldtest.cpp | 385 ++++++++++++ tests_riva/testEstimationVariations.cpp | 457 +++++++++++++++ 8 files changed, 3073 insertions(+), 1 deletion(-) create mode 100644 tests_riva/multiarc-observ.cpp create mode 100644 tests_riva/multiarc-trial2.cpp create mode 100644 tests_riva/multiarc-trial3.cpp create mode 100644 tests_riva/newtest.cpp create mode 100644 tests_riva/oldtest.cpp create mode 100644 tests_riva/testEstimationVariations.cpp diff --git a/cmake_modules/TudatFindBoost.cmake b/cmake_modules/TudatFindBoost.cmake index e44ef2d725..9384dfea7f 100644 --- a/cmake_modules/TudatFindBoost.cmake +++ b/cmake_modules/TudatFindBoost.cmake @@ -47,7 +47,15 @@ foreach(_TUDAT_BOOST_COMPONENT ${_TUDAT_REQUIRED_BOOST_LIBS}) add_library(Boost::${_TUDAT_BOOST_COMPONENT} UNKNOWN IMPORTED ../tests/src/simulation/unitTestPolyhedron.cpp ../tests_riva/unitTestMarsDtm.cpp ../tests_riva/unitTestMarsDtm.cpp - ../tests_riva/unitTestMarsDtm.cpp) + ../tests_riva/unitTestMarsDtm.cpp + ../tests_riva/testEstimationVariations.cpp + ../tests_riva/oldtest.cpp + ../tests_riva/newtest.cpp + ../tests_riva/multiarc-trial2.cpp + ../tests_riva/multiarc-trial3.cpp + ../tests_riva/multiarc-trial3.cpp + ../tests_riva/multiarc-observ.cpp + ) endif() set_target_properties(Boost::${_TUDAT_BOOST_COMPONENT} PROPERTIES INTERFACE_INCLUDE_DIRECTORIES "${Boost_INCLUDE_DIRS}") diff --git a/tests_riva/CMakeLists.txt b/tests_riva/CMakeLists.txt index f85989c5c5..8a237e93ec 100644 --- a/tests_riva/CMakeLists.txt +++ b/tests_riva/CMakeLists.txt @@ -6,3 +6,27 @@ TUDAT_ADD_EXECUTABLE(application_test_mars_dtm2 "TestMarsDtm_.cpp" ${Tudat_ESTIMATION_LIBRARIES} ) +TUDAT_ADD_EXECUTABLE(application_test_mars_dtm3 + "testEstimationVariations.cpp" + ${Tudat_ESTIMATION_LIBRARIES} +) +TUDAT_ADD_EXECUTABLE(application_test_mars_dtm4 + "newtest.cpp" + ${Tudat_ESTIMATION_LIBRARIES} +) +TUDAT_ADD_EXECUTABLE(application_test_mars_dtm5 + "oldtest.cpp" + ${Tudat_ESTIMATION_LIBRARIES} +) +TUDAT_ADD_EXECUTABLE(application_test_mars_dtm6 + "multiarc-trial2.cpp" + ${Tudat_ESTIMATION_LIBRARIES} +) +TUDAT_ADD_EXECUTABLE(application_test_mars_dtm7 + "multiarc-trial3.cpp" + ${Tudat_ESTIMATION_LIBRARIES} +) +TUDAT_ADD_EXECUTABLE(application_test_mars_dtm8 + "multiarc-observ.cpp" + ${Tudat_ESTIMATION_LIBRARIES} +) \ No newline at end of file diff --git a/tests_riva/multiarc-observ.cpp b/tests_riva/multiarc-observ.cpp new file mode 100644 index 0000000000..11b0089ed9 --- /dev/null +++ b/tests_riva/multiarc-observ.cpp @@ -0,0 +1,749 @@ +// +// Created by Riva Alkahal on 19/08/2024. +// +// +// Created by Riva Alkahal on 11/08/2024. +// +// +// Created by Riva Alkahal on 26/07/2024. +// + +#include +#include +#include +#include "fstream" +#include "iostream" + +#include + +#include "tudat/basics/testMacros.h" +#include "tudat/astro/aerodynamics/customAerodynamicCoefficientInterface.h" +#include "tudat/astro/aerodynamics/aerodynamicAcceleration.h" +#include "tudat/astro/reference_frames/aerodynamicAngleCalculator.h" +#include "tudat/simulation/propagation_setup/dynamicsSimulator.h" +#include "tudat/interface/spice/spiceEphemeris.h" +#include "tudat/interface/spice/spiceRotationalEphemeris.h" +#include "tudat/io/basicInputOutput.h" +#include "tudat/simulation/environment_setup/body.h" +#include "tudat/simulation/estimation_setup/createNumericalSimulator.h" +#include "tudat/simulation/environment_setup/defaultBodies.h" +#include "tudat/simulation/environment_setup/body.h" +#include "tudat/astro/basic_astro/timeConversions.h" +#include "tudat/astro/aerodynamics/marsDtmAtmosphereModel.h" +#include "tudat/io/solarActivityData.h" +#include "tudat/astro/basic_astro/unitConversions.h" + +#include "tudat/basics/testMacros.h" +#include "tudat/simulation/estimation.h" +#include "tudat/simulation/estimation_setup.h" + +#include "tudat/io/readOdfFile.h" +#include "tudat/io/readTabulatedMediaCorrections.h" +#include "tudat/io/readTabulatedWeatherData.h" +#include "tudat/simulation/estimation_setup/processOdfFile.h" +#include "tudat/simulation/propagation_setup/setNumericallyIntegratedStates.h" + +#include + +#include "tudat/astro/ground_stations/transmittingFrequencies.h" + + +int main( ) { + using namespace tudat; + using namespace aerodynamics; + using namespace simulation_setup; + using namespace numerical_integrators; + using namespace simulation_setup; + using namespace basic_astrodynamics; + using namespace propagators; + using namespace estimatable_parameters; + using namespace observation_models; + using namespace basic_mathematics; + using namespace basic_astrodynamics; + using namespace tudat::spice_interface; + using namespace tudat::ephemerides; + using namespace tudat::input_output; + using namespace tudat::orbit_determination; + using namespace tudat::interpolators; + using namespace tudat::orbital_element_conversions; + + spice_interface::loadStandardSpiceKernels( ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map4.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map5.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map6.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map7.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map8.bsp" ); + + std::string saveDirectory = "/Users/ralkahal/OneDrive - Delft University of Technology/PhD/Programs/testingestimation/"; + std::string fileTag = "polygrav"; + + // set input options + double epehemeridesTimeStep = 60.0; + bool useInterpolatedEphemerides = true; + double observationsSamplingTime = 60.0; + double buffer = 10.0 * epehemeridesTimeStep; + //const double gravitationalParameter = 4.2828378e13; + //const double planetaryRadius = 3389.5E3; + + // Select ephemeris time range (based on available data in loaded SPICE ephemeris) + //Time initialEphemerisTime = Time( 185976000 - 1.0 * 86400.0 ); // 23 November 2005, 0h + //Time finalEphemerisTime = Time( 186580800 + 1.0 * 86400.0 ); // 30 November 2005, 0h + // time at 2000-01-01T00:00:00 + Time initialEphemerisTime = Time( 0.0 ) ; + Time finalEphemerisTime = Time( 86400.0 * 6.0 ); // 5 years later + double totalDuration = finalEphemerisTime - initialEphemerisTime; + std::cout<<"Total duration: "< bodiesToCreate = { + // "Earth", "Sun", "Mercury", "Venus", "Mars", "Jupiter", "Saturn", "Uranus", "Neptune", "Phobos", "Deimos", + // "Io", "Ganymede", "Callisto", "Europa", "Titan" }; + std::vector< std::string > bodiesToCreate = { + "Earth", "Sun", "Mercury", "Venus", "Mars", "Jupiter", "Phobos", "Deimos" }; + + std::string baseFrameOrientation = "MARSIAU"; + std::string baseFrameOrigin = "SSB"; + + // Specify ephemeris time steps and buffers + Time ephemerisTimeStepPlanets = epehemeridesTimeStep; + std::cout<<"Ephemeris time step planets: "<groundStationSettings = getDsnStationSettings( ); + + std::string filename ="/Users/ralkahal/OneDrive - Delft University of Technology/PhD/Programs/atmodensitydtm/dtm_mars";; + bodySettings.at( "Mars" )->atmosphereSettings = marsDtmAtmosphereSettings( filename, 3378.0E3); + + // Create spacecraft + std::string spacecraftName = "MGS"; + bodySettings.addSettings( spacecraftName ); + if ( useInterpolatedEphemerides ) + { + bodySettings.at( spacecraftName )->ephemerisSettings = + std::make_shared< InterpolatedSpiceEphemerisSettings >( + initialEphemerisTime - buffer, finalEphemerisTime + buffer, + ephemerisTimeStepSpacecraft, baseFrameOrigin, baseFrameOrientation, + std::make_shared< interpolators::LagrangeInterpolatorSettings >( 8 ), spacecraftName ); + } + else + { + bodySettings.at( spacecraftName )->ephemerisSettings = + std::make_shared< DirectSpiceEphemerisSettings >( baseFrameOrigin, baseFrameOrientation ); + } + bodySettings.at( spacecraftName )->constantMass = 700.0; + + // Create radiation pressure settings + double referenceAreaRadiation = 4.0; + double radiationPressureCoefficient = 1.2; + std::vector< std::string > occultingBodies = { "Mars" }; + bodySettings.at( spacecraftName )->radiationPressureSettings[ "Sun" ] = + cannonBallRadiationPressureSettings( "Sun", referenceAreaRadiation, radiationPressureCoefficient, occultingBodies ); + + std::shared_ptr aerodynamicCoefficientSettings = + std::make_shared( + 2.0, 4.0, Eigen::Vector3d::Zero( ), Eigen::Vector3d::UnitX( ), Eigen::Vector3d::Zero( ), + negative_aerodynamic_frame_coefficients, negative_aerodynamic_frame_coefficients ); + bodySettings.at( spacecraftName )->aerodynamicCoefficientSettings = aerodynamicCoefficientSettings; + bodySettings.at( spacecraftName )->ephemerisSettings->resetMakeMultiArcEphemeris( true ); + + std::vector< std::shared_ptr< GravityFieldVariationSettings > > gravityFieldVariations; + + std::map cosineAmplitudes; + cosineAmplitudes[ 1 ] = Eigen::Matrix< double, 3, 5 >::Zero( ); + //cosineAmplitudes[ 2 ] = Eigen::Matrix< double, 3, 5 >::Zero( ); + std::map sineAmplitudes; + sineAmplitudes[ 1 ] = Eigen::Matrix< double, 3, 5 >::Zero( ); + std::shared_ptr< GravityFieldVariationSettings > polynomialGravityFieldVariations = + std::make_shared< PolynomialGravityFieldVariationsSettings >( + cosineAmplitudes, sineAmplitudes, -1.0E4, 2, 0 ); + gravityFieldVariations.push_back( polynomialGravityFieldVariations ); + + std::vector< std::shared_ptr< GravityFieldVariationSettings > > gravityFieldVariationSettings = + gravityFieldVariations; + bodySettings.at( "Mars" )->gravityFieldVariationSettings = gravityFieldVariations; + + SystemOfBodies bodies = createSystemOfBodies< long double, Time >( bodySettings ); + + //Time initialPropagationTime = initialEphemerisTime + 600.0; + //Time finalPropagationTime = finalEphemerisTime - 600.0; + + + + // Set accelerations on Vehicle that are to be taken into account. + SelectedAccelerationMap accelerationMap; + std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle; + accelerationsOfVehicle[ "Sun" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Sun" ].push_back( cannonBallRadiationPressureAcceleration( ) ); +// accelerationsOfVehicle[ "Sun" ].push_back( relativisticAccelerationCorrection( ) ); + accelerationsOfVehicle[ "Mercury" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Venus" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Earth" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Mars" ].push_back( sphericalHarmonicAcceleration( 50, 50 ) ); + accelerationsOfVehicle[ "Mars" ].push_back( relativisticAccelerationCorrection( ) ); + accelerationsOfVehicle[ "Mars" ].push_back( aerodynamicAcceleration( ) ); + accelerationsOfVehicle[ "Phobos" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Deimos" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Jupiter" ].push_back( pointMassGravityAcceleration( ) ); + + accelerationMap[ spacecraftName ] = accelerationsOfVehicle; + + // Set bodies for which initial state is to be estimated and integrated. + std::vector< std::string > bodiesToIntegrate; + std::string centralBody = "Mars"; + std::vector< std::string > centralBodies = { centralBody }; + bodiesToIntegrate.push_back( spacecraftName ); + + // Create acceleration models + AccelerationMap accelerationModelMap = createAccelerationModelsMap( bodies, accelerationMap, bodiesToIntegrate, centralBodies ); + std::cout<<"acceleration map created"< integrationArcStartTimes; + std::vector< double > integrationArcEndTimes; + std::vector< double > integrationArcLimits; + + double integrationStartTime = initialEphemerisTime + 120.0; //1.0E2; + double integrationEndTime = finalEphemerisTime - 120.0 ; //1.0E2; + double totalDurationIntegration = integrationEndTime - integrationStartTime; + double arcDuration = 86400.0;//2.0E4; + double arcOverlap = 60.0;//2.0E2; + + double currentStartTime = integrationStartTime, currentEndTime = integrationStartTime + arcDuration; + + do + { + integrationArcLimits.push_back( currentStartTime ); + integrationArcEndTimes.push_back( currentEndTime ); + std::cout<<"integration arc start times: "< observationTimes; + std::vector< Eigen::Matrix< long double, Eigen::Dynamic, 1 > > observations; + for ( Time t = integrationStartTime + 600; t < integrationEndTime - 600; t += observationsSamplingTime ) + { + try + { + observations.push_back( bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( t ).segment( 0, 3 ) ); + observationTimes.push_back( t ); + } + catch( ... ) + { } + } + // Retrieve state history from SPICE + std::map< long double, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > spiceStateHistory; + for ( Time t : observationTimes ) + { + spiceStateHistory[ t.getSeconds< long double >() ] = + bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( t ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( t ); + } + writeDataMapToTextFile( spiceStateHistory, "stateHistorySpice_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + + // Define integrator settings. + //std::shared_ptr< IntegratorSettings< double > > integratorSettings = + // rungeKutta4Settings< double >( 60.0 ); + std::shared_ptr< IntegratorSettings< > > integratorSettings = + std::make_shared< IntegratorSettings< > > + ( rungeKutta4, integrationArcStartTimes.at(0), 60.0 ); + + std::cout<<"Integration settings created"< integrationMinMaxStep = std::make_pair( 1e-16, 1e16 ); + + double initialStep = 5.0; + if ( integrationMinMaxStep.first == integrationMinMaxStep.second ) + { + initialStep = integrationMinMaxStep.first; + } + std::shared_ptr< IntegratorSettings< Time > > integratorSettings = rungeKuttaVariableStepSettingsScalarTolerances< Time >( + initialStep, rungeKutta87DormandPrince, integrationMinMaxStep.first, + integrationMinMaxStep.second, 1e-10, 1e-10 );*/ + + /*std::shared_ptr< IntegratorSettings< Time > > integratorSettings = + rungeKutta4Settings< Time >( 60.0 );*/ + // Set initial state from ephemerides + /* Eigen::Matrix< long double, 6, 1 > spacecraftInitialState = + bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( initialPropagationTime ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( initialPropagationTime ); + */ + + + int numberOfIntegrationArcs = integrationArcStartTimes.size( ); + for( unsigned int i = 0; i < numberOfIntegrationArcs; i++ ) { + std::cout << "integration arc start times"< systemInitialStates(numberOfIntegrationArcs, Eigen::VectorXd(6)); + + std::vector< std::shared_ptr< SingleArcPropagatorSettings< double > > > arcPropagationSettingsList; + for( unsigned int i = 0; i < numberOfIntegrationArcs; i++ ) + { + std::cout<<"iteration started"< > + ( centralBodies, accelerationModelMap, bodiesToIntegrate, + systemInitialStates.at( i ), integrationArcEndTimes.at( i ) ) ); + } + + std::cout<<"single arc propagation done"< > multiArcPropagatorSettings = + validateDeprecatedMultiArcSettings< double, double >( + integratorSettings, std::make_shared< MultiArcPropagatorSettings< double > >( arcPropagationSettingsList ), + integrationArcStartTimes, false, true ); + + + //std::shared_ptr< PropagationPrintSettings > multiArcPrintSettings = + // std::make_shared< PropagationPrintSettings >( ); + //multiArcPrintSettings->reset( + // true, true, TUDAT_NAN, 0, true, true, true, true, true, true ); + + // multiArcPropagatorSettings->getOutputSettings( )->resetAndApplyConsistentSingleArcPrintSettings( + // multiArcPrintSettings ); + + MultiArcDynamicsSimulator< > dynamicsSimulator( + bodies, multiArcPropagatorSettings ); + + std::map< long double, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > propagatedStateHistory; + for ( Time t : observationTimes ) + { + propagatedStateHistory[ t.getSeconds< long double >() ] = + bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( t ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( t ); + } + writeDataMapToTextFile( propagatedStateHistory, "stateHistoryPropagation_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + + + //initial state? + std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames = + getInitialMultiArcParameterSettings< double, double >( multiArcPropagatorSettings, bodies, integrationArcStartTimes ); + //polynomial gravity field + std::map > > cosineBlockIndicesPerPower; + cosineBlockIndicesPerPower[ 1 ].push_back( std::make_pair( 2, 0 ) ); + std::map > > sineBlockIndicesPerPower; + sineBlockIndicesPerPower[ 1 ].push_back( std::make_pair( 3, 2 ) ); + parameterNames.push_back( std::make_shared< PolynomialGravityFieldVariationEstimatableParameterSettings >( + "Mars", cosineBlockIndicesPerPower, sineBlockIndicesPerPower ) ); + + std::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parametersToEstimate = + createParametersToEstimate< double, double >( parameterNames, bodies ); + + std::cout<<"parameters to estimate created"<(totalDuration / physical_constants::JULIAN_DAY ); + std::cout<<"days: "< initial_times_list = { integrationArcStartTimes[0] + buffer}; + for (int day = 1; day < days; ++day) { + initial_times_list.push_back(integrationArcStartTimes[0] + buffer + day * 24 * 3600); + } + std::vector final_times_list = { integrationArcStartTimes[0] + buffer + hoursperday * 3600 }; + for (int day = 1; day < days; ++day) { + final_times_list.push_back(integrationArcStartTimes[0] + buffer + day * 24 * 3600 + hoursperday * 3600); + } + std::vector observationTimesList; + for (int i = 0; i < days; ++i) { + double start = initial_times_list[i]; + std::cout<<"start time: "< stationTransmitterLinkEnds; + //linkEnds[ receiver ] = spacecraftName; + std::vector< std::string > GroundStations = { "DSS-26" , "DSS-42", "DSS-61"}; + for ( std::string groundStation : GroundStations ) + { + linkEnds[ transmitter ] = LinkEndId( "Earth", groundStation ); + linkEnds[ receiver ] = spacecraftName; + stationTransmitterLinkEnds.push_back( linkEnds ); + } + // Define (arbitrarily) link ends to be used for the chosen observable type (e.g., one-way range) + //std::map< ObservableType, std::vector< LinkEnds > > linkEndsPerObservable; + ObservableType selectedObservable = one_way_range; // Define the observable type (replace with desired type) + std::vector< std::shared_ptr< ObservationModelSettings > > observationSettingsList; + // Populate the map with all the transmitter link ends + for( unsigned int i = 0; i < stationTransmitterLinkEnds.size( ); i++ ) + { + //linkEndsPerObservable[selectedObservable].push_back( stationTransmitterLinkEnds[ i ] ); + observationSettingsList.push_back( std::make_shared< ObservationModelSettings >( + one_way_range, stationTransmitterLinkEnds.at( i ) ) ); + } + + std::cout<<"link ends created"< > observationSettingsList; + // observationSettingsList.push_back( std::make_shared< ObservationModelSettings >( + // one_way_range, linkEnds ) ); + std::cout<<"observation settings created"< orbitDeterminationManager = + OrbitDeterminationManager< double, double >( + bodies, parametersToEstimate, + observationSettingsList, multiArcPropagatorSettings ); + std::cout<<"orbit determination manager created"< > observationViabilitySettings; + for ( std::string groundStation : GroundStations ) + { + observationViabilitySettings.push_back( std::make_shared< ObservationViabilitySettings >( + minimum_elevation_angle, std::make_pair( "Earth", groundStation ), "", + unit_conversions::convertDegreesToRadians( 15.0 ) ) ); + observationViabilitySettings.push_back( std::make_shared( + body_occultation, + std::make_pair(groundStation, spacecraftName), + "Mars", + TUDAT_NAN + )); + } + std::cout<<"Observation viability settings created"< > viabilityCalculators; + // for (unsigned int i = 0; i >(createObservationViabilityCalculators( bodies, stationTransmitterLinkEnds[i], one_way_range, observationViabilitySettings ))); + + // } + std::vector< std::shared_ptr< ObservationViabilityCalculator > > viabilityCalculators; + for (unsigned int i = 0; i < stationTransmitterLinkEnds.size(); i++) { + std::vector< std::shared_ptr< ObservationViabilityCalculator > > calculators = + createObservationViabilityCalculators(bodies, stationTransmitterLinkEnds[i], one_way_range, observationViabilitySettings); + viabilityCalculators.insert(viabilityCalculators.end(), calculators.begin(), calculators.end()); + } + // PerObservableObservationViabilityCalculatorList viabilityCalculators = createObservationViabilityCalculators( + // bodies, stationTransmitterLinkEnds, observationViabilitySettings ); + + std::cout<<"Observation viability calculator created"< > noiseFunctions; + noiseFunctions[ one_way_range ] = + std::bind( &utilities::evaluateFunctionWithoutInputArgumentDependency< double, const double >, + createBoostContinuousRandomVariableGeneratorFunction( + tudat::statistics::normal_boost_distribution, { 0.0, rangeNoise }, 0.0 ), std::placeholders::_1 ); + + std::cout<<"noise functions created"< noiseFunctionWrapper = + [noiseFunctions](const double time) -> Eigen::VectorXd + { + double noiseValue = noiseFunctions.at(one_way_range)(time); + Eigen::VectorXd noiseVector(1); + noiseVector(0) = noiseValue; + return noiseVector; + }; + std::cout<<"noise functions converted to Eigen::VectorXd"< initialParameterEstimate = + parametersToEstimate->template getFullParameterValues< double >( ); + + // Create observation model settings + //std::vector< std::shared_ptr< observation_models::ObservationModelSettings > > observationModelSettingsList; + //observationModelSettingsList.push_back( positionObservableSettings( linkEnds ) ); + + +// Observation simulation settings + std::vector< std::shared_ptr< ObservationSimulationSettings< double > > > measurementSimulationInput; + for (unsigned int r = 0; r < stationTransmitterLinkEnds.size(); r++) { + measurementSimulationInput.push_back( + std::make_shared< TabulatedObservationSimulationSettings< double > >( + one_way_range, stationTransmitterLinkEnds[r], observationTimesList, receiver, observationViabilitySettings, noiseFunctionWrapper)); + } + //maybe add the viability settings here + //check out the measurment simulation settings + + + std::shared_ptr< ObservationCollection< double, double > > observationsAndTimes = simulateObservations< double, double >( + measurementSimulationInput, orbitDeterminationManager.getObservationSimulators( ), bodies ); + //std::shared_ptr< ObservationCollection< double, double > > observationsAndTimes = simulateObservationsWithNoise< double, double >( + // measurementSimulationInput, orbitDeterminationManager.getObservationSimulators( ), noiseFunctions, viabilityCalculators ); + //PodInputDataType observationsAndTimes = simulateObservationsWithNoise< double, double >( + // measurementSimulationInput, orbitDeterminationManager.getObservationSimulators( ), noiseFunctions, + // viabilityCalculators ); + std::cout<<"observations and times created"< truthParameters = initialParameterEstimate; + int numberOfParameters = initialParameterEstimate.rows( ); + // Perturb initial states + for( unsigned int i = 0; i < integrationArcStartTimes.size( ); i++ ) + { + initialParameterEstimate[ 0 + 6 * i ] += 1.0E0; + initialParameterEstimate[ 1 + 6 * i ] += 1.0E0; + initialParameterEstimate[ 2 + 6 * i ] += 1.0E0; + initialParameterEstimate[ 3 + 6 * i ] += 1.0E-5; + initialParameterEstimate[ 4 + 6 * i ] += 1.0E-5; + initialParameterEstimate[ 5 + 6 * i ] += 1.0E-5; + } + std::cout<<"initial parameters perturbed"<( initialParameterEstimate.rows( ) ); i++ ) + { + initialParameterEstimate[ i ] *= ( 1.0 + 1.0E-6 ); + } +*/ + parametersToEstimate->resetParameterValues( initialParameterEstimate ); + + // Define estimation input + std::shared_ptr< EstimationInput< double, double > > estimationInput = + std::make_shared< EstimationInput< double, double > >( + observationsAndTimes, + Eigen::MatrixXd::Zero( 0, 0 ), + std::make_shared< EstimationConvergenceChecker >( 5 ) ); + // Call the function with reintegrateVariationalEquations set to true + estimationInput->defineEstimationSettings( + true, // reintegrateEquationsOnFirstIteration + true, // reintegrateVariationalEquations + true, // saveDesignMatrix + true, // printOutput + true, // saveResidualsAndParametersFromEachIteration + true, // saveStateHistoryForEachIteration + 1.0E8, // limitConditionNumberForWarning + true // conditionNumberWarningEachIteration + ); + std::cout<<"estimation input created"< > estimationInput = + // std::make_shared< EstimationInput< double, double > >( + // observationsAndTimes ); + std::shared_ptr< CovarianceAnalysisInput< double, double > > covarianceInput = + std::make_shared< CovarianceAnalysisInput< double, double > >( + observationsAndTimes ); + std::cout<<"covariance input created"< > estimationOutput = orbitDeterminationManager.estimateParameters( + estimationInput ); + std::cout<<"estimation performed"< finalParameters = parametersToEstimate->template getFullParameterValues< double >( ); + + // save true errors + Eigen::Matrix< double, Eigen::Dynamic, 1 > TrueError = ( estimationOutput->parameterEstimate_ - truthParameters ).transpose( ); + //TrueError = ( estimationOutput->parameterEstimate_ - truthParameters ).transpose( ); + std::cout<<"True error: "<<( estimationOutput->parameterEstimate_ - truthParameters ).transpose( )< FormalError = estimationOutput->getFormalErrorVector( ); + std::cout<<"Formal error: "<getFormalErrorVector( ).transpose( )<getFormalErrorVector( ).segment( 0, numberOfParameters ) ).cwiseQuotient( + estimationOutput->parameterEstimate_ - truthParameters ) ).transpose( )<resetParameterValues(estimationOutput->parameterHistory_.at(estimationOutput->bestIteration_)); + + std::vector< std::shared_ptr< propagators::SimulationResults< long double, Time > > > bestIterationSimulationResultsPerArc; + std::map< Time, Eigen::Matrix< long double, Eigen::Dynamic, 1 > > concatenatedStateHistoryPostFitDynamic; + std::map< Time, Eigen::Matrix< long double, 6, 1 > > concatenatedStateHistoryPostFit; + + for ( unsigned int arcIndex = 0; arcIndex < bestIterationSimulationResultsPerArc.size(); ++arcIndex ) + { + // Retrieve the best iteration simulation results for the current arc + std::shared_ptr< propagators::SimulationResults< long double, Time > > bestIterationSimulationResults = + bestIterationSimulationResultsPerArc[arcIndex]; + bestIterationSimulationResultsPerArc.push_back( bestIterationSimulationResults ); + + // Retrieve the state history for the current arc + std::map< Time, Eigen::Matrix< long double, Eigen::Dynamic, 1 > > arcStateHistoryPostFitDynamic = + std::dynamic_pointer_cast< SingleArcVariationalSimulationResults< long double, Time > >( + bestIterationSimulationResults )->getDynamicsResults( )->getEquationsOfMotionNumericalSolution(); + + // Append the state history to the concatenated map + for ( auto it = arcStateHistoryPostFitDynamic.begin(); it != arcStateHistoryPostFitDynamic.end(); ++it ) + { + concatenatedStateHistoryPostFitDynamic[ it->first ] = it->second; + } + } + +// Convert the dynamic state history to fixed size + for ( auto it = concatenatedStateHistoryPostFitDynamic.begin(); it != concatenatedStateHistoryPostFitDynamic.end(); ++it ) + { + concatenatedStateHistoryPostFit[ it->first ] = it->second; + } + + //parametersToEstimate->resetParameterValues( estimationOutput->parameterHistory_.at( estimationOutput->bestIteration_ ) ); + + std::shared_ptr< CovarianceAnalysisOutput< double, double > > covarianceOutput = orbitDeterminationManager.computeCovariance( + covarianceInput ); + + //compareEstimationAndCovarianceResults( estimationOutput, covarianceOutput ); + + + //return ( finalParameters - truthParameters ).template cast< double >( ); + + + + + // Create link ends + /*LinkEnds linkEnds; + linkEnds[ observed_body ] = spacecraftName; + + // Create observation model settings + std::vector< std::shared_ptr< observation_models::ObservationModelSettings > > observationModelSettingsList; + observationModelSettingsList.push_back( positionObservableSettings( linkEnds ) ); + + // Select parameters to estimate + std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames; + parameterNames.push_back( std::make_shared< InitialTranslationalStateEstimatableParameterSettings< long double > >( + spacecraftName, spacecraftInitialState, centralBody, baseFrameOrientation ) ); + //parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( spacecraftName, radiation_pressure_coefficient ) ); + //parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( spacecraftName, constant_drag_coefficient ) ); + + std::map > > cosineBlockIndicesPerPower; + cosineBlockIndicesPerPower[ 1 ].push_back( std::make_pair( 2, 0 ) ); + std::map > > sineBlockIndicesPerPower; + sineBlockIndicesPerPower[ 1 ].push_back( std::make_pair( 3, 2 ) ); + parameterNames.push_back( std::make_shared< PolynomialGravityFieldVariationEstimatableParameterSettings >( + "Mars", cosineBlockIndicesPerPower, sineBlockIndicesPerPower ) ); + + std::shared_ptr< estimatable_parameters::EstimatableParameterSet< long double > > parametersToEstimate = + createParametersToEstimate< long double, Time >( parameterNames, bodies ); + + // Create orbit determination object. + OrbitDeterminationManager< long double, Time > orbitDeterminationManager = + OrbitDeterminationManager< long double, Time >( + bodies, parametersToEstimate, + observationModelSettingsList, propagatorSettings, true ); + + // Retrieve state history + std::map< long double, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > propagatedStateHistory; + for ( Time t : observationTimes ) + { + propagatedStateHistory[ t.getSeconds< long double >() ] = + bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( t ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( t ); + } + + writeDataMapToTextFile( propagatedStateHistory, "stateHistoryPropagatedPreFit_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + writeDataMapToTextFile( spiceStateHistory, "stateHistorySpice_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + + std::vector< std::shared_ptr< SingleObservationSet< long double, Time > > > observationSetList; + observationSetList.push_back( + std::make_shared< SingleObservationSet< long double, Time > >( + position_observable, linkEnds, observations, observationTimes, observed_body ) ); + std::shared_ptr< ObservationCollection< long double, Time > > observedObservationCollection = + std::make_shared< ObservationCollection< long double, Time > >( observationSetList ); + + // Define estimation input + std::shared_ptr< EstimationInput< long double, Time > > estimationInput = + std::make_shared< EstimationInput< long double, Time > >( + observedObservationCollection, + Eigen::MatrixXd::Zero( 0, 0 ), + std::make_shared< EstimationConvergenceChecker >( 5 ) ); + estimationInput->saveStateHistoryForEachIteration_ = true; + + // Perform estimation + std::shared_ptr< EstimationOutput< long double, Time > > estimationOutput = orbitDeterminationManager.estimateParameters( + estimationInput ); + + // Retrieve post-fit state history + std::shared_ptr< propagators::SimulationResults< long double, Time > > postFitSimulationResults = + estimationOutput->getBestIterationSimulationResults( ); + std::map < Time, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > propagatedStateHistoryPostFitDynamic = + std::dynamic_pointer_cast< SingleArcVariationalSimulationResults< long double, Time > >( + postFitSimulationResults )->getDynamicsResults( )->getEquationsOfMotionNumericalSolution( ); + std::map < Time, Eigen::Matrix < long double, 6, 1 > > propagatedStateHistoryPostFit; + for ( auto it = propagatedStateHistoryPostFitDynamic.begin( ); it != propagatedStateHistoryPostFitDynamic.end( ); ++it ) + { + propagatedStateHistoryPostFit[ it->first ] = it->second; + } + std::shared_ptr< interpolators::OneDimensionalInterpolator< Time, Eigen::Matrix< long double, 6, 1 > > > postFitStateInterpolator = + propagators::createStateInterpolator< Time, long double >( propagatedStateHistoryPostFit ); + std::map< long double, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > propagatedStateHistoryPostFitToWrite; + for ( Time t : observationTimes ) + { + propagatedStateHistoryPostFitToWrite[ t.getSeconds< long double >() ] = postFitStateInterpolator->interpolate( t ); + } +// for ( auto it = propagatedStateHistoryPostFit.begin( ); it != propagatedStateHistoryPostFit.end( ); ++it ) +// { +// propagatedStateHistoryPostFitToWrite[ it->first.getSeconds< long double >() ] = it->second; +// } + writeDataMapToTextFile( propagatedStateHistoryPostFitToWrite, "stateHistoryPropagatedPostFit_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + + // Retrieve residuals and set them in matrix + Eigen::MatrixXd residualHistory = estimationOutput->getResidualHistoryMatrix( ); + Eigen::Matrix< long double, Eigen::Dynamic, Eigen::Dynamic > parameterHistory = estimationOutput->getParameterHistoryMatrix( ); + + Eigen::MatrixXd residualsWithTime; + residualsWithTime.resize( residualHistory.rows( ), residualHistory.cols( ) + 1 ); + residualsWithTime.rightCols( residualHistory.cols( ) ) = residualHistory; + + for ( unsigned int i = 0; i < observedObservationCollection->getObservationVector( ).size( ); ++i ) + { + residualsWithTime( i, 0 ) = static_cast< Time >( observedObservationCollection->getConcatenatedTimeVector( ).at( i ) + ).getSeconds< long double >(); + } + + std::ofstream file(saveDirectory + "residuals_" + fileTag + ".txt"); + file << std::setprecision( 17 ) << residualsWithTime; + file.close(); + + std::ofstream file3(saveDirectory + "parameters_" + fileTag + ".txt"); + file3 << std::setprecision( 21 ) << parameterHistory; + file3.close(); + + // Retrieve covariance matrix + Eigen::MatrixXd normalizedCovarianceMatrix = estimationOutput->getNormalizedCovarianceMatrix( ); + Eigen::MatrixXd unnormalizedCovarianceMatrix = estimationOutput->getUnnormalizedCovarianceMatrix( ); + std::ofstream file4(saveDirectory + "covariance_" + fileTag + ".txt"); + file4 << std::setprecision( 17 ) << "Normalized covariance matrix: " << std::endl << normalizedCovarianceMatrix; + file4 << std::endl << std::endl; + file4 << "Unnormalized covariance matrix: " << std::endl << unnormalizedCovarianceMatrix; + file4.close( ); + */ + +} + diff --git a/tests_riva/multiarc-trial2.cpp b/tests_riva/multiarc-trial2.cpp new file mode 100644 index 0000000000..2b89a5ce4e --- /dev/null +++ b/tests_riva/multiarc-trial2.cpp @@ -0,0 +1,470 @@ +// +// Created by Riva Alkahal on 11/08/2024. +// +// +// Created by Riva Alkahal on 26/07/2024. +// + +#include +#include +#include +#include "fstream" +#include "iostream" + +#include + +#include "tudat/basics/testMacros.h" +#include "tudat/astro/aerodynamics/customAerodynamicCoefficientInterface.h" +#include "tudat/astro/aerodynamics/aerodynamicAcceleration.h" +#include "tudat/astro/reference_frames/aerodynamicAngleCalculator.h" +#include "tudat/simulation/propagation_setup/dynamicsSimulator.h" +#include "tudat/interface/spice/spiceEphemeris.h" +#include "tudat/interface/spice/spiceRotationalEphemeris.h" +#include "tudat/io/basicInputOutput.h" +#include "tudat/simulation/environment_setup/body.h" +#include "tudat/simulation/estimation_setup/createNumericalSimulator.h" +#include "tudat/simulation/environment_setup/defaultBodies.h" +#include "tudat/simulation/environment_setup/body.h" +#include "tudat/astro/basic_astro/timeConversions.h" +#include "tudat/astro/aerodynamics/marsDtmAtmosphereModel.h" +#include "tudat/io/solarActivityData.h" +#include "tudat/astro/basic_astro/unitConversions.h" + +#include "tudat/basics/testMacros.h" +#include "tudat/simulation/estimation.h" +#include "tudat/simulation/estimation_setup.h" + +#include "tudat/io/readOdfFile.h" +#include "tudat/io/readTabulatedMediaCorrections.h" +#include "tudat/io/readTabulatedWeatherData.h" +#include "tudat/simulation/estimation_setup/processOdfFile.h" +#include "tudat/simulation/propagation_setup/setNumericallyIntegratedStates.h" + +#include + +#include "tudat/astro/ground_stations/transmittingFrequencies.h" + + +int main( ) { + using namespace tudat; + using namespace aerodynamics; + using namespace simulation_setup; + using namespace numerical_integrators; + using namespace simulation_setup; + using namespace basic_astrodynamics; + using namespace propagators; + using namespace estimatable_parameters; + using namespace observation_models; + using namespace basic_mathematics; + using namespace basic_astrodynamics; + using namespace tudat::spice_interface; + using namespace tudat::ephemerides; + using namespace tudat::input_output; + using namespace tudat::orbit_determination; + using namespace tudat::interpolators; + using namespace tudat::orbital_element_conversions; + + spice_interface::loadStandardSpiceKernels( ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map4.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map5.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map6.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map7.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map8.bsp" ); + + std::string saveDirectory = "/Users/ralkahal/OneDrive - Delft University of Technology/PhD/Programs/testingestimation/"; + std::string fileTag = "polygrav"; + + // set input options + double epehemeridesTimeStep = 60.0; + bool useInterpolatedEphemerides = true; + double observationsSamplingTime = 60.0; + double buffer = 10.0 * epehemeridesTimeStep; + //const double gravitationalParameter = 4.2828378e13; + //const double planetaryRadius = 3389.5E3; + + // Select ephemeris time range (based on available data in loaded SPICE ephemeris) + //Time initialEphemerisTime = Time( 185976000 - 1.0 * 86400.0 ); // 23 November 2005, 0h + //Time finalEphemerisTime = Time( 186580800 + 1.0 * 86400.0 ); // 30 November 2005, 0h + // time at 2000-01-01T00:00:00 + Time initialEphemerisTime = Time( 0.0 ) ; + Time finalEphemerisTime = Time( 86400.0 * 6.0 ); // 5 years later + double totalDuration = finalEphemerisTime - initialEphemerisTime; + double hoursperday = 10.0; + + // Define bodies to use. + //std::vector< std::string > bodiesToCreate = { + // "Earth", "Sun", "Mercury", "Venus", "Mars", "Jupiter", "Saturn", "Uranus", "Neptune", "Phobos", "Deimos", + // "Io", "Ganymede", "Callisto", "Europa", "Titan" }; + std::vector< std::string > bodiesToCreate = { + "Earth", "Sun", "Mercury", "Venus", "Mars", "Jupiter", "Phobos", "Deimos" }; + + std::string baseFrameOrientation = "MARSIAU"; + std::string baseFrameOrigin = "SSB"; + + // Specify ephemeris time steps and buffers + Time ephemerisTimeStepPlanets = epehemeridesTimeStep; + std::cout<<"Ephemeris time step planets: "<groundStationSettings = getDsnStationSettings( ); + + std::string filename ="/Users/ralkahal/OneDrive - Delft University of Technology/PhD/Programs/atmodensitydtm/dtm_mars";; + bodySettings.at( "Mars" )->atmosphereSettings = marsDtmAtmosphereSettings( filename, 3378.0E3); + + // Create spacecraft + std::string spacecraftName = "MGS"; + bodySettings.addSettings( spacecraftName ); + if ( useInterpolatedEphemerides ) + { + bodySettings.at( spacecraftName )->ephemerisSettings = + std::make_shared< InterpolatedSpiceEphemerisSettings >( + initialEphemerisTime - buffer, finalEphemerisTime + buffer, + ephemerisTimeStepSpacecraft, baseFrameOrigin, baseFrameOrientation, + std::make_shared< interpolators::LagrangeInterpolatorSettings >( 8 ), spacecraftName ); + } + else + { + bodySettings.at( spacecraftName )->ephemerisSettings = + std::make_shared< DirectSpiceEphemerisSettings >( baseFrameOrigin, baseFrameOrientation ); + } + bodySettings.at( spacecraftName )->constantMass = 700.0; + + // Create radiation pressure settings + double referenceAreaRadiation = 20.0; + double radiationPressureCoefficient = 1.0; + std::vector< std::string > occultingBodies = { "Mars" }; + bodySettings.at( spacecraftName )->radiationPressureSettings[ "Sun" ] = + cannonBallRadiationPressureSettings( "Sun", referenceAreaRadiation, radiationPressureCoefficient, occultingBodies ); + + std::shared_ptr aerodynamicCoefficientSettings = + std::make_shared( + 2.0, 4.0, Eigen::Vector3d::Zero( ), Eigen::Vector3d::UnitX( ), Eigen::Vector3d::Zero( ), + negative_aerodynamic_frame_coefficients, negative_aerodynamic_frame_coefficients ); + bodySettings.at( spacecraftName )->aerodynamicCoefficientSettings = aerodynamicCoefficientSettings; + bodySettings.at( spacecraftName )->ephemerisSettings->resetMakeMultiArcEphemeris( true ); + + std::vector< std::shared_ptr< GravityFieldVariationSettings > > gravityFieldVariations; + + std::map cosineAmplitudes; + cosineAmplitudes[ 1 ] = Eigen::Matrix< double, 3, 5 >::Zero( ); + //cosineAmplitudes[ 2 ] = Eigen::Matrix< double, 3, 5 >::Zero( ); + std::map sineAmplitudes; + sineAmplitudes[ 1 ] = Eigen::Matrix< double, 3, 5 >::Zero( ); + std::shared_ptr< GravityFieldVariationSettings > polynomialGravityFieldVariations = + std::make_shared< PolynomialGravityFieldVariationsSettings >( + cosineAmplitudes, sineAmplitudes, -1.0E4, 2, 0 ); + gravityFieldVariations.push_back( polynomialGravityFieldVariations ); + + std::vector< std::shared_ptr< GravityFieldVariationSettings > > gravityFieldVariationSettings = + gravityFieldVariations; + bodySettings.at( "Mars" )->gravityFieldVariationSettings = gravityFieldVariations; + + SystemOfBodies bodies = createSystemOfBodies< long double, Time >( bodySettings ); + + //Time initialPropagationTime = initialEphemerisTime + 600.0; + //Time finalPropagationTime = finalEphemerisTime - 600.0; + + + + // Set accelerations on Vehicle that are to be taken into account. + SelectedAccelerationMap accelerationMap; + std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle; + accelerationsOfVehicle[ "Sun" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Sun" ].push_back( cannonBallRadiationPressureAcceleration( ) ); +// accelerationsOfVehicle[ "Sun" ].push_back( relativisticAccelerationCorrection( ) ); + accelerationsOfVehicle[ "Mercury" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Venus" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Earth" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Mars" ].push_back( sphericalHarmonicAcceleration( 50, 50 ) ); + accelerationsOfVehicle[ "Mars" ].push_back( relativisticAccelerationCorrection( ) ); + accelerationsOfVehicle[ "Mars" ].push_back( aerodynamicAcceleration( ) ); + accelerationsOfVehicle[ "Phobos" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Deimos" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Jupiter" ].push_back( pointMassGravityAcceleration( ) ); + + accelerationMap[ spacecraftName ] = accelerationsOfVehicle; + + // Set bodies for which initial state is to be estimated and integrated. + std::vector< std::string > bodiesToIntegrate; + std::string centralBody = "Mars"; + std::vector< std::string > centralBodies = { centralBody }; + bodiesToIntegrate.push_back( spacecraftName ); + + // Create acceleration models + AccelerationMap accelerationModelMap = createAccelerationModelsMap( bodies, accelerationMap, bodiesToIntegrate, centralBodies ); + std::cout<<"acceleration map created"< integrationArcStartTimes; + std::vector< double > integrationArcEndTimes; + std::vector< double > integrationArcLimits; + + double integrationStartTime = initialEphemerisTime + 120.0; //1.0E2; + double integrationEndTime = finalEphemerisTime - 120.0 ; //1.0E2; + double totalDurationIntegration = integrationEndTime - integrationStartTime; + double arcDuration = 86400.0;//2.0E4; + double arcOverlap = 60.0;//2.0E2; + + double currentStartTime = integrationStartTime, currentEndTime = integrationStartTime + arcDuration; + + do + { + integrationArcLimits.push_back( currentStartTime ); + integrationArcEndTimes.push_back( currentEndTime ); + std::cout<<"integration arc start times: "< observationTimes; + std::vector< Eigen::Matrix< long double, Eigen::Dynamic, 1 > > observations; + for ( Time t = integrationStartTime + 600; t < integrationEndTime - 600; t += observationsSamplingTime ) + { + try + { + observations.push_back( bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( t ).segment( 0, 3 ) ); + observationTimes.push_back( t ); + } + catch( ... ) + { } + } + // Retrieve state history from SPICE + std::map< long double, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > spiceStateHistory; + for ( Time t : observationTimes ) + { + spiceStateHistory[ t.getSeconds< long double >() ] = + bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( t ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( t ); + } + writeDataMapToTextFile( spiceStateHistory, "stateHistorySpice_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + + // Define integrator settings. + //std::shared_ptr< IntegratorSettings< double > > integratorSettings = + // rungeKutta4Settings< double >( 60.0 ); + std::shared_ptr< IntegratorSettings< > > integratorSettings = + std::make_shared< IntegratorSettings< > > + ( rungeKutta4, integrationArcStartTimes.at(1), 60.0 ); + + std::cout<<"Integration settings created"< integrationMinMaxStep = std::make_pair( 1e-16, 1e16 ); + + double initialStep = 5.0; + if ( integrationMinMaxStep.first == integrationMinMaxStep.second ) + { + initialStep = integrationMinMaxStep.first; + } + std::shared_ptr< IntegratorSettings< Time > > integratorSettings = rungeKuttaVariableStepSettingsScalarTolerances< Time >( + initialStep, rungeKutta87DormandPrince, integrationMinMaxStep.first, + integrationMinMaxStep.second, 1e-10, 1e-10 );*/ + + /*std::shared_ptr< IntegratorSettings< Time > > integratorSettings = + rungeKutta4Settings< Time >( 60.0 );*/ + // Set initial state from ephemerides + /* Eigen::Matrix< long double, 6, 1 > spacecraftInitialState = + bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( initialPropagationTime ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( initialPropagationTime ); +*/ + + + int numberOfIntegrationArcs = integrationArcStartTimes.size( ); + for( unsigned int i = 0; i < numberOfIntegrationArcs; i++ ) { + std::cout << "integration arc start times"< systemInitialStates(numberOfIntegrationArcs, Eigen::VectorXd(6)); + + std::vector< std::shared_ptr< SingleArcPropagatorSettings< double > > > arcPropagationSettingsList; + for( unsigned int i = 0; i < numberOfIntegrationArcs; i++ ) + { + std::cout<<"iteration started"< > + ( centralBodies, accelerationModelMap, bodiesToIntegrate, + systemInitialStates.at( i ), integrationArcEndTimes.at( i ) ) ); + } + + std::cout<<"single arc propagation done"< > multiArcPropagatorSettings = + validateDeprecatedMultiArcSettings< double, double >( + integratorSettings, std::make_shared< MultiArcPropagatorSettings< double > >( arcPropagationSettingsList ), + integrationArcStartTimes, true, true ); + + + //std::shared_ptr< PropagationPrintSettings > multiArcPrintSettings = + // std::make_shared< PropagationPrintSettings >( ); + //multiArcPrintSettings->reset( + // true, true, TUDAT_NAN, 0, true, true, true, true, true, true ); + + // multiArcPropagatorSettings->getOutputSettings( )->resetAndApplyConsistentSingleArcPrintSettings( + // multiArcPrintSettings ); + + MultiArcDynamicsSimulator< > dynamicsSimulator( + bodies, multiArcPropagatorSettings ); + + std::map< long double, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > propagatedStateHistory; + for ( Time t : observationTimes ) + { + propagatedStateHistory[ t.getSeconds< long double >() ] = + bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( t ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( t ); + } + writeDataMapToTextFile( propagatedStateHistory, "stateHistoryPropagation_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + + + + // Create link ends + /*LinkEnds linkEnds; + linkEnds[ observed_body ] = spacecraftName; + + // Create observation model settings + std::vector< std::shared_ptr< observation_models::ObservationModelSettings > > observationModelSettingsList; + observationModelSettingsList.push_back( positionObservableSettings( linkEnds ) ); + + // Select parameters to estimate + std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames; + parameterNames.push_back( std::make_shared< InitialTranslationalStateEstimatableParameterSettings< long double > >( + spacecraftName, spacecraftInitialState, centralBody, baseFrameOrientation ) ); + //parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( spacecraftName, radiation_pressure_coefficient ) ); + //parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( spacecraftName, constant_drag_coefficient ) ); + + std::map > > cosineBlockIndicesPerPower; + cosineBlockIndicesPerPower[ 1 ].push_back( std::make_pair( 2, 0 ) ); + std::map > > sineBlockIndicesPerPower; + sineBlockIndicesPerPower[ 1 ].push_back( std::make_pair( 3, 2 ) ); + parameterNames.push_back( std::make_shared< PolynomialGravityFieldVariationEstimatableParameterSettings >( + "Mars", cosineBlockIndicesPerPower, sineBlockIndicesPerPower ) ); + + std::shared_ptr< estimatable_parameters::EstimatableParameterSet< long double > > parametersToEstimate = + createParametersToEstimate< long double, Time >( parameterNames, bodies ); + + // Create orbit determination object. + OrbitDeterminationManager< long double, Time > orbitDeterminationManager = + OrbitDeterminationManager< long double, Time >( + bodies, parametersToEstimate, + observationModelSettingsList, propagatorSettings, true ); + + // Retrieve state history + std::map< long double, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > propagatedStateHistory; + for ( Time t : observationTimes ) + { + propagatedStateHistory[ t.getSeconds< long double >() ] = + bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( t ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( t ); + } + + writeDataMapToTextFile( propagatedStateHistory, "stateHistoryPropagatedPreFit_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + writeDataMapToTextFile( spiceStateHistory, "stateHistorySpice_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + + std::vector< std::shared_ptr< SingleObservationSet< long double, Time > > > observationSetList; + observationSetList.push_back( + std::make_shared< SingleObservationSet< long double, Time > >( + position_observable, linkEnds, observations, observationTimes, observed_body ) ); + std::shared_ptr< ObservationCollection< long double, Time > > observedObservationCollection = + std::make_shared< ObservationCollection< long double, Time > >( observationSetList ); + + // Define estimation input + std::shared_ptr< EstimationInput< long double, Time > > estimationInput = + std::make_shared< EstimationInput< long double, Time > >( + observedObservationCollection, + Eigen::MatrixXd::Zero( 0, 0 ), + std::make_shared< EstimationConvergenceChecker >( 5 ) ); + estimationInput->saveStateHistoryForEachIteration_ = true; + + // Perform estimation + std::shared_ptr< EstimationOutput< long double, Time > > estimationOutput = orbitDeterminationManager.estimateParameters( + estimationInput ); + + // Retrieve post-fit state history + std::shared_ptr< propagators::SimulationResults< long double, Time > > postFitSimulationResults = + estimationOutput->getBestIterationSimulationResults( ); + std::map < Time, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > propagatedStateHistoryPostFitDynamic = + std::dynamic_pointer_cast< SingleArcVariationalSimulationResults< long double, Time > >( + postFitSimulationResults )->getDynamicsResults( )->getEquationsOfMotionNumericalSolution( ); + std::map < Time, Eigen::Matrix < long double, 6, 1 > > propagatedStateHistoryPostFit; + for ( auto it = propagatedStateHistoryPostFitDynamic.begin( ); it != propagatedStateHistoryPostFitDynamic.end( ); ++it ) + { + propagatedStateHistoryPostFit[ it->first ] = it->second; + } + std::shared_ptr< interpolators::OneDimensionalInterpolator< Time, Eigen::Matrix< long double, 6, 1 > > > postFitStateInterpolator = + propagators::createStateInterpolator< Time, long double >( propagatedStateHistoryPostFit ); + std::map< long double, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > propagatedStateHistoryPostFitToWrite; + for ( Time t : observationTimes ) + { + propagatedStateHistoryPostFitToWrite[ t.getSeconds< long double >() ] = postFitStateInterpolator->interpolate( t ); + } +// for ( auto it = propagatedStateHistoryPostFit.begin( ); it != propagatedStateHistoryPostFit.end( ); ++it ) +// { +// propagatedStateHistoryPostFitToWrite[ it->first.getSeconds< long double >() ] = it->second; +// } + writeDataMapToTextFile( propagatedStateHistoryPostFitToWrite, "stateHistoryPropagatedPostFit_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + + // Retrieve residuals and set them in matrix + Eigen::MatrixXd residualHistory = estimationOutput->getResidualHistoryMatrix( ); + Eigen::Matrix< long double, Eigen::Dynamic, Eigen::Dynamic > parameterHistory = estimationOutput->getParameterHistoryMatrix( ); + + Eigen::MatrixXd residualsWithTime; + residualsWithTime.resize( residualHistory.rows( ), residualHistory.cols( ) + 1 ); + residualsWithTime.rightCols( residualHistory.cols( ) ) = residualHistory; + + for ( unsigned int i = 0; i < observedObservationCollection->getObservationVector( ).size( ); ++i ) + { + residualsWithTime( i, 0 ) = static_cast< Time >( observedObservationCollection->getConcatenatedTimeVector( ).at( i ) + ).getSeconds< long double >(); + } + + std::ofstream file(saveDirectory + "residuals_" + fileTag + ".txt"); + file << std::setprecision( 17 ) << residualsWithTime; + file.close(); + + std::ofstream file3(saveDirectory + "parameters_" + fileTag + ".txt"); + file3 << std::setprecision( 21 ) << parameterHistory; + file3.close(); + + // Retrieve covariance matrix + Eigen::MatrixXd normalizedCovarianceMatrix = estimationOutput->getNormalizedCovarianceMatrix( ); + Eigen::MatrixXd unnormalizedCovarianceMatrix = estimationOutput->getUnnormalizedCovarianceMatrix( ); + std::ofstream file4(saveDirectory + "covariance_" + fileTag + ".txt"); + file4 << std::setprecision( 17 ) << "Normalized covariance matrix: " << std::endl << normalizedCovarianceMatrix; + file4 << std::endl << std::endl; + file4 << "Unnormalized covariance matrix: " << std::endl << unnormalizedCovarianceMatrix; + file4.close( ); + */ + +} + diff --git a/tests_riva/multiarc-trial3.cpp b/tests_riva/multiarc-trial3.cpp new file mode 100644 index 0000000000..07333d2671 --- /dev/null +++ b/tests_riva/multiarc-trial3.cpp @@ -0,0 +1,679 @@ +// +// Created by Riva Alkahal on 11/08/2024. +// +// +// Created by Riva Alkahal on 26/07/2024. +// + +#include +#include +#include +#include "fstream" +#include "iostream" + +#include + +#include "tudat/basics/testMacros.h" +#include "tudat/astro/aerodynamics/customAerodynamicCoefficientInterface.h" +#include "tudat/astro/aerodynamics/aerodynamicAcceleration.h" +#include "tudat/astro/reference_frames/aerodynamicAngleCalculator.h" +#include "tudat/simulation/propagation_setup/dynamicsSimulator.h" +#include "tudat/interface/spice/spiceEphemeris.h" +#include "tudat/interface/spice/spiceRotationalEphemeris.h" +#include "tudat/io/basicInputOutput.h" +#include "tudat/simulation/environment_setup/body.h" +#include "tudat/simulation/estimation_setup/createNumericalSimulator.h" +#include "tudat/simulation/environment_setup/defaultBodies.h" +#include "tudat/simulation/environment_setup/body.h" +#include "tudat/astro/basic_astro/timeConversions.h" +#include "tudat/astro/aerodynamics/marsDtmAtmosphereModel.h" +#include "tudat/io/solarActivityData.h" +#include "tudat/astro/basic_astro/unitConversions.h" + +#include "tudat/basics/testMacros.h" +#include "tudat/simulation/estimation.h" +#include "tudat/simulation/estimation_setup.h" + +#include "tudat/io/readOdfFile.h" +#include "tudat/io/readTabulatedMediaCorrections.h" +#include "tudat/io/readTabulatedWeatherData.h" +#include "tudat/simulation/estimation_setup/processOdfFile.h" +#include "tudat/simulation/propagation_setup/setNumericallyIntegratedStates.h" + +#include + +#include "tudat/astro/ground_stations/transmittingFrequencies.h" + + +int main( ) { + using namespace tudat; + using namespace aerodynamics; + using namespace simulation_setup; + using namespace numerical_integrators; + using namespace simulation_setup; + using namespace basic_astrodynamics; + using namespace propagators; + using namespace estimatable_parameters; + using namespace observation_models; + using namespace basic_mathematics; + using namespace basic_astrodynamics; + using namespace tudat::spice_interface; + using namespace tudat::ephemerides; + using namespace tudat::input_output; + using namespace tudat::orbit_determination; + using namespace tudat::interpolators; + using namespace tudat::orbital_element_conversions; + + spice_interface::loadStandardSpiceKernels( ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map4.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map5.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map6.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map7.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map8.bsp" ); + + std::string saveDirectory = "/Users/ralkahal/OneDrive - Delft University of Technology/PhD/Programs/testingestimation/"; + std::string fileTag = "polygrav"; + + // set input options + double epehemeridesTimeStep = 60.0; + bool useInterpolatedEphemerides = true; + double observationsSamplingTime = 60.0; + double buffer = 10.0 * epehemeridesTimeStep; + //const double gravitationalParameter = 4.2828378e13; + //const double planetaryRadius = 3389.5E3; + + // Select ephemeris time range (based on available data in loaded SPICE ephemeris) + //Time initialEphemerisTime = Time( 185976000 - 1.0 * 86400.0 ); // 23 November 2005, 0h + //Time finalEphemerisTime = Time( 186580800 + 1.0 * 86400.0 ); // 30 November 2005, 0h + // time at 2000-01-01T00:00:00 + Time initialEphemerisTime = Time( 0.0 ) ; + Time finalEphemerisTime = Time( 86400.0 * 6.0 ); // 5 years later + double totalDuration = finalEphemerisTime - initialEphemerisTime; + double hoursperday = 10.0; + + // Define bodies to use. + //std::vector< std::string > bodiesToCreate = { + // "Earth", "Sun", "Mercury", "Venus", "Mars", "Jupiter", "Saturn", "Uranus", "Neptune", "Phobos", "Deimos", + // "Io", "Ganymede", "Callisto", "Europa", "Titan" }; + std::vector< std::string > bodiesToCreate = { + "Earth", "Sun", "Mercury", "Venus", "Mars", "Jupiter", "Phobos", "Deimos" }; + + std::string baseFrameOrientation = "MARSIAU"; + std::string baseFrameOrigin = "SSB"; + + // Specify ephemeris time steps and buffers + Time ephemerisTimeStepPlanets = epehemeridesTimeStep; + std::cout<<"Ephemeris time step planets: "<groundStationSettings = getDsnStationSettings( ); + + std::string filename ="/Users/ralkahal/OneDrive - Delft University of Technology/PhD/Programs/atmodensitydtm/dtm_mars";; + bodySettings.at( "Mars" )->atmosphereSettings = marsDtmAtmosphereSettings( filename, 3378.0E3); + + // Create spacecraft + std::string spacecraftName = "MGS"; + bodySettings.addSettings( spacecraftName ); + if ( useInterpolatedEphemerides ) + { + bodySettings.at( spacecraftName )->ephemerisSettings = + std::make_shared< InterpolatedSpiceEphemerisSettings >( + initialEphemerisTime - buffer, finalEphemerisTime + buffer, + ephemerisTimeStepSpacecraft, baseFrameOrigin, baseFrameOrientation, + std::make_shared< interpolators::LagrangeInterpolatorSettings >( 8 ), spacecraftName ); + } + else + { + bodySettings.at( spacecraftName )->ephemerisSettings = + std::make_shared< DirectSpiceEphemerisSettings >( baseFrameOrigin, baseFrameOrientation ); + } + bodySettings.at( spacecraftName )->constantMass = 700.0; + + // Create radiation pressure settings + double referenceAreaRadiation = 20.0; + double radiationPressureCoefficient = 1.0; + std::vector< std::string > occultingBodies = { "Mars" }; + bodySettings.at( spacecraftName )->radiationPressureSettings[ "Sun" ] = + cannonBallRadiationPressureSettings( "Sun", referenceAreaRadiation, radiationPressureCoefficient, occultingBodies ); + + std::shared_ptr aerodynamicCoefficientSettings = + std::make_shared( + 2.0, 4.0, Eigen::Vector3d::Zero( ), Eigen::Vector3d::UnitX( ), Eigen::Vector3d::Zero( ), + negative_aerodynamic_frame_coefficients, negative_aerodynamic_frame_coefficients ); + bodySettings.at( spacecraftName )->aerodynamicCoefficientSettings = aerodynamicCoefficientSettings; + bodySettings.at( spacecraftName )->ephemerisSettings->resetMakeMultiArcEphemeris( true ); + + std::vector< std::shared_ptr< GravityFieldVariationSettings > > gravityFieldVariations; + + std::map cosineAmplitudes; + cosineAmplitudes[ 1 ] = Eigen::Matrix< double, 3, 5 >::Zero( ); + //cosineAmplitudes[ 2 ] = Eigen::Matrix< double, 3, 5 >::Zero( ); + std::map sineAmplitudes; + sineAmplitudes[ 1 ] = Eigen::Matrix< double, 3, 5 >::Zero( ); + std::shared_ptr< GravityFieldVariationSettings > polynomialGravityFieldVariations = + std::make_shared< PolynomialGravityFieldVariationsSettings >( + cosineAmplitudes, sineAmplitudes, -1.0E4, 2, 0 ); + gravityFieldVariations.push_back( polynomialGravityFieldVariations ); + + std::vector< std::shared_ptr< GravityFieldVariationSettings > > gravityFieldVariationSettings = + gravityFieldVariations; + bodySettings.at( "Mars" )->gravityFieldVariationSettings = gravityFieldVariations; + + SystemOfBodies bodies = createSystemOfBodies< long double, Time >( bodySettings ); + + //Time initialPropagationTime = initialEphemerisTime + 600.0; + //Time finalPropagationTime = finalEphemerisTime - 600.0; + + + + // Set accelerations on Vehicle that are to be taken into account. + SelectedAccelerationMap accelerationMap; + std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle; + accelerationsOfVehicle[ "Sun" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Sun" ].push_back( cannonBallRadiationPressureAcceleration( ) ); +// accelerationsOfVehicle[ "Sun" ].push_back( relativisticAccelerationCorrection( ) ); + accelerationsOfVehicle[ "Mercury" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Venus" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Earth" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Mars" ].push_back( sphericalHarmonicAcceleration( 50, 50 ) ); + accelerationsOfVehicle[ "Mars" ].push_back( relativisticAccelerationCorrection( ) ); + accelerationsOfVehicle[ "Mars" ].push_back( aerodynamicAcceleration( ) ); + accelerationsOfVehicle[ "Phobos" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Deimos" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Jupiter" ].push_back( pointMassGravityAcceleration( ) ); + + accelerationMap[ spacecraftName ] = accelerationsOfVehicle; + + // Set bodies for which initial state is to be estimated and integrated. + std::vector< std::string > bodiesToIntegrate; + std::string centralBody = "Mars"; + std::vector< std::string > centralBodies = { centralBody }; + bodiesToIntegrate.push_back( spacecraftName ); + + // Create acceleration models + AccelerationMap accelerationModelMap = createAccelerationModelsMap( bodies, accelerationMap, bodiesToIntegrate, centralBodies ); + std::cout<<"acceleration map created"< integrationArcStartTimes; + std::vector< double > integrationArcEndTimes; + std::vector< double > integrationArcLimits; + + double integrationStartTime = initialEphemerisTime + 120.0; //1.0E2; + double integrationEndTime = finalEphemerisTime - 120.0 ; //1.0E2; + double totalDurationIntegration = integrationEndTime - integrationStartTime; + double arcDuration = 86400.0;//2.0E4; + double arcOverlap = 60.0;//2.0E2; + + double currentStartTime = integrationStartTime, currentEndTime = integrationStartTime + arcDuration; + + do + { + integrationArcLimits.push_back( currentStartTime ); + integrationArcEndTimes.push_back( currentEndTime ); + std::cout<<"integration arc start times: "< observationTimes; + std::vector< Eigen::Matrix< long double, Eigen::Dynamic, 1 > > observations; + for ( Time t = integrationStartTime + 600; t < integrationEndTime - 600; t += observationsSamplingTime ) + { + try + { + observations.push_back( bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( t ).segment( 0, 3 ) ); + observationTimes.push_back( t ); + } + catch( ... ) + { } + } + // Retrieve state history from SPICE + std::map< long double, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > spiceStateHistory; + for ( Time t : observationTimes ) + { + spiceStateHistory[ t.getSeconds< long double >() ] = + bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( t ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( t ); + } + writeDataMapToTextFile( spiceStateHistory, "stateHistorySpice_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + + // Define integrator settings. + //std::shared_ptr< IntegratorSettings< double > > integratorSettings = + // rungeKutta4Settings< double >( 60.0 ); + std::shared_ptr< IntegratorSettings< > > integratorSettings = + std::make_shared< IntegratorSettings< > > + ( rungeKutta4, integrationArcStartTimes.at(0), 60.0 ); + + std::cout<<"Integration settings created"< integrationMinMaxStep = std::make_pair( 1e-16, 1e16 ); + + double initialStep = 5.0; + if ( integrationMinMaxStep.first == integrationMinMaxStep.second ) + { + initialStep = integrationMinMaxStep.first; + } + std::shared_ptr< IntegratorSettings< Time > > integratorSettings = rungeKuttaVariableStepSettingsScalarTolerances< Time >( + initialStep, rungeKutta87DormandPrince, integrationMinMaxStep.first, + integrationMinMaxStep.second, 1e-10, 1e-10 );*/ + + /*std::shared_ptr< IntegratorSettings< Time > > integratorSettings = + rungeKutta4Settings< Time >( 60.0 );*/ + // Set initial state from ephemerides + /* Eigen::Matrix< long double, 6, 1 > spacecraftInitialState = + bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( initialPropagationTime ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( initialPropagationTime ); + */ + + + int numberOfIntegrationArcs = integrationArcStartTimes.size( ); + for( unsigned int i = 0; i < numberOfIntegrationArcs; i++ ) { + std::cout << "integration arc start times"< systemInitialStates(numberOfIntegrationArcs, Eigen::VectorXd(6)); + + std::vector< std::shared_ptr< SingleArcPropagatorSettings< double > > > arcPropagationSettingsList; + for( unsigned int i = 0; i < numberOfIntegrationArcs; i++ ) + { + std::cout<<"iteration started"< > + ( centralBodies, accelerationModelMap, bodiesToIntegrate, + systemInitialStates.at( i ), integrationArcEndTimes.at( i ) ) ); + } + + std::cout<<"single arc propagation done"< > multiArcPropagatorSettings = + validateDeprecatedMultiArcSettings< double, double >( + integratorSettings, std::make_shared< MultiArcPropagatorSettings< double > >( arcPropagationSettingsList ), + integrationArcStartTimes, false, true ); + + + //std::shared_ptr< PropagationPrintSettings > multiArcPrintSettings = + // std::make_shared< PropagationPrintSettings >( ); + //multiArcPrintSettings->reset( + // true, true, TUDAT_NAN, 0, true, true, true, true, true, true ); + + // multiArcPropagatorSettings->getOutputSettings( )->resetAndApplyConsistentSingleArcPrintSettings( + // multiArcPrintSettings ); + + MultiArcDynamicsSimulator< > dynamicsSimulator( + bodies, multiArcPropagatorSettings ); + + std::map< long double, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > propagatedStateHistory; + for ( Time t : observationTimes ) + { + propagatedStateHistory[ t.getSeconds< long double >() ] = + bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( t ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( t ); + } + writeDataMapToTextFile( propagatedStateHistory, "stateHistoryPropagation_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + + + //initial state? + std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames = + getInitialMultiArcParameterSettings< double, double >( multiArcPropagatorSettings, bodies, integrationArcStartTimes ); + //polynomial gravity field + std::map > > cosineBlockIndicesPerPower; + cosineBlockIndicesPerPower[ 1 ].push_back( std::make_pair( 2, 0 ) ); + std::map > > sineBlockIndicesPerPower; + sineBlockIndicesPerPower[ 1 ].push_back( std::make_pair( 3, 2 ) ); + parameterNames.push_back( std::make_shared< PolynomialGravityFieldVariationEstimatableParameterSettings >( + "Mars", cosineBlockIndicesPerPower, sineBlockIndicesPerPower ) ); + + std::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parametersToEstimate = + createParametersToEstimate< double, double >( parameterNames, bodies ); + + std::cout<<"parameters to estimate created"<(totalDurationIntegration / physical_constants::JULIAN_DAY ); + + std::vector initial_times_list = { integrationArcStartTimes[0] + buffer}; + for (int day = 1; day < days; ++day) { + initial_times_list.push_back(integrationArcStartTimes[0] + buffer + day * 24 * 3600); + } + std::vector final_times_list = { integrationArcStartTimes[0] + buffer + hoursperday * 3600 }; + for (int day = 1; day < days; ++day) { + final_times_list.push_back(integrationArcStartTimes[0] + buffer + day * 24 * 3600 + hoursperday * 3600); + } + std::vector observationTimesList; + for (int i = 0; i < days; ++i) { + double start = initial_times_list[i]; + std::cout<<"start time: "< stationTransmitterLinkEnds; + //linkEnds[ receiver ] = spacecraftName; + std::vector< std::string > GroundStations = { "DSS-26" , "DSS-42", "DSS-61"}; + for ( std::string groundStation : GroundStations ) + { + linkEnds[ transmitter ] = LinkEndId( "Earth", groundStation ); + linkEnds[ receiver ] = spacecraftName; + stationTransmitterLinkEnds.push_back( linkEnds ); + } + + std::cout<<"link ends created"< > observationSettingsList; + observationSettingsList.push_back( std::make_shared< ObservationModelSettings >( + one_way_range, linkEnds ) ); + std::cout<<"observation settings created"< orbitDeterminationManager = + OrbitDeterminationManager< double, double >( + bodies, parametersToEstimate, + observationSettingsList, multiArcPropagatorSettings ); + std::cout<<"orbit determination manager created"< > observationViabilitySettings; + for ( std::string groundStation : GroundStations ) + { + observationViabilitySettings.push_back( std::make_shared< ObservationViabilitySettings >( + minimum_elevation_angle, std::make_pair( "Earth", groundStation ), "", + unit_conversions::convertDegreesToRadians( 15.0 ) ) ); + observationViabilitySettings.push_back( std::make_shared( + body_occultation, + std::make_pair(groundStation, spacecraftName), + "Mars", + TUDAT_NAN + ); + } + */ + //std::cout<<"Observation viability settings created"< initialParameterEstimate = + parametersToEstimate->template getFullParameterValues< double >( ); + + // Create observation model settings + //std::vector< std::shared_ptr< observation_models::ObservationModelSettings > > observationModelSettingsList; + //observationModelSettingsList.push_back( positionObservableSettings( linkEnds ) ); + + + // Observation simulation settings + std::vector< std::shared_ptr< ObservationSimulationSettings< double > > > measurementSimulationInput; + measurementSimulationInput.push_back( + std::make_shared< TabulatedObservationSimulationSettings< double > >( + one_way_range, linkEnds , observationTimesList, receiver ) ); + + std::shared_ptr< ObservationCollection< double, double > > observationsAndTimes = simulateObservations< double, double >( + measurementSimulationInput, orbitDeterminationManager.getObservationSimulators( ), bodies ); + std::cout<<"observations and times created"< truthParameters = initialParameterEstimate; + int numberOfParameters = initialParameterEstimate.rows( ); + // Perturb initial states + for( unsigned int i = 0; i < integrationArcStartTimes.size( ); i++ ) + { + initialParameterEstimate[ 0 + 6 * i ] += 1.0E0; + initialParameterEstimate[ 1 + 6 * i ] += 1.0E0; + initialParameterEstimate[ 2 + 6 * i ] += 1.0E0; + initialParameterEstimate[ 3 + 6 * i ] += 1.0E-5; + initialParameterEstimate[ 4 + 6 * i ] += 1.0E-5; + initialParameterEstimate[ 5 + 6 * i ] += 1.0E-5; + } + std::cout<<"initial parameters perturbed"<( initialParameterEstimate.rows( ) ); i++ ) + { + initialParameterEstimate[ i ] *= ( 1.0 + 1.0E-6 ); + } +*/ + parametersToEstimate->resetParameterValues( initialParameterEstimate ); + + // Define estimation input + std::shared_ptr< EstimationInput< double, double > > estimationInput = + std::make_shared< EstimationInput< double, double > >( + observationsAndTimes, + Eigen::MatrixXd::Zero( 0, 0 ), + std::make_shared< EstimationConvergenceChecker >( 5 ) ); + estimationInput->saveStateHistoryForEachIteration_ = true; + std::cout<<"estimation input created"< > estimationInput = + // std::make_shared< EstimationInput< double, double > >( + // observationsAndTimes ); + std::shared_ptr< CovarianceAnalysisInput< double, double > > covarianceInput = + std::make_shared< CovarianceAnalysisInput< double, double > >( + observationsAndTimes ); + std::cout<<"covariance input created"< > estimationOutput = orbitDeterminationManager.estimateParameters( + estimationInput ); + std::cout<<"estimation performed"< finalParameters = parametersToEstimate->template getFullParameterValues< double >( ); + + // save true errors + Eigen::Matrix< double, Eigen::Dynamic, 1 > TrueError = ( estimationOutput->parameterEstimate_ - truthParameters ).transpose( ); + //TrueError = ( estimationOutput->parameterEstimate_ - truthParameters ).transpose( ); + std::cout<<"True error: "<<( estimationOutput->parameterEstimate_ - truthParameters ).transpose( )< FormalError = estimationOutput->getFormalErrorVector( ); + std::cout<<"Formal error: "<getFormalErrorVector( ).transpose( )<getFormalErrorVector( ).segment( 0, numberOfParameters ) ).cwiseQuotient( + estimationOutput->parameterEstimate_ - truthParameters ) ).transpose( )<resetParameterValues(estimationOutput->parameterHistory_.at(estimationOutput->bestIteration_)); + + std::vector< std::shared_ptr< propagators::SimulationResults< long double, Time > > > bestIterationSimulationResultsPerArc; + std::map< Time, Eigen::Matrix< long double, Eigen::Dynamic, 1 > > concatenatedStateHistoryPostFitDynamic; + std::map< Time, Eigen::Matrix< long double, 6, 1 > > concatenatedStateHistoryPostFit; + + for ( unsigned int arcIndex = 0; arcIndex < bestIterationSimulationResultsPerArc.size(); ++arcIndex ) + { + // Retrieve the best iteration simulation results for the current arc + std::shared_ptr< propagators::SimulationResults< long double, Time > > bestIterationSimulationResults = + bestIterationSimulationResultsPerArc[arcIndex]; + bestIterationSimulationResultsPerArc.push_back( bestIterationSimulationResults ); + + // Retrieve the state history for the current arc + std::map< Time, Eigen::Matrix< long double, Eigen::Dynamic, 1 > > arcStateHistoryPostFitDynamic = + std::dynamic_pointer_cast< SingleArcVariationalSimulationResults< long double, Time > >( + bestIterationSimulationResults )->getDynamicsResults( )->getEquationsOfMotionNumericalSolution(); + + // Append the state history to the concatenated map + for ( auto it = arcStateHistoryPostFitDynamic.begin(); it != arcStateHistoryPostFitDynamic.end(); ++it ) + { + concatenatedStateHistoryPostFitDynamic[ it->first ] = it->second; + } + } + +// Convert the dynamic state history to fixed size + for ( auto it = concatenatedStateHistoryPostFitDynamic.begin(); it != concatenatedStateHistoryPostFitDynamic.end(); ++it ) + { + concatenatedStateHistoryPostFit[ it->first ] = it->second; + } + + //parametersToEstimate->resetParameterValues( estimationOutput->parameterHistory_.at( estimationOutput->bestIteration_ ) ); + + std::shared_ptr< CovarianceAnalysisOutput< double, double > > covarianceOutput = orbitDeterminationManager.computeCovariance( + covarianceInput ); + + //compareEstimationAndCovarianceResults( estimationOutput, covarianceOutput ); + + + //return ( finalParameters - truthParameters ).template cast< double >( ); + + + + + // Create link ends + /*LinkEnds linkEnds; + linkEnds[ observed_body ] = spacecraftName; + + // Create observation model settings + std::vector< std::shared_ptr< observation_models::ObservationModelSettings > > observationModelSettingsList; + observationModelSettingsList.push_back( positionObservableSettings( linkEnds ) ); + + // Select parameters to estimate + std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames; + parameterNames.push_back( std::make_shared< InitialTranslationalStateEstimatableParameterSettings< long double > >( + spacecraftName, spacecraftInitialState, centralBody, baseFrameOrientation ) ); + //parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( spacecraftName, radiation_pressure_coefficient ) ); + //parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( spacecraftName, constant_drag_coefficient ) ); + + std::map > > cosineBlockIndicesPerPower; + cosineBlockIndicesPerPower[ 1 ].push_back( std::make_pair( 2, 0 ) ); + std::map > > sineBlockIndicesPerPower; + sineBlockIndicesPerPower[ 1 ].push_back( std::make_pair( 3, 2 ) ); + parameterNames.push_back( std::make_shared< PolynomialGravityFieldVariationEstimatableParameterSettings >( + "Mars", cosineBlockIndicesPerPower, sineBlockIndicesPerPower ) ); + + std::shared_ptr< estimatable_parameters::EstimatableParameterSet< long double > > parametersToEstimate = + createParametersToEstimate< long double, Time >( parameterNames, bodies ); + + // Create orbit determination object. + OrbitDeterminationManager< long double, Time > orbitDeterminationManager = + OrbitDeterminationManager< long double, Time >( + bodies, parametersToEstimate, + observationModelSettingsList, propagatorSettings, true ); + + // Retrieve state history + std::map< long double, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > propagatedStateHistory; + for ( Time t : observationTimes ) + { + propagatedStateHistory[ t.getSeconds< long double >() ] = + bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( t ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( t ); + } + + writeDataMapToTextFile( propagatedStateHistory, "stateHistoryPropagatedPreFit_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + writeDataMapToTextFile( spiceStateHistory, "stateHistorySpice_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + + std::vector< std::shared_ptr< SingleObservationSet< long double, Time > > > observationSetList; + observationSetList.push_back( + std::make_shared< SingleObservationSet< long double, Time > >( + position_observable, linkEnds, observations, observationTimes, observed_body ) ); + std::shared_ptr< ObservationCollection< long double, Time > > observedObservationCollection = + std::make_shared< ObservationCollection< long double, Time > >( observationSetList ); + + // Define estimation input + std::shared_ptr< EstimationInput< long double, Time > > estimationInput = + std::make_shared< EstimationInput< long double, Time > >( + observedObservationCollection, + Eigen::MatrixXd::Zero( 0, 0 ), + std::make_shared< EstimationConvergenceChecker >( 5 ) ); + estimationInput->saveStateHistoryForEachIteration_ = true; + + // Perform estimation + std::shared_ptr< EstimationOutput< long double, Time > > estimationOutput = orbitDeterminationManager.estimateParameters( + estimationInput ); + + // Retrieve post-fit state history + std::shared_ptr< propagators::SimulationResults< long double, Time > > postFitSimulationResults = + estimationOutput->getBestIterationSimulationResults( ); + std::map < Time, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > propagatedStateHistoryPostFitDynamic = + std::dynamic_pointer_cast< SingleArcVariationalSimulationResults< long double, Time > >( + postFitSimulationResults )->getDynamicsResults( )->getEquationsOfMotionNumericalSolution( ); + std::map < Time, Eigen::Matrix < long double, 6, 1 > > propagatedStateHistoryPostFit; + for ( auto it = propagatedStateHistoryPostFitDynamic.begin( ); it != propagatedStateHistoryPostFitDynamic.end( ); ++it ) + { + propagatedStateHistoryPostFit[ it->first ] = it->second; + } + std::shared_ptr< interpolators::OneDimensionalInterpolator< Time, Eigen::Matrix< long double, 6, 1 > > > postFitStateInterpolator = + propagators::createStateInterpolator< Time, long double >( propagatedStateHistoryPostFit ); + std::map< long double, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > propagatedStateHistoryPostFitToWrite; + for ( Time t : observationTimes ) + { + propagatedStateHistoryPostFitToWrite[ t.getSeconds< long double >() ] = postFitStateInterpolator->interpolate( t ); + } +// for ( auto it = propagatedStateHistoryPostFit.begin( ); it != propagatedStateHistoryPostFit.end( ); ++it ) +// { +// propagatedStateHistoryPostFitToWrite[ it->first.getSeconds< long double >() ] = it->second; +// } + writeDataMapToTextFile( propagatedStateHistoryPostFitToWrite, "stateHistoryPropagatedPostFit_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + + // Retrieve residuals and set them in matrix + Eigen::MatrixXd residualHistory = estimationOutput->getResidualHistoryMatrix( ); + Eigen::Matrix< long double, Eigen::Dynamic, Eigen::Dynamic > parameterHistory = estimationOutput->getParameterHistoryMatrix( ); + + Eigen::MatrixXd residualsWithTime; + residualsWithTime.resize( residualHistory.rows( ), residualHistory.cols( ) + 1 ); + residualsWithTime.rightCols( residualHistory.cols( ) ) = residualHistory; + + for ( unsigned int i = 0; i < observedObservationCollection->getObservationVector( ).size( ); ++i ) + { + residualsWithTime( i, 0 ) = static_cast< Time >( observedObservationCollection->getConcatenatedTimeVector( ).at( i ) + ).getSeconds< long double >(); + } + + std::ofstream file(saveDirectory + "residuals_" + fileTag + ".txt"); + file << std::setprecision( 17 ) << residualsWithTime; + file.close(); + + std::ofstream file3(saveDirectory + "parameters_" + fileTag + ".txt"); + file3 << std::setprecision( 21 ) << parameterHistory; + file3.close(); + + // Retrieve covariance matrix + Eigen::MatrixXd normalizedCovarianceMatrix = estimationOutput->getNormalizedCovarianceMatrix( ); + Eigen::MatrixXd unnormalizedCovarianceMatrix = estimationOutput->getUnnormalizedCovarianceMatrix( ); + std::ofstream file4(saveDirectory + "covariance_" + fileTag + ".txt"); + file4 << std::setprecision( 17 ) << "Normalized covariance matrix: " << std::endl << normalizedCovarianceMatrix; + file4 << std::endl << std::endl; + file4 << "Unnormalized covariance matrix: " << std::endl << unnormalizedCovarianceMatrix; + file4.close( ); + */ + +} + diff --git a/tests_riva/newtest.cpp b/tests_riva/newtest.cpp new file mode 100644 index 0000000000..921675cc75 --- /dev/null +++ b/tests_riva/newtest.cpp @@ -0,0 +1,300 @@ +// +// Created by Riva Alkahal on 06/08/2024. +// +#include +#include +#include +#include "fstream" +#include "iostream" + +#include + +#include "tudat/basics/testMacros.h" +#include "tudat/astro/aerodynamics/customAerodynamicCoefficientInterface.h" +#include "tudat/astro/aerodynamics/aerodynamicAcceleration.h" +#include "tudat/astro/reference_frames/aerodynamicAngleCalculator.h" +#include "tudat/simulation/propagation_setup/dynamicsSimulator.h" +#include "tudat/interface/spice/spiceEphemeris.h" +#include "tudat/interface/spice/spiceRotationalEphemeris.h" +#include "tudat/io/basicInputOutput.h" +#include "tudat/simulation/environment_setup/body.h" +#include "tudat/simulation/estimation_setup/createNumericalSimulator.h" +#include "tudat/simulation/environment_setup/defaultBodies.h" +#include "tudat/simulation/environment_setup/body.h" +#include "tudat/astro/basic_astro/timeConversions.h" +#include "tudat/astro/aerodynamics/marsDtmAtmosphereModel.h" +#include "tudat/io/solarActivityData.h" +#include "tudat/astro/basic_astro/unitConversions.h" + +#include "tudat/basics/testMacros.h" +#include "tudat/simulation/estimation.h" +#include "tudat/simulation/estimation_setup.h" + +#include "tudat/io/readOdfFile.h" +#include "tudat/io/readTabulatedMediaCorrections.h" +#include "tudat/io/readTabulatedWeatherData.h" +#include "tudat/simulation/estimation_setup/processOdfFile.h" +#include "tudat/simulation/propagation_setup/setNumericallyIntegratedStates.h" + +#include + +#include "tudat/astro/ground_stations/transmittingFrequencies.h" + + +int main( ) { + using namespace tudat; + using namespace aerodynamics; + using namespace simulation_setup; + using namespace numerical_integrators; + using namespace simulation_setup; + using namespace basic_astrodynamics; + using namespace propagators; + using namespace estimatable_parameters; + using namespace observation_models; + using namespace basic_mathematics; + using namespace basic_astrodynamics; + using namespace tudat::spice_interface; + using namespace tudat::ephemerides; + using namespace tudat::input_output; + using namespace tudat::orbit_determination; + using namespace tudat::interpolators; + using namespace tudat::orbital_element_conversions; + + spice_interface::loadStandardSpiceKernels(); + spice_interface::loadSpiceKernelInTudat("/Users/ralkahal/estimate/sod_assignments/mgs_map4.bsp"); + spice_interface::loadSpiceKernelInTudat("/Users/ralkahal/estimate/sod_assignments/mgs_map5.bsp"); + spice_interface::loadSpiceKernelInTudat("/Users/ralkahal/estimate/sod_assignments/mgs_map6.bsp"); + spice_interface::loadSpiceKernelInTudat("/Users/ralkahal/estimate/sod_assignments/mgs_map7.bsp"); + spice_interface::loadSpiceKernelInTudat("/Users/ralkahal/estimate/sod_assignments/mgs_map8.bsp"); + + // Set output directory + std::string saveDirectory = "/Users/ralkahal/OneDrive - Delft University of Technology/PhD/Programs/testingestimation/"; + std::string fileTag = "polygrav"; + + // Set simulation time settings + Time initialEphemerisTime = Time(1.0E7);//Time( 0.0 ); + Time finalEphemerisTime = Time(3.0E7);//Time( 86400.0 * 8.0 ); // 5 years later + double totalDuration = finalEphemerisTime - initialEphemerisTime; + double ephemeridesTimeStep = 3600;//60.0; + bool useInterpolatedEphemerides = true; + double maximumTimeStep = 3600;//60.0; + double hoursperday = 10.0; + Time buffer = Time(10.0 * maximumTimeStep); + + // Set arcs, integrator, and observation times + std::vector< double > integrationArcStartTimes; + std::vector< double > integrationArcEndTimes; + std::vector< double > integrationArcLimits; + + double integrationStartTime = initialEphemerisTime + 1.0E4; + double integrationEndTime = finalEphemerisTime - 1.0E4; + double arcDuration = 1.6E7; //86400.0*4.0; + double arcOverlap = 2.0E4; + + // arc times + double currentStartTime = integrationStartTime, currentEndTime = integrationStartTime + arcDuration; + do + { + integrationArcLimits.push_back( currentStartTime ); + integrationArcEndTimes.push_back( currentEndTime ); + integrationArcStartTimes.push_back( currentStartTime ); + currentStartTime = currentEndTime - arcOverlap; + currentEndTime = currentStartTime + arcDuration; + } + while( currentEndTime < integrationEndTime ); + integrationArcLimits.push_back( currentStartTime + arcOverlap ); + + // observations times + int days = static_cast(totalDuration / physical_constants::JULIAN_DAY ); + std::vector initial_times_list = { integrationArcStartTimes[0] }; + for (int day = 1; day < days; ++day) { + initial_times_list.push_back(integrationArcStartTimes[0] + day * 24 * 3600); + } + std::vector final_times_list = { integrationArcStartTimes[0] + hoursperday * 3600 }; + for (int day = 1; day < days; ++day) { + final_times_list.push_back(integrationArcStartTimes[0] + day * 24 * 3600 + hoursperday * 3600); + } + std::vector observationTimesList; + for (int i = 0; i < days; ++i) { + double start = initial_times_list[i]; + double end = final_times_list[i]; + for (double time = start; time < end; time += 3600.0) { + observationTimesList.push_back(time); + } + } + std::cout<< "Observation times created" << std::endl; + // print observation times + std::cout << "Observation Times:" << std::endl; + for (const auto& time : observationTimesList) { + std::cout << time << std::endl; + } + // Set up the environment settings + // Create bodies + std::vector< std::string > bodiesToCreate = { + "Earth", "Sun", "Mercury", "Venus", "Mars", "Jupiter", "Phobos", "Deimos" }; + std::string baseFrameOrientation = "MARSIAU"; + std::string baseFrameOrigin = "SSB"; + + // Create body settings + BodyListSettings bodySettings = getDefaultBodySettings( + bodiesToCreate, initialEphemerisTime - buffer, finalEphemerisTime + buffer, + baseFrameOrigin, baseFrameOrientation, ephemeridesTimeStep ); + //BodyListSettings bodySettings = + // getDefaultBodySettings( bodiesToCreate, initialEphemerisTime - buffer, finalEphemerisTime + buffer ); + // at Earth + bodySettings.at( "Earth" )->groundStationSettings = getDsnStationSettings( ); + // at Mars + std::string filename ="/Users/ralkahal/OneDrive - Delft University of Technology/PhD/Programs/atmodensitydtm/dtm_mars";; + bodySettings.at( "Mars" )->atmosphereSettings = marsDtmAtmosphereSettings( filename, 3378.0E3); + std::cout << "Mars atmosphere settings created" << std::endl; + // Create spacecraft + std::string spacecraftName = "MGS"; + bodySettings.addSettings( spacecraftName ); + bodySettings.at( spacecraftName )->ephemerisSettings = std::make_shared< InterpolatedSpiceEphemerisSettings >( + initialEphemerisTime - buffer, finalEphemerisTime + buffer, + maximumTimeStep, baseFrameOrigin, baseFrameOrientation, + std::make_shared< interpolators::LagrangeInterpolatorSettings >( 6 ), spacecraftName ); + + bodySettings.at( spacecraftName )->constantMass = 700.0; + std::cout << "Spacecraft settings created" << std::endl; + // Create radiation pressure settings + double referenceAreaRadiation = 20.0; + double radiationPressureCoefficient = 1.0; + std::vector< std::string > occultingBodies = { "Mars" }; + bodySettings.at( spacecraftName )->radiationPressureSettings[ "Sun" ] = + cannonBallRadiationPressureSettings( "Sun", referenceAreaRadiation, radiationPressureCoefficient, occultingBodies ); + std::cout<< "Radiation pressure settings created" << std::endl; + // Create aerodynamic settings + std::shared_ptr aerodynamicCoefficientSettings = + std::make_shared( + 2.0, 4.0, Eigen::Vector3d::Zero( ), Eigen::Vector3d::UnitX( ), Eigen::Vector3d::Zero( ), + negative_aerodynamic_frame_coefficients, negative_aerodynamic_frame_coefficients ); + bodySettings.at( spacecraftName )->aerodynamicCoefficientSettings = aerodynamicCoefficientSettings; + + // Create gravity field variations + std::vector< std::shared_ptr< GravityFieldVariationSettings > > gravityFieldVariations; + std::map cosineAmplitudes; + cosineAmplitudes[ 1 ] = Eigen::Matrix< double, 3, 5 >::Zero( ); + std::map sineAmplitudes; + sineAmplitudes[ 1 ] = Eigen::Matrix< double, 3, 5 >::Zero( ); + std::shared_ptr< GravityFieldVariationSettings > polynomialGravityFieldVariations = + std::make_shared< PolynomialGravityFieldVariationsSettings >( + cosineAmplitudes, sineAmplitudes, -1.0E4, 2, 0 ); + gravityFieldVariations.push_back( polynomialGravityFieldVariations ); + std::vector< std::shared_ptr< GravityFieldVariationSettings > > gravityFieldVariationSettings = + gravityFieldVariations; + bodySettings.at( "Mars" )->gravityFieldVariationSettings = gravityFieldVariations; + + // Create body map + SystemOfBodies bodies = createSystemOfBodies< long double, Time >( bodySettings ); + std::cout<< "Bodies created" << std::endl; + // Create acceleration map + SelectedAccelerationMap accelerationMap; + std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle; + accelerationsOfVehicle[ "Sun" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Sun" ].push_back( cannonBallRadiationPressureAcceleration( ) ); + accelerationsOfVehicle[ "Sun" ].push_back( relativisticAccelerationCorrection( ) ); + accelerationsOfVehicle[ "Mercury" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Venus" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Earth" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Mars" ].push_back( sphericalHarmonicAcceleration( 50, 50 ) ); + accelerationsOfVehicle[ "Mars" ].push_back( relativisticAccelerationCorrection( ) ); + accelerationsOfVehicle[ "Mars" ].push_back( aerodynamicAcceleration( ) ); + accelerationsOfVehicle[ "Phobos" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Deimos" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Jupiter" ].push_back( pointMassGravityAcceleration( ) ); + accelerationMap[ spacecraftName ] = accelerationsOfVehicle; + + // Set bodies for which initial state is to be estimated and integrated. + std::vector< std::string > bodiesToIntegrate; + bodiesToIntegrate.push_back( spacecraftName ); + unsigned int numberOfNumericalBodies = bodiesToIntegrate.size( ); + std::vector< std::string > bodiesToEstimate; + bodiesToEstimate.push_back( spacecraftName ); + std::string centralBody = "Mars"; + std::vector< std::string > centralBodies = { centralBody }; + + // Create acceleration models + AccelerationMap accelerationModelMap = createAccelerationModelsMap( bodies, accelerationMap, bodiesToIntegrate, centralBodies ); + std::cout<< "Acceleration models created" << std::endl; + // Propagation settings + // Define integrator settings + std::shared_ptr< IntegratorSettings< double > > integratorSettings = + rungeKutta4Settings< double >( 3600.0 ); + + // Define propagator settings + // Get initial state + double marsGravitationalParameter = bodies.at( "Mars" )->getGravityFieldModel( )->getGravitationalParameter( ); + //std::vector< Eigen::VectorXd > systemInitialStates; + std::vector< Eigen::Vector6d > initialKeplerElements; + std::cout< > spiceStateHistory; + for ( Time t : observationTimesList ) + { + spiceStateHistory[ t.getSeconds< long double >() ] = + bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( t ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( t ); + } + writeDataMapToTextFile( spiceStateHistory, "stateHistorySpice_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + // Create propagator settings + // Eigen::Matrix< long double, 6, 1 > systemInitialState = bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, double >( integrationArcStartTimes[0] ) - + // bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( integrationArcStartTimes[0] ); + //propagatorSettingsList.push_back( + //> systemInitialState = getBodyCartesianStateAtEpoch(spacecraftName, "Mars", "MARSIAU", "NONE", integrationArcStartTimes[0] ); + std::vector< std::shared_ptr< SingleArcPropagatorSettings< double, double > > > propagatorSettingsList; + for( unsigned int i = 0; i < integrationArcStartTimes.size( ); i++ ) { + std::cout<< i << std::endl; + Eigen::Matrix< double, 6, 1 > systemInitialStates = bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< double, double >( integrationArcStartTimes[i] ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< double, double >( integrationArcStartTimes[i] ); + //systemInitialStates[i] = spice_interface::getBodyCartesianStateAtEpoch( + // spacecraftName, "Mars", "MARSIAU", "NONE", integrationArcStartTimes[i] ); + std::cout<< "Initial state created" << std::endl; + + propagatorSettingsList.push_back( + std::make_shared< TranslationalStatePropagatorSettings< double, double > > + ( centralBodies, accelerationModelMap, bodiesToIntegrate, + systemInitialStates, + integrationArcStartTimes[i], + integratorSettings, + propagationTimeTerminationSettings( integrationArcEndTimes[i]))); + } + + std::cout<< "Propagator settings created" << std::endl; + + std::shared_ptr< MultiArcPropagatorSettings< double, double > > propagatorSettings = + std::make_shared< MultiArcPropagatorSettings< double, double > >( propagatorSettingsList, 0 ); + /*std::shared_ptr< MultiArcPropagatorSettings< double > > multiArcPropagatorSettings = + validateDeprecatedMultiArcSettings< double, double >( + integratorSettings, std::make_shared< MultiArcPropagatorSettings< double > >( propagatorSettingsList ), + integrationArcStartTimes, true, true ); + */ + std::cout<< "Multi arc propagator settings created" << std::endl; + std::shared_ptr< PropagationPrintSettings > multiArcPrintSettings = + std::make_shared< PropagationPrintSettings >( ); + multiArcPrintSettings->reset( + true, true, TUDAT_NAN, 0, true, true, true, true, true, true ); + + propagatorSettings->getOutputSettings( )->resetAndApplyConsistentSingleArcPrintSettings( + multiArcPrintSettings ); + std::cout<< "Multi arc print settings created" << std::endl; + //multiArcPropagatorSettings->getOutputSettings( )->resetAndApplyConsistentSingleArcPrintSettings( + // multiArcPrintSettings ); + + MultiArcDynamicsSimulator< > dynamicsSimulator( + bodies, propagatorSettings); + + // Retrieve state history + std::map< long double, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > propagatedStateHistory; + for ( Time t : observationTimesList ) + { + propagatedStateHistory[ t.getSeconds< long double >() ] = + bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( t ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( t ); + } + writeDataMapToTextFile( propagatedStateHistory, "stateHistoryPropagation_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + + + } \ No newline at end of file diff --git a/tests_riva/oldtest.cpp b/tests_riva/oldtest.cpp new file mode 100644 index 0000000000..3259812bfb --- /dev/null +++ b/tests_riva/oldtest.cpp @@ -0,0 +1,385 @@ +// +// Created by Riva Alkahal on 26/07/2024. +// + +#include +#include +#include +#include "fstream" +#include "iostream" + +#include + +#include "tudat/basics/testMacros.h" +#include "tudat/astro/aerodynamics/customAerodynamicCoefficientInterface.h" +#include "tudat/astro/aerodynamics/aerodynamicAcceleration.h" +#include "tudat/astro/reference_frames/aerodynamicAngleCalculator.h" +#include "tudat/simulation/propagation_setup/dynamicsSimulator.h" +#include "tudat/interface/spice/spiceEphemeris.h" +#include "tudat/interface/spice/spiceRotationalEphemeris.h" +#include "tudat/io/basicInputOutput.h" +#include "tudat/simulation/environment_setup/body.h" +#include "tudat/simulation/estimation_setup/createNumericalSimulator.h" +#include "tudat/simulation/environment_setup/defaultBodies.h" +#include "tudat/simulation/environment_setup/body.h" +#include "tudat/astro/basic_astro/timeConversions.h" +#include "tudat/astro/aerodynamics/marsDtmAtmosphereModel.h" +#include "tudat/io/solarActivityData.h" +#include "tudat/astro/basic_astro/unitConversions.h" + +#include "tudat/basics/testMacros.h" +#include "tudat/simulation/estimation.h" +#include "tudat/simulation/estimation_setup.h" + +#include "tudat/io/readOdfFile.h" +#include "tudat/io/readTabulatedMediaCorrections.h" +#include "tudat/io/readTabulatedWeatherData.h" +#include "tudat/simulation/estimation_setup/processOdfFile.h" +#include "tudat/simulation/propagation_setup/setNumericallyIntegratedStates.h" + +#include + +#include "tudat/astro/ground_stations/transmittingFrequencies.h" + + +int main( ) { + using namespace tudat; + using namespace aerodynamics; + using namespace simulation_setup; + using namespace numerical_integrators; + using namespace simulation_setup; + using namespace basic_astrodynamics; + using namespace propagators; + using namespace estimatable_parameters; + using namespace observation_models; + using namespace basic_mathematics; + using namespace basic_astrodynamics; + using namespace tudat::spice_interface; + using namespace tudat::ephemerides; + using namespace tudat::input_output; + using namespace tudat::orbit_determination; + using namespace tudat::interpolators; + using namespace tudat::orbital_element_conversions; + + spice_interface::loadStandardSpiceKernels( ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map4.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map5.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map6.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map7.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map8.bsp" ); + + std::string saveDirectory = "/Users/ralkahal/OneDrive - Delft University of Technology/PhD/Programs/testingestimation/"; + std::string fileTag = "polygrav"; + + // set input options + double epehemeridesTimeStep = 60.0; + bool useInterpolatedEphemerides = true; + double observationsSamplingTime = 10.0; + + //const double gravitationalParameter = 4.2828378e13; + //const double planetaryRadius = 3389.5E3; + + // Select ephemeris time range (based on available data in loaded SPICE ephemeris) + //Time initialEphemerisTime = Time( 185976000 - 1.0 * 86400.0 ); // 23 November 2005, 0h + //Time finalEphemerisTime = Time( 186580800 + 1.0 * 86400.0 ); // 30 November 2005, 0h + // time at 2000-01-01T00:00:00 + Time initialEphemerisTime = Time( 0.0 ); + Time finalEphemerisTime = Time( 86400.0/2 * 1.0 ); // 5 years later + + // Define bodies to use. + //std::vector< std::string > bodiesToCreate = { + // "Earth", "Sun", "Mercury", "Venus", "Mars", "Jupiter", "Saturn", "Uranus", "Neptune", "Phobos", "Deimos", + // "Io", "Ganymede", "Callisto", "Europa", "Titan" }; + std::vector< std::string > bodiesToCreate = { + "Earth", "Sun", "Mercury", "Venus", "Mars", "Jupiter", "Phobos", "Deimos" }; + + std::string baseFrameOrientation = "MARSIAU"; + std::string baseFrameOrigin = "SSB"; + + // Specify ephemeris time steps and buffers + Time ephemerisTimeStepPlanets = Time( epehemeridesTimeStep ); + std::cout<<"Ephemeris time step planets: "<shapeModelSettings = fromSpiceOblateSphericalBodyShapeSettings( ); + //bodySettings.at( "Earth" )->rotationModelSettings = gcrsToItrsRotationModelSettings( + // basic_astrodynamics::iau_2006, baseFrameOrientation ); + bodySettings.at( "Earth" )->groundStationSettings = getDsnStationSettings( ); + + std::string filename ="/Users/ralkahal/OneDrive - Delft University of Technology/PhD/Programs/atmodensitydtm/dtm_mars";; + bodySettings.at( "Mars" )->atmosphereSettings = marsDtmAtmosphereSettings( filename, 3378.0E3); + + // Create spacecraft + std::string spacecraftName = "MGS"; + bodySettings.addSettings( spacecraftName ); + if ( useInterpolatedEphemerides ) + { + bodySettings.at( spacecraftName )->ephemerisSettings = + std::make_shared< InterpolatedSpiceEphemerisSettings >( + initialEphemerisTime - bufferSpacecraft, finalEphemerisTime + bufferSpacecraft, + ephemerisTimeStepSpacecraft, baseFrameOrigin, baseFrameOrientation, + std::make_shared< interpolators::LagrangeInterpolatorSettings >( 6 ), spacecraftName ); + } + else + { + bodySettings.at( spacecraftName )->ephemerisSettings = + std::make_shared< DirectSpiceEphemerisSettings >( baseFrameOrigin, baseFrameOrientation ); + } + bodySettings.at( spacecraftName )->constantMass = 700.0; + + // Create radiation pressure settings + double referenceAreaRadiation = 20.0; + double radiationPressureCoefficient = 1.0; + std::vector< std::string > occultingBodies = { "Mars" }; + bodySettings.at( spacecraftName )->radiationPressureSettings[ "Sun" ] = + cannonBallRadiationPressureSettings( "Sun", referenceAreaRadiation, radiationPressureCoefficient, occultingBodies ); + + std::shared_ptr aerodynamicCoefficientSettings = + std::make_shared( + 2.0, 4.0, Eigen::Vector3d::Zero( ), Eigen::Vector3d::UnitX( ), Eigen::Vector3d::Zero( ), + negative_aerodynamic_frame_coefficients, negative_aerodynamic_frame_coefficients ); + bodySettings.at( spacecraftName )->aerodynamicCoefficientSettings = aerodynamicCoefficientSettings; + + + std::vector< std::shared_ptr< GravityFieldVariationSettings > > gravityFieldVariations; + + std::map cosineAmplitudes; + cosineAmplitudes[ 1 ] = Eigen::Matrix< double, 3, 5 >::Zero( ); + //cosineAmplitudes[ 2 ] = Eigen::Matrix< double, 3, 5 >::Zero( ); + std::map sineAmplitudes; + sineAmplitudes[ 1 ] = Eigen::Matrix< double, 3, 5 >::Zero( ); + std::shared_ptr< GravityFieldVariationSettings > polynomialGravityFieldVariations = + std::make_shared< PolynomialGravityFieldVariationsSettings >( + cosineAmplitudes, sineAmplitudes, -1.0E4, 2, 0 ); + gravityFieldVariations.push_back( polynomialGravityFieldVariations ); + + std::vector< std::shared_ptr< GravityFieldVariationSettings > > gravityFieldVariationSettings = + gravityFieldVariations; + bodySettings.at( "Mars" )->gravityFieldVariationSettings = gravityFieldVariations; + + SystemOfBodies bodies = createSystemOfBodies< long double, Time >( bodySettings ); + + Time initialPropagationTime = initialEphemerisTime + 600.0; + Time finalPropagationTime = finalEphemerisTime - 600.0; + + // Select observation times + // Compute observed observations. NOTE: don't move this to after the creation of the OrbitDeterminationManager! + std::vector< Time > observationTimes; + std::vector< Eigen::Matrix< long double, Eigen::Dynamic, 1 > > observations; + for ( Time t = initialPropagationTime; t < finalPropagationTime; t += observationsSamplingTime ) + { + try + { + observations.push_back( bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( t ).segment( 0, 3 ) ); + observationTimes.push_back( t ); + } + catch( ... ) + { } + } + + + // Set accelerations on Vehicle that are to be taken into account. + SelectedAccelerationMap accelerationMap; + std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle; + accelerationsOfVehicle[ "Sun" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Sun" ].push_back( cannonBallRadiationPressureAcceleration( ) ); +// accelerationsOfVehicle[ "Sun" ].push_back( relativisticAccelerationCorrection( ) ); + accelerationsOfVehicle[ "Mercury" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Venus" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Earth" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Mars" ].push_back( sphericalHarmonicAcceleration( 50, 50 ) ); + accelerationsOfVehicle[ "Mars" ].push_back( relativisticAccelerationCorrection( ) ); + accelerationsOfVehicle[ "Mars" ].push_back( aerodynamicAcceleration( ) ); + accelerationsOfVehicle[ "Phobos" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Deimos" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Jupiter" ].push_back( pointMassGravityAcceleration( ) ); + + accelerationMap[ spacecraftName ] = accelerationsOfVehicle; + + // Set bodies for which initial state is to be estimated and integrated. + std::vector< std::string > bodiesToIntegrate; + std::string centralBody = "Mars"; + std::vector< std::string > centralBodies = { centralBody }; + bodiesToIntegrate.push_back( spacecraftName ); + + // Create acceleration models + AccelerationMap accelerationModelMap = createAccelerationModelsMap( bodies, accelerationMap, bodiesToIntegrate, centralBodies ); + + // Create integrator settings + std::pair< double, double > integrationMinMaxStep = std::make_pair( 1e-16, 1e16 ); + + double initialStep = 5.0; + if ( integrationMinMaxStep.first == integrationMinMaxStep.second ) + { + initialStep = integrationMinMaxStep.first; + } + std::shared_ptr< IntegratorSettings< Time > > integratorSettings = rungeKuttaVariableStepSettingsScalarTolerances< Time >( + initialStep, rungeKutta87DormandPrince, integrationMinMaxStep.first, + integrationMinMaxStep.second, 1e-10, 1e-10 ); + + /*std::shared_ptr< IntegratorSettings< Time > > integratorSettings = + rungeKutta4Settings< Time >( 60.0 );*/ + // Set initial state from ephemerides + Eigen::Matrix< long double, 6, 1 > spacecraftInitialState = + bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( initialPropagationTime ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( initialPropagationTime ); + + // Retrieve state history from SPICE + std::map< long double, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > spiceStateHistory; + for ( Time t : observationTimes ) + { + spiceStateHistory[ t.getSeconds< long double >() ] = + bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( t ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( t ); + } + + // Create termination settings + std::shared_ptr< PropagationTerminationSettings > terminationSettings = propagationTimeTerminationSettings( + finalPropagationTime ); + // Create propagator settings + std::shared_ptr< TranslationalStatePropagatorSettings< long double, Time > > propagatorSettings = translationalStatePropagatorSettings< + long double, Time >( centralBodies, + accelerationModelMap, + bodiesToIntegrate, + spacecraftInitialState, + initialPropagationTime, + integratorSettings, + terminationSettings, + gauss_modified_equinoctial ); + + // Create link ends + LinkEnds linkEnds; + linkEnds[ observed_body ] = spacecraftName; + + // Create observation model settings + std::vector< std::shared_ptr< observation_models::ObservationModelSettings > > observationModelSettingsList; + observationModelSettingsList.push_back( positionObservableSettings( linkEnds ) ); + + // Select parameters to estimate + std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames; + parameterNames.push_back( std::make_shared< InitialTranslationalStateEstimatableParameterSettings< long double > >( + spacecraftName, spacecraftInitialState, centralBody, baseFrameOrientation ) ); + //parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( spacecraftName, radiation_pressure_coefficient ) ); + //parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( spacecraftName, constant_drag_coefficient ) ); + + std::map > > cosineBlockIndicesPerPower; + cosineBlockIndicesPerPower[ 1 ].push_back( std::make_pair( 2, 0 ) ); + std::map > > sineBlockIndicesPerPower; + sineBlockIndicesPerPower[ 1 ].push_back( std::make_pair( 3, 2 ) ); + parameterNames.push_back( std::make_shared< PolynomialGravityFieldVariationEstimatableParameterSettings >( + "Mars", cosineBlockIndicesPerPower, sineBlockIndicesPerPower ) ); + + std::shared_ptr< estimatable_parameters::EstimatableParameterSet< long double > > parametersToEstimate = + createParametersToEstimate< long double, Time >( parameterNames, bodies ); + + // Create orbit determination object. + OrbitDeterminationManager< long double, Time > orbitDeterminationManager = + OrbitDeterminationManager< long double, Time >( + bodies, parametersToEstimate, + observationModelSettingsList, propagatorSettings, true ); + + // Retrieve state history + std::map< long double, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > propagatedStateHistory; + for ( Time t : observationTimes ) + { + propagatedStateHistory[ t.getSeconds< long double >() ] = + bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( t ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( t ); + } + + writeDataMapToTextFile( propagatedStateHistory, "stateHistoryPropagatedPreFit_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + writeDataMapToTextFile( spiceStateHistory, "stateHistorySpice_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + + std::vector< std::shared_ptr< SingleObservationSet< long double, Time > > > observationSetList; + observationSetList.push_back( + std::make_shared< SingleObservationSet< long double, Time > >( + position_observable, linkEnds, observations, observationTimes, observed_body ) ); + std::shared_ptr< ObservationCollection< long double, Time > > observedObservationCollection = + std::make_shared< ObservationCollection< long double, Time > >( observationSetList ); + + // Define estimation input + std::shared_ptr< EstimationInput< long double, Time > > estimationInput = + std::make_shared< EstimationInput< long double, Time > >( + observedObservationCollection, + Eigen::MatrixXd::Zero( 0, 0 ), + std::make_shared< EstimationConvergenceChecker >( 5 ) ); + estimationInput->saveStateHistoryForEachIteration_ = true; + + // Perform estimation + std::shared_ptr< EstimationOutput< long double, Time > > estimationOutput = orbitDeterminationManager.estimateParameters( + estimationInput ); + + // Retrieve post-fit state history + std::shared_ptr< propagators::SimulationResults< long double, Time > > postFitSimulationResults = + estimationOutput->getBestIterationSimulationResults( ); + std::map < Time, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > propagatedStateHistoryPostFitDynamic = + std::dynamic_pointer_cast< SingleArcVariationalSimulationResults< long double, Time > >( + postFitSimulationResults )->getDynamicsResults( )->getEquationsOfMotionNumericalSolution( ); + std::map < Time, Eigen::Matrix < long double, 6, 1 > > propagatedStateHistoryPostFit; + for ( auto it = propagatedStateHistoryPostFitDynamic.begin( ); it != propagatedStateHistoryPostFitDynamic.end( ); ++it ) + { + propagatedStateHistoryPostFit[ it->first ] = it->second; + } + std::shared_ptr< interpolators::OneDimensionalInterpolator< Time, Eigen::Matrix< long double, 6, 1 > > > postFitStateInterpolator = + propagators::createStateInterpolator< Time, long double >( propagatedStateHistoryPostFit ); + std::map< long double, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > propagatedStateHistoryPostFitToWrite; + for ( Time t : observationTimes ) + { + propagatedStateHistoryPostFitToWrite[ t.getSeconds< long double >() ] = postFitStateInterpolator->interpolate( t ); + } +// for ( auto it = propagatedStateHistoryPostFit.begin( ); it != propagatedStateHistoryPostFit.end( ); ++it ) +// { +// propagatedStateHistoryPostFitToWrite[ it->first.getSeconds< long double >() ] = it->second; +// } + writeDataMapToTextFile( propagatedStateHistoryPostFitToWrite, "stateHistoryPropagatedPostFit_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + + // Retrieve residuals and set them in matrix + Eigen::MatrixXd residualHistory = estimationOutput->getResidualHistoryMatrix( ); + Eigen::Matrix< long double, Eigen::Dynamic, Eigen::Dynamic > parameterHistory = estimationOutput->getParameterHistoryMatrix( ); + + Eigen::MatrixXd residualsWithTime; + residualsWithTime.resize( residualHistory.rows( ), residualHistory.cols( ) + 1 ); + residualsWithTime.rightCols( residualHistory.cols( ) ) = residualHistory; + + for ( unsigned int i = 0; i < observedObservationCollection->getObservationVector( ).size( ); ++i ) + { + residualsWithTime( i, 0 ) = static_cast< Time >( observedObservationCollection->getConcatenatedTimeVector( ).at( i ) + ).getSeconds< long double >(); + } + + std::ofstream file(saveDirectory + "residuals_" + fileTag + ".txt"); + file << std::setprecision( 17 ) << residualsWithTime; + file.close(); + + std::ofstream file3(saveDirectory + "parameters_" + fileTag + ".txt"); + file3 << std::setprecision( 21 ) << parameterHistory; + file3.close(); + + // Retrieve covariance matrix + Eigen::MatrixXd normalizedCovarianceMatrix = estimationOutput->getNormalizedCovarianceMatrix( ); + Eigen::MatrixXd unnormalizedCovarianceMatrix = estimationOutput->getUnnormalizedCovarianceMatrix( ); + std::ofstream file4(saveDirectory + "covariance_" + fileTag + ".txt"); + file4 << std::setprecision( 17 ) << "Normalized covariance matrix: " << std::endl << normalizedCovarianceMatrix; + file4 << std::endl << std::endl; + file4 << "Unnormalized covariance matrix: " << std::endl << unnormalizedCovarianceMatrix; + file4.close( ); + +} + diff --git a/tests_riva/testEstimationVariations.cpp b/tests_riva/testEstimationVariations.cpp new file mode 100644 index 0000000000..dd23019701 --- /dev/null +++ b/tests_riva/testEstimationVariations.cpp @@ -0,0 +1,457 @@ +// +// Created by Riva Alkahal on 26/07/2024. +// + +#include +#include +#include +#include "fstream" +#include "iostream" + +#include + +#include "tudat/basics/testMacros.h" +#include "tudat/astro/aerodynamics/customAerodynamicCoefficientInterface.h" +#include "tudat/astro/aerodynamics/aerodynamicAcceleration.h" +#include "tudat/astro/reference_frames/aerodynamicAngleCalculator.h" +#include "tudat/simulation/propagation_setup/dynamicsSimulator.h" +#include "tudat/interface/spice/spiceEphemeris.h" +#include "tudat/interface/spice/spiceRotationalEphemeris.h" +#include "tudat/io/basicInputOutput.h" +#include "tudat/simulation/environment_setup/body.h" +#include "tudat/simulation/estimation_setup/createNumericalSimulator.h" +#include "tudat/simulation/environment_setup/defaultBodies.h" +#include "tudat/simulation/environment_setup/body.h" +#include "tudat/astro/basic_astro/timeConversions.h" +#include "tudat/astro/aerodynamics/marsDtmAtmosphereModel.h" +#include "tudat/io/solarActivityData.h" +#include "tudat/astro/basic_astro/unitConversions.h" + +#include "tudat/basics/testMacros.h" +#include "tudat/simulation/estimation.h" +#include "tudat/simulation/estimation_setup.h" + +#include "tudat/io/readOdfFile.h" +#include "tudat/io/readTabulatedMediaCorrections.h" +#include "tudat/io/readTabulatedWeatherData.h" +#include "tudat/simulation/estimation_setup/processOdfFile.h" +#include "tudat/simulation/propagation_setup/setNumericallyIntegratedStates.h" + +#include + +#include "tudat/astro/ground_stations/transmittingFrequencies.h" + + +int main( ) { + using namespace tudat; + using namespace aerodynamics; + using namespace simulation_setup; + using namespace numerical_integrators; + using namespace simulation_setup; + using namespace basic_astrodynamics; + using namespace propagators; + using namespace estimatable_parameters; + using namespace observation_models; + using namespace basic_mathematics; + using namespace basic_astrodynamics; + using namespace tudat::spice_interface; + using namespace tudat::ephemerides; + using namespace tudat::input_output; + using namespace tudat::orbit_determination; + using namespace tudat::interpolators; + using namespace tudat::orbital_element_conversions; + + spice_interface::loadStandardSpiceKernels( ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map4.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map5.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map6.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map7.bsp" ); + spice_interface::loadSpiceKernelInTudat( "/Users/ralkahal/estimate/sod_assignments/mgs_map8.bsp" ); + + //template< typename ObservationScalarType = double , typename TimeType = double , typename StateScalarType = double > + + std::string saveDirectory = "/Users/ralkahal/OneDrive - Delft University of Technology/PhD/Programs/testingestimation/"; + std::string fileTag = "polygrav"; + + // Specify initial time + Time initialEphemerisTime = Time( 1.0E7 ); + Time finalEphemerisTime = Time(6.0E7 ); + double maximumTimeStep = 3600.0; + + Time buffer = Time(10.0 * maximumTimeStep); + // set input options + //double epehemeridesTimeStep = 60.0; + bool useInterpolatedEphemerides = true; + //double observationsSamplingTime = 10.0; + + //const double gravitationalParameter = 4.2828378e13; + //const double planetaryRadius = 3389.5E3; + + // Select ephemeris time range (based on available data in loaded SPICE ephemeris) + //Time initialEphemerisTime = Time( 185976000 - 1.0 * 86400.0 ); // 23 November 2005, 0h + //Time finalEphemerisTime = Time( 186580800 + 1.0 * 86400.0 ); // 30 November 2005, 0h + // time at 2000-01-01T00:00:00 + //Time initialEphemerisTime = Time( 0.0 ); + //Time finalEphemerisTime = Time( 86400.0/2 * 1.0 ); // 5 years later + + // Define bodies to use. + //std::vector< std::string > bodiesToCreate = { + // "Earth", "Sun", "Mercury", "Venus", "Mars", "Jupiter", "Saturn", "Uranus", "Neptune", "Phobos", "Deimos", + // "Io", "Ganymede", "Callisto", "Europa", "Titan" }; + std::vector< std::string > bodiesToCreate = { + "Earth", "Sun", "Mercury", "Venus", "Mars", "Jupiter", "Phobos", "Deimos" }; + + std::string baseFrameOrientation = "MARSIAU"; + std::string baseFrameOrigin = "SSB"; + BodyListSettings bodySettings = + getDefaultBodySettings( bodiesToCreate, initialEphemerisTime - buffer, finalEphemerisTime + buffer ); + + // Specify ephemeris time steps and buffers + //Time ephemerisTimeStepPlanets = Time( epehemeridesTimeStep ); + //std::cout<<"Ephemeris time step planets: "<shapeModelSettings = fromSpiceOblateSphericalBodyShapeSettings( ); + //bodySettings.at( "Earth" )->rotationModelSettings = gcrsToItrsRotationModelSettings( + // basic_astrodynamics::iau_2006, baseFrameOrientation ); + bodySettings.at( "Earth" )->groundStationSettings = getDsnStationSettings( ); + + std::string filename ="/Users/ralkahal/OneDrive - Delft University of Technology/PhD/Programs/atmodensitydtm/dtm_mars";; + bodySettings.at( "Mars" )->atmosphereSettings = marsDtmAtmosphereSettings( filename, 3378.0E3); + + // Create spacecraft + std::string spacecraftName = "MGS"; + + bodySettings.addSettings( spacecraftName ); + bodySettings.at( spacecraftName )->ephemerisSettings = std::make_shared< InterpolatedSpiceEphemerisSettings >( + initialEphemerisTime - buffer, finalEphemerisTime + buffer, + maximumTimeStep, baseFrameOrigin, baseFrameOrientation, + std::make_shared< interpolators::LagrangeInterpolatorSettings >( 6 ), spacecraftName ); + + //getDefaultBodySettings( bodiesToCreate, initialEphemerisTime - buffer, finalEphemerisTime + buffer ); + + /* + * if ( useInterpolatedEphemerides ) + { + bodySettings.at( spacecraftName )->ephemerisSettings = + std::make_shared< InterpolatedSpiceEphemerisSettings >( + initialEphemerisTime - bufferSpacecraft, finalEphemerisTime + bufferSpacecraft, + ephemerisTimeStepSpacecraft, baseFrameOrigin, baseFrameOrientation, + std::make_shared< interpolators::LagrangeInterpolatorSettings >( 6 ), spacecraftName ); + } + else + { + bodySettings.at( spacecraftName )->ephemerisSettings = + std::make_shared< DirectSpiceEphemerisSettings >( baseFrameOrigin, baseFrameOrientation ); + } + */ + + bodySettings.at( spacecraftName )->constantMass = 700.0; + + // Create radiation pressure settings + double referenceAreaRadiation = 20.0; + double radiationPressureCoefficient = 1.0; + std::vector< std::string > occultingBodies = { "Mars" }; + bodySettings.at( spacecraftName )->radiationPressureSettings[ "Sun" ] = + cannonBallRadiationPressureSettings( "Sun", referenceAreaRadiation, radiationPressureCoefficient, occultingBodies ); + + std::shared_ptr aerodynamicCoefficientSettings = + std::make_shared( + 2.0, 4.0, Eigen::Vector3d::Zero( ), Eigen::Vector3d::UnitX( ), Eigen::Vector3d::Zero( ), + negative_aerodynamic_frame_coefficients, negative_aerodynamic_frame_coefficients ); + bodySettings.at( spacecraftName )->aerodynamicCoefficientSettings = aerodynamicCoefficientSettings; + + + std::vector< std::shared_ptr< GravityFieldVariationSettings > > gravityFieldVariations; + + std::map cosineAmplitudes; + cosineAmplitudes[ 1 ] = Eigen::Matrix< double, 3, 5 >::Zero( ); + //cosineAmplitudes[ 2 ] = Eigen::Matrix< double, 3, 5 >::Zero( ); + std::map sineAmplitudes; + sineAmplitudes[ 1 ] = Eigen::Matrix< double, 3, 5 >::Zero( ); + std::shared_ptr< GravityFieldVariationSettings > polynomialGravityFieldVariations = + std::make_shared< PolynomialGravityFieldVariationsSettings >( + cosineAmplitudes, sineAmplitudes, -1.0E4, 2, 0 ); + gravityFieldVariations.push_back( polynomialGravityFieldVariations ); + + std::vector< std::shared_ptr< GravityFieldVariationSettings > > gravityFieldVariationSettings = + gravityFieldVariations; + bodySettings.at( "Mars" )->gravityFieldVariationSettings = gravityFieldVariations; + + SystemOfBodies bodies = createSystemOfBodies< long double, Time >( bodySettings ); + + Time initialPropagationTime = initialEphemerisTime + 600.0; + Time finalPropagationTime = finalEphemerisTime - 600.0; + + // Set accelerations on Vehicle that are to be taken into account. + + SelectedAccelerationMap accelerationMap; + std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle; + accelerationsOfVehicle[ "Sun" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Sun" ].push_back( cannonBallRadiationPressureAcceleration( ) ); +// accelerationsOfVehicle[ "Sun" ].push_back( relativisticAccelerationCorrection( ) ); + accelerationsOfVehicle[ "Mercury" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Venus" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Earth" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Mars" ].push_back( sphericalHarmonicAcceleration( 50, 50 ) ); + accelerationsOfVehicle[ "Mars" ].push_back( relativisticAccelerationCorrection( ) ); + accelerationsOfVehicle[ "Mars" ].push_back( aerodynamicAcceleration( ) ); + accelerationsOfVehicle[ "Phobos" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Deimos" ].push_back( pointMassGravityAcceleration( ) ); + accelerationsOfVehicle[ "Jupiter" ].push_back( pointMassGravityAcceleration( ) ); + + accelerationMap[ spacecraftName ] = accelerationsOfVehicle; + // Set bodies for which initial state is to be estimated and integrated. + std::vector< std::string > bodiesToIntegrate; + bodiesToIntegrate.push_back( spacecraftName ); + unsigned int numberOfNumericalBodies = bodiesToIntegrate.size( ); + std::vector< std::string > bodiesToEstimate; + bodiesToEstimate.push_back( spacecraftName ); + std::string centralBody = "Mars"; + std::vector< std::string > centralBodies = { centralBody }; + + + // Create acceleration models + AccelerationMap accelerationModelMap = createAccelerationModelsMap( bodies, accelerationMap, bodiesToIntegrate, centralBodies ); + + // Create integrator settings + std::vector< double > integrationArcStartTimes; + std::vector< double > integrationArcEndTimes; + + std::vector< double > integrationArcLimits; + + double integrationStartTime = initialEphemerisTime + 1.0E4; + double integrationEndTime = finalEphemerisTime - 1.0E4; + double arcDuration = 1.6E7; + double arcOverlap = 2.0E4; + + double currentStartTime = integrationStartTime, currentEndTime = integrationStartTime + arcDuration; + + do + { + integrationArcLimits.push_back( currentStartTime ); + integrationArcEndTimes.push_back( currentEndTime ); + integrationArcStartTimes.push_back( currentStartTime ); + currentStartTime = currentEndTime - arcOverlap; + currentEndTime = currentStartTime + arcDuration; + } + while( currentEndTime < integrationEndTime ); + integrationArcLimits.push_back( currentStartTime + arcOverlap ); + + // Define integrator settings. + std::shared_ptr< IntegratorSettings< double > > integratorSettings = + rungeKutta4Settings< double >( 3600.0 ); + + + std::vector< Eigen::VectorXd > systemInitialStates; + std::vector< Eigen::Vector6d > initialKeplerElements; + + double marsGravitationalParameter = bodies.at( "Mars" )->getGravityFieldModel( )->getGravitationalParameter( ); + + + std::vector< std::shared_ptr< SingleArcPropagatorSettings< double, double > > > propagatorSettingsList; + for( unsigned int i = 0; i < integrationArcStartTimes.size( ); i++ ) + { + systemInitialStates[ i ] = spice_interface::getBodyCartesianStateAtEpoch( + bodiesToIntegrate[ 0 ], "Mars", "MARSIAU", "NONE", integrationArcStartTimes.at(i)); + initialKeplerElements[ i ] = ( + orbital_element_conversions::convertCartesianToKeplerianElements( + Eigen::Vector6d( systemInitialStates[ i ] ), + marsGravitationalParameter ) ); + + propagatorSettingsList.push_back( + std::make_shared< TranslationalStatePropagatorSettings< double, double > > + ( centralBodies, accelerationModelMap, bodiesToIntegrate, + systemInitialStates[i], + integrationArcStartTimes.at( i ), + integratorSettings, + propagationTimeTerminationSettings( integrationArcEndTimes.at( i ) ) ) ); + } + std::shared_ptr< MultiArcPropagatorSettings< double, double > > propagatorSettings = + std::make_shared< MultiArcPropagatorSettings< double, double > >( propagatorSettingsList, 0 ); + + // Select parameters to estimate + std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames; + parameterNames = getInitialMultiArcParameterSettings< double, double >( propagatorSettings, bodies, integrationArcStartTimes ); + std::map > > cosineBlockIndicesPerPower; + cosineBlockIndicesPerPower[ 1 ].push_back( std::make_pair( 2, 0 ) ); + std::map > > sineBlockIndicesPerPower; + sineBlockIndicesPerPower[ 1 ].push_back( std::make_pair( 3, 2 ) ); + parameterNames.push_back( std::make_shared< PolynomialGravityFieldVariationEstimatableParameterSettings >( + "Mars", cosineBlockIndicesPerPower, sineBlockIndicesPerPower ) ); + + std::shared_ptr< estimatable_parameters::EstimatableParameterSet< long double > > parametersToEstimate = + createParametersToEstimate< long double, Time >( parameterNames, bodies ); + + + // Create link ends + LinkEnds linkEnds; + linkEnds[ transmitter ] = spacecraftName; + std::vector< std::string > GroundStations = { "DSS-26" , "DSS-42", "DSS-61"}; + for ( std::string groundStation : GroundStations ) + { + linkEnds[ receiver ] = LinkEndId( "Earth", groundStation ); + } + // Create observation model settings + std::vector< std::shared_ptr< observation_models::ObservationModelSettings > > observationModelSettingsList; + observationModelSettingsList.push_back( positionObservableSettings( linkEnds ) ); + + std::vector< std::shared_ptr< ObservationModelSettings > > observationSettingsList; + observationSettingsList.push_back( std::make_shared< ObservationModelSettings >( + one_way_range, linkEnds ) ); + + // Observation times + Time observationTime; + int numberOfObservationsPerArc = 5000; + double timeBuffer = 9000.0; + + std::vector< Time > initialObservationTimes; + initialObservationTimes.resize( numberOfObservationsPerArc * integrationArcStartTimes.size( ) ); + + for( unsigned int i = 0; i < integrationArcLimits.size( ) - 1; i++ ) + { + double currentTimeStep = ( integrationArcLimits[ i + 1 ] - integrationArcLimits[ i ] - 2.0 * timeBuffer ) / + static_cast< double >( numberOfObservationsPerArc - 1 ); + observationTime = integrationArcLimits[ i ] + timeBuffer; + for( int j = 0; j < numberOfObservationsPerArc; j++ ) + { + initialObservationTimes[ j + i * numberOfObservationsPerArc ] = observationTime; + observationTime += currentTimeStep; + } + } + + // Observation simulation settings + std::vector< std::shared_ptr< ObservationSimulationSettings< Time > > > measurementSimulationInput; + measurementSimulationInput.push_back( + std::make_shared< TabulatedObservationSimulationSettings< Time > >( + one_way_range, linkEnds , initialObservationTimes, receiver ) ); + + + // Retrieve state history from SPICE + std::map< long double, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > spiceStateHistory; + for ( Time t : initialObservationTimes ) + { + spiceStateHistory[ t.getSeconds< long double >() ] = + bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( t ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( t ); + } + + // Create orbit determination object. + OrbitDeterminationManager< long double, double > orbitDeterminationManager = + OrbitDeterminationManager< long double, double >( + bodies, parametersToEstimate, + observationSettingsList, propagatorSettings ); + + Eigen::Matrix< double, Eigen::Dynamic, 1 > initialParameterEstimate = + parametersToEstimate->template getFullParameterValues< double >( ); + + std::shared_ptr< ObservationCollection< double, Time > > observationsAndTimes = simulateObservations< double, Time >( + measurementSimulationInput, orbitDeterminationManager.getObservationSimulators( ), bodies ); + + + // Retrieve state history + std::map< long double, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > propagatedStateHistory; + for ( Time t : initialObservationTimes ) + { + propagatedStateHistory[ t.getSeconds< long double >() ] = + bodies.getBody( spacecraftName )->getStateInBaseFrameFromEphemeris< long double, Time >( t ) - + bodies.getBody( centralBody )->getStateInBaseFrameFromEphemeris< long double, Time >( t ); + } + + writeDataMapToTextFile( propagatedStateHistory, "stateHistoryPropagatedPreFit_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + writeDataMapToTextFile( spiceStateHistory, "stateHistorySpice_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + + std::vector< std::shared_ptr< SingleObservationSet< long double, Time > > > observationSetList; + observationSetList.push_back( + std::make_shared< SingleObservationSet< long double, Time > >( + position_observable, linkEnds, observations, observationTimes, observed_body ) ); + std::shared_ptr< ObservationCollection< long double, Time > > observedObservationCollection = + std::make_shared< ObservationCollection< long double, Time > >( observationSetList ); + + // Define estimation input + std::shared_ptr< EstimationInput< long double, Time > > estimationInput = + std::make_shared< EstimationInput< long double, Time > >( + observedObservationCollection, + Eigen::MatrixXd::Zero( 0, 0 ), + std::make_shared< EstimationConvergenceChecker >( 5 ) ); + estimationInput->saveStateHistoryForEachIteration_ = true; + + // Perform estimation + std::shared_ptr< EstimationOutput< long double, Time > > estimationOutput = orbitDeterminationManager.estimateParameters( + estimationInput ); + + // Retrieve post-fit state history + std::shared_ptr< propagators::SimulationResults< long double, Time > > postFitSimulationResults = + estimationOutput->getBestIterationSimulationResults( ); + std::map < Time, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > propagatedStateHistoryPostFitDynamic = + std::dynamic_pointer_cast< SingleArcVariationalSimulationResults< long double, Time > >( + postFitSimulationResults )->getDynamicsResults( )->getEquationsOfMotionNumericalSolution( ); + std::map < Time, Eigen::Matrix < long double, 6, 1 > > propagatedStateHistoryPostFit; + for ( auto it = propagatedStateHistoryPostFitDynamic.begin( ); it != propagatedStateHistoryPostFitDynamic.end( ); ++it ) + { + propagatedStateHistoryPostFit[ it->first ] = it->second; + } + std::shared_ptr< interpolators::OneDimensionalInterpolator< Time, Eigen::Matrix< long double, 6, 1 > > > postFitStateInterpolator = + propagators::createStateInterpolator< Time, long double >( propagatedStateHistoryPostFit ); + std::map< long double, Eigen::Matrix < long double, Eigen::Dynamic, 1 > > propagatedStateHistoryPostFitToWrite; + for ( Time t : observationTimes ) + { + propagatedStateHistoryPostFitToWrite[ t.getSeconds< long double >() ] = postFitStateInterpolator->interpolate( t ); + } +// for ( auto it = propagatedStateHistoryPostFit.begin( ); it != propagatedStateHistoryPostFit.end( ); ++it ) +// { +// propagatedStateHistoryPostFitToWrite[ it->first.getSeconds< long double >() ] = it->second; +// } + writeDataMapToTextFile( propagatedStateHistoryPostFitToWrite, "stateHistoryPropagatedPostFit_" + fileTag + ".txt", saveDirectory, + "", 18, 18 ); + + // Retrieve residuals and set them in matrix + Eigen::MatrixXd residualHistory = estimationOutput->getResidualHistoryMatrix( ); + Eigen::Matrix< long double, Eigen::Dynamic, Eigen::Dynamic > parameterHistory = estimationOutput->getParameterHistoryMatrix( ); + + Eigen::MatrixXd residualsWithTime; + residualsWithTime.resize( residualHistory.rows( ), residualHistory.cols( ) + 1 ); + residualsWithTime.rightCols( residualHistory.cols( ) ) = residualHistory; + + for ( unsigned int i = 0; i < observedObservationCollection->getObservationVector( ).size( ); ++i ) + { + residualsWithTime( i, 0 ) = static_cast< Time >( observedObservationCollection->getConcatenatedTimeVector( ).at( i ) + ).getSeconds< long double >(); + } + + std::ofstream file(saveDirectory + "residuals_" + fileTag + ".txt"); + file << std::setprecision( 17 ) << residualsWithTime; + file.close(); + + std::ofstream file3(saveDirectory + "parameters_" + fileTag + ".txt"); + file3 << std::setprecision( 21 ) << parameterHistory; + file3.close(); + + // Retrieve covariance matrix + Eigen::MatrixXd normalizedCovarianceMatrix = estimationOutput->getNormalizedCovarianceMatrix( ); + Eigen::MatrixXd unnormalizedCovarianceMatrix = estimationOutput->getUnnormalizedCovarianceMatrix( ); + std::ofstream file4(saveDirectory + "covariance_" + fileTag + ".txt"); + file4 << std::setprecision( 17 ) << "Normalized covariance matrix: " << std::endl << normalizedCovarianceMatrix; + file4 << std::endl << std::endl; + file4 << "Unnormalized covariance matrix: " << std::endl << unnormalizedCovarianceMatrix; + file4.close( ); + +} +