From 654d500ba3c522cd22f12089d55e91668de99fd7 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Wed, 8 Nov 2023 15:22:45 +0100 Subject: [PATCH] Playing with ODF-based fit --- ...fullRadiationPressureAccelerationPartial.h | 33 +- .../orbit_determination/podInputOutputTypes.h | 7 +- .../estimation_setup/observations.h | 2 + .../estimation_setup/processOdfFile.h | 1 - ...llRadiationPressureAccelerationPartial.cpp | 10 + .../astro/observation_models/CMakeLists.txt | 2 +- .../unitTestDsnOdfGrail.cpp | 289 ++++++++++++------ .../astro/orbit_determination/CMakeLists.txt | 6 + .../unitTestFitToSpice.cpp | 24 +- .../unitTestFitToSpiceGrail.cpp | 158 ++++++++++ 10 files changed, 420 insertions(+), 112 deletions(-) create mode 100644 tests/src/astro/orbit_determination/unitTestFitToSpiceGrail.cpp diff --git a/include/tudat/astro/orbit_determination/acceleration_partials/fullRadiationPressureAccelerationPartial.h b/include/tudat/astro/orbit_determination/acceleration_partials/fullRadiationPressureAccelerationPartial.h index 8cb5c26ad5..f2f8e914b5 100644 --- a/include/tudat/astro/orbit_determination/acceleration_partials/fullRadiationPressureAccelerationPartial.h +++ b/include/tudat/astro/orbit_determination/acceleration_partials/fullRadiationPressureAccelerationPartial.h @@ -172,6 +172,22 @@ class RadiationPressureAccelerationPartial: public AccelerationPartial customAccelerationPartialSet_->customDoubleParameterPartials_.at( parameter->getParameterName() ) ); parameterSize = 1; } + else if( parameter->getParameterName( ).first == estimatable_parameters::radiation_pressure_coefficient && + parameter->getParameterName( ).second.first == acceleratedBody_ ) + { + if ( std::dynamic_pointer_cast( + radiationPressureAcceleration_->getTargetModel( ) ) != nullptr ) + { + partialFunction = std::bind( &RadiationPressureAccelerationPartial::wrtRadiationPressureCoefficient, + this, std::placeholders::_1, std::dynamic_pointer_cast( + radiationPressureAcceleration_->getTargetModel( ) ) ); + } + else + { + throw std::runtime_error( "Error in radiation pressure partial for " + acceleratedBody_ + ", requested partial w.r.t. Cr, but no cannonball target found" ); + } + parameterSize = 1; + } return std::make_pair( partialFunction, parameterSize ); } @@ -187,13 +203,13 @@ class RadiationPressureAccelerationPartial: public AccelerationPartial { std::function< void( Eigen::MatrixXd& ) > partialFunction; int parameterSize = 0; - if( customAccelerationPartialSet_->customVectorParameterPartials_.count( parameter->getParameterName() )!= 0 ) - { - partialFunction = std::bind( &RadiationPressureAccelerationPartial::createCustomParameterPartialFunction, this, - std::placeholders::_1, - customAccelerationPartialSet_->customVectorParameterPartials_.at( parameter->getParameterName() ) ); - parameterSize = parameter->getParameterSize( ); - } +// if( customAccelerationPartialSet_->customVectorParameterPartials_.count( parameter->getParameterName() )!= 0 ) +// { +// partialFunction = std::bind( &RadiationPressureAccelerationPartial::createCustomParameterPartialFunction, this, +// std::placeholders::_1, +// customAccelerationPartialSet_->customVectorParameterPartials_.at( parameter->getParameterName() ) ); +// parameterSize = parameter->getParameterSize( ); +// } return std::make_pair( partialFunction, parameterSize ); } @@ -207,6 +223,9 @@ class RadiationPressureAccelerationPartial: public AccelerationPartial protected: + void wrtRadiationPressureCoefficient( + Eigen::MatrixXd& partial, std::shared_ptr< electromagnetism::CannonballRadiationPressureTargetModel > targetModel ); + std::shared_ptr< electromagnetism::PaneledSourceRadiationPressureAcceleration > radiationPressureAcceleration_; std::shared_ptr< estimatable_parameters::CustomSingleAccelerationPartialCalculatorSet > customAccelerationPartialSet_; diff --git a/include/tudat/astro/orbit_determination/podInputOutputTypes.h b/include/tudat/astro/orbit_determination/podInputOutputTypes.h index 800b646845..89ba0a0b4e 100644 --- a/include/tudat/astro/orbit_determination/podInputOutputTypes.h +++ b/include/tudat/astro/orbit_determination/podInputOutputTypes.h @@ -195,7 +195,7 @@ class CovarianceAnalysisInput } } - //! Set constant vector weight for all observables of given type and link ends + //! Set constant vector weight for alweightsMatrixDiagonals_l observables of given type and link ends void setTabulatedSingleObservableAndLinkEndsWeights( const observation_models::ObservableType currentObservable, const observation_models::LinkEnds currentLinkEnds, @@ -443,6 +443,11 @@ class CovarianceAnalysisInput return weightsMatrixDiagonals_; } + void setWeightsMatrixDiagonals( const Eigen::VectorXd& weightsMatrixDiagonals ) + { + weightsMatrixDiagonals_ = weightsMatrixDiagonals; + } + //! Function to return the boolean denoting whether the dynamics and variational equations are reintegrated on first iteration /*! * Function to return the boolean denoting whether the dynamics and variational equations are to be reintegrated on first diff --git a/include/tudat/simulation/estimation_setup/observations.h b/include/tudat/simulation/estimation_setup/observations.h index 065bf2727d..fce442f403 100644 --- a/include/tudat/simulation/estimation_setup/observations.h +++ b/include/tudat/simulation/estimation_setup/observations.h @@ -795,6 +795,7 @@ std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeT return splitSingleObervationSet; } + template< typename ObservationScalarType = double, typename TimeType = double, typename std::enable_if< is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, int >::type = 0 > std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > splitObservationSetsIntoArcs( @@ -821,6 +822,7 @@ std::shared_ptr< ObservationCollection< ObservationScalarType, TimeType > > spli return std::make_shared< ObservationCollection< ObservationScalarType, TimeType > >( splitObservationSets ); } + template< typename ObservationScalarType = double, typename TimeType = double, typename std::enable_if< is_state_scalar_and_time_type< ObservationScalarType, TimeType >::value, int >::type = 0 > std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > getObservationListWithDependentVariables( diff --git a/include/tudat/simulation/estimation_setup/processOdfFile.h b/include/tudat/simulation/estimation_setup/processOdfFile.h index 9076c58ce8..f54e5acd47 100644 --- a/include/tudat/simulation/estimation_setup/processOdfFile.h +++ b/include/tudat/simulation/estimation_setup/processOdfFile.h @@ -766,7 +766,6 @@ std::shared_ptr< observation_models::SingleObservationSet< ObservationScalarType } else { - std::cout<<"SKIP"< targetModel ) +{ + if( targetModel->getCoefficient( ) == 0.0 ) + { + throw std::runtime_error( "Error in full radiation pressure partial w.r.t. Cr, partial is only implemented for non-zero coefficient" ); + } + partial = radiationPressureAcceleration_->getAcceleration( ) / targetModel->getCoefficient( ); +} + } } diff --git a/tests/src/astro/observation_models/CMakeLists.txt b/tests/src/astro/observation_models/CMakeLists.txt index f24bfd9f15..993eb572c3 100644 --- a/tests/src/astro/observation_models/CMakeLists.txt +++ b/tests/src/astro/observation_models/CMakeLists.txt @@ -63,7 +63,7 @@ TUDAT_ADD_TEST_CASE(TimeBias PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) # #TUDAT_ADD_TEST_CASE(DsnOdfMro PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) # -#TUDAT_ADD_TEST_CASE(DsnOdfGrail PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) +TUDAT_ADD_TEST_CASE(DsnOdfGrail PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) #TUDAT_ADD_TEST_CASE(DsnNWayDopplerObservationModel PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) diff --git a/tests/src/astro/observation_models/unitTestDsnOdfGrail.cpp b/tests/src/astro/observation_models/unitTestDsnOdfGrail.cpp index 7c2473b331..25f20a96d6 100644 --- a/tests/src/astro/observation_models/unitTestDsnOdfGrail.cpp +++ b/tests/src/astro/observation_models/unitTestDsnOdfGrail.cpp @@ -17,6 +17,7 @@ #include "tudat/basics/testMacros.h" #include "tudat/simulation/estimation.h" #include "tudat/simulation/estimation_setup.h" +#include "tudat/simulation/estimation_setup/createEstimatableParameters.h" #include "tudat/io/readOdfFile.h" #include "tudat/io/readTabulatedMediaCorrections.h" @@ -36,17 +37,27 @@ using namespace tudat::observation_models; using namespace tudat::simulation_setup; using namespace tudat::numerical_integrators; using namespace tudat::basic_astrodynamics; +using namespace tudat::reference_frames; using namespace tudat; int main( ) { - double initialTimeEnvironment = Time(107561, 2262.19) - 2.0 * 3600.0; - double finalTimeEnvironment = Time(108258, 2771.19) + 2.0 * 3600.0; + double initialTimeEnvironment = DateTime( 2012, 03, 02, 0, 0, 0.0 ).epoch< double >(); + double finalTimeEnvironment = DateTime( 2012, 05, 29, 0, 0, 0.0 ).epoch< double >(); + +// DateTime initialDateTime = basic_astrodynamics::getCalendarDateFromTime< double >( initialTimeEnvironment ); +// DateTime finalDateTime = basic_astrodynamics::getCalendarDateFromTime< double >( finalTimeEnvironment ); + + /**************************************************************************************** + ************************** CREATE EVIRONMENT + *****************************************************************************************/ // Load spice kernels spice_interface::loadStandardSpiceKernels( ); spice_interface::loadSpiceKernelInTudat( "/home/dominic/Tudat/Data/GRAIL_Spice/grail_120301_120529_sci_v02.bsp" ); + spice_interface::loadSpiceKernelInTudat( get_spice_kernels_path( ) + + "/moon_de440_200625.tf"); + spice_interface::loadSpiceKernelInTudat( get_spice_kernels_path( ) + + "/moon_pa_de440_200625.bpc"); // Create settings for default bodies std::vector< std::string > bodiesToCreate = { "Earth", "Sun", "Mercury", "Venus", "Mars", "Jupiter", "Saturn", "Moon" }; @@ -66,8 +77,28 @@ int main( ) std::make_shared< interpolators::InterpolatorGenerationSettings< double > >( interpolators::cubicSplineInterpolation( ), initialTimeEnvironment, finalTimeEnvironment, 60.0 )); bodySettings.at( "Earth" )->groundStationSettings = getDsnStationSettings( ); + bodySettings.at( "Moon" )->rotationModelSettings = spiceRotationModelSettings( + bodySettings.getFrameOrientation( ), "MOON_PA_DE440", "MOON_PA_DE440" ); + bodySettings.at( "Moon" )->gravityFieldSettings = + std::make_shared< FromFileSphericalHarmonicsGravityFieldSettings >( gggrx1200, 500 ); + std::dynamic_pointer_cast< SphericalHarmonicsGravityFieldSettings >( + bodySettings.at( "Moon" )->gravityFieldSettings )->resetAssociatedReferenceFrame( "MOON_PA_DE440" ); + bodySettings.at( "Moon" )->gravityFieldVariationSettings.push_back( + fixedSingleDegreeLoveNumberGravityFieldVariationSettings( "Earth", 0.02405, 2 ) ); + bodySettings.at( "Moon" )->gravityFieldVariationSettings.push_back( + fixedSingleDegreeLoveNumberGravityFieldVariationSettings( "Sun", 0.02405, 2 ) ); bodySettings.at( "Moon" )->ephemerisSettings->resetFrameOrigin( "Earth" ); + std::vector< std::shared_ptr< PanelRadiosityModelSettings > > panelRadiosityModels; + panelRadiosityModels.push_back(angleBasedThermalPanelRadiosityModelSettings( 95.0, 385.0, 0.95, "Sun" ) ); + panelRadiosityModels.push_back(albedoPanelRadiosityModelSettings( SphericalHarmonicsSurfacePropertyDistributionModel::albedo_dlam1, "Sun" ) ); + + std::map< std::string, std::vector< std::string > > originalSourceToSourceOccultingBodies; + originalSourceToSourceOccultingBodies[ "Sun" ].push_back( "Earth" ); + bodySettings.at( "Moon" )->radiationSourceModelSettings = + extendedRadiationSourceModelSettingsWithOccultationMap( + panelRadiosityModels, { 4, 8, 12, 16 }, originalSourceToSourceOccultingBodies ); + // Add spacecraft settings std::string spacecraftName = "GRAIL-A"; std::string spacecraftCentralBody = "Moon"; @@ -75,36 +106,29 @@ int main( ) bodySettings.at( spacecraftName )->ephemerisSettings = std::make_shared< InterpolatedSpiceEphemerisSettings >( initialTimeEnvironment, finalTimeEnvironment, 10.0, spacecraftCentralBody, globalFrameOrientation ); -// 107560 * 3600.0 , 107579.0 * 3600.0, 10.0, spacecraftCentralBody, globalFrameOrientation ); - - -// std::make_shared< DirectSpiceEphemerisSettings >( spacecraftCentralBody, "J2000" ); // Create spacecraft - bodySettings.at( spacecraftName )->constantMass = 1200.0; + bodySettings.at( spacecraftName )->constantMass = 150.0; // Create radiation pressure settings - double referenceAreaRadiation = 10.0; - double radiationPressureCoefficient = 1.2; - std::vector< std::string > occultingBodies = { "Mars" }; - bodySettings.at( spacecraftName )->radiationPressureSettings[ "Sun" ] = - cannonBallRadiationPressureSettings( "Sun", referenceAreaRadiation, radiationPressureCoefficient, occultingBodies ); + double referenceAreaRadiation = 5.0; + double radiationPressureCoefficient = 1.5; + std::map< std::string, std::vector< std::string > > sourceToTargetOccultingBodies; + sourceToTargetOccultingBodies[ "Sun" ].push_back( "Moon" ); + bodySettings.at( spacecraftName )->radiationPressureTargetModelSettings = cannonballRadiationPressureTargetModelSettingsWithOccultationMap( + referenceAreaRadiation, radiationPressureCoefficient, sourceToTargetOccultingBodies ); // Create bodies SystemOfBodies bodies = createSystemOfBodies< long double, Time >( bodySettings ); + + /**************************************************************************************** + ************************** LOAD ODF FILES + *****************************************************************************************/ + // Define ODF data paths std::string dataDirectory = "/home/dominic/Tudat/Data/GRAIL_ODF/"; - std::vector< std::string > odfFiles = { "gralugf2012_100_0540smmmv1.odf", "gralugf2012_101_0235smmmv1.odf", - "gralugf2012_102_0358smmmv1.odf", "gralugf2012_103_0145smmmv1.odf", - "gralugf2012_105_0352smmmv1.odf", "gralugf2012_107_0405smmmv1.odf", - "gralugf2012_108_0450smmmv1.odf", "gralugf2012_109_1227smmmv1.odf", - "gralugf2012_111_1332smmmv1.odf", "gralugf2012_114_0900smmmv1.odf" }; -// "gralugf2012_117_0300smmmv1.odf", "gralugf2012_119_0545smmmv1.odf", -// "gralugf2012_121_0155smmmv1.odf", "gralugf2012_122_1113smmmv1.odf", -// "gralugf2012_123_0607smmmv1.odf", "gralugf2012_124_0253smmmv1.odf", -// "gralugf2012_125_0400smmmv1.odf", "gralugf2012_126_0203smmmv1.odf", -// "gralugf2012_128_0233smmmv1.odf"}; + std::vector< std::string > odfFiles = { "gralugf2012_097_0235smmmv1.odf" }; // Laod raw ODF data std::vector< std::shared_ptr< input_output::OdfRawFileContents > > rawOdfDataVector; @@ -120,12 +144,31 @@ int main( ) observation_models::setOdfInformationInBodies( processedOdfFileContents, bodies ); // Create data structure that handles Observed Data in Tudat - std::shared_ptr< observation_models::ObservationCollection< long double, Time > > observedObservationCollection = + std::shared_ptr< observation_models::ObservationCollection< long double, Time > > observedUncompressedObservationCollection = splitObservationSetsIntoArcs< long double, Time >( observation_models::createOdfObservedObservationCollection< long double, Time >( processedOdfFileContents, { dsn_n_way_averaged_doppler } ), 60.0, 10 ); + std::map< LinkEnds, std::vector< std::shared_ptr< observation_models::SingleObservationSet< long double, Time > > > > uncompressedObservationSets = + observedUncompressedObservationCollection->getObservations( ).at( dsn_n_way_averaged_doppler ); + std::vector< std::shared_ptr< observation_models::SingleObservationSet< long double, Time > > > compressedObservationSets; + for( auto it : uncompressedObservationSets ) + { + for( unsigned int i = 0; i < it.second.size( ); i++ ) + { + compressedObservationSets.push_back( compressDopplerData< long double, Time >( it.second.at( i ), 60 ) ); + } + } - std::map< int, observation_models::LinkEnds > linkEndIds = observedObservationCollection->getInverseLinkEndIdentifierMap( ); + std::shared_ptr< observation_models::ObservationCollection< long double, Time > > observedObservationCollection = + std::make_shared< observation_models::ObservationCollection< long double, Time > >( compressedObservationSets ); + + + /**************************************************************************************** + ************************** PRINT DATA SUMMARY + *****************************************************************************************/ + + + std::map< int, observation_models::LinkEnds > linkEndIds = observedObservationCollection->getInverseLinkEndIdentifierMap( ); for( auto it : linkEndIds ) { std::cout< > observationModelSettingsList; - - std::vector< std::shared_ptr< observation_models::LightTimeCorrectionSettings > > lightTimeCorrectionSettings; lightTimeCorrectionSettings.push_back( firstOrderRelativisticLightTimeCorrectionSettings( { "Sun" } ) ); std::vector< std::string > troposphericCorrectionFileNames = @@ -156,7 +202,6 @@ int main( ) {"/home/dominic/Tudat/Data/GRAIL_Ancilliary/gralugf2012_092_2012_122.ion", "/home/dominic/Tudat/Data/GRAIL_Ancilliary/gralugf2012_122_2012_153.ion"}; std::map< int, std::string > spacecraftNamePerSpacecraftId; spacecraftNamePerSpacecraftId[ 177 ] = "GRAIL-A"; - spacecraftNamePerSpacecraftId[ 181 ] = "GRAIL-B"; lightTimeCorrectionSettings.push_back( tabulatedTroposphericCorrectionSettings( troposphericCorrectionFileNames ) ); lightTimeCorrectionSettings.push_back( tabulatedIonosphericCorrectionSettings( ionosphericCorrectionFileNames, spacecraftNamePerSpacecraftId ) ); @@ -181,98 +226,162 @@ int main( ) std::vector< std::shared_ptr< ObservationSimulatorBase< long double, Time > > > observationSimulators = createObservationSimulators< long double, Time >( observationModelSettingsList, bodies ); + + /**************************************************************************************** + ************************** SIMULATE OBSERVATIONS AND COMPUTE RESIDUALS + *****************************************************************************************/ + std::vector< std::shared_ptr< simulation_setup::ObservationSimulationSettings< Time > > > observationSimulationSettings = getObservationSimulationSettingsFromObservations( observedObservationCollection ); std::shared_ptr< observation_models::ObservationCollection< long double, Time > > computedObservationCollection = simulateObservations( observationSimulationSettings, observationSimulators, bodies ); - std::cout<<"Create residuals"< > residualObservationCollection = createResidualCollection( observedObservationCollection, computedObservationCollection ); - std::cout<<"Filter observations"< residualCutoffValuePerObservable; residualCutoffValuePerObservable[ dsn_n_way_averaged_doppler ] = 0.05; std::shared_ptr< observation_models::ObservationCollection< long double, Time > > filteredObservedObservationCollection = filterResidualOutliers( observedObservationCollection, residualObservationCollection, residualCutoffValuePerObservable ); - std::cout<<"Filtered size: "<getTotalObservableSize( )<<" "<getTotalObservableSize( )< > > filteredObservationSimulationSettings = getObservationSimulationSettingsFromObservations( filteredObservedObservationCollection ); std::shared_ptr< observation_models::ObservationCollection< long double, Time > > filteredComputedObservationCollection = simulateObservations( filteredObservationSimulationSettings, observationSimulators, bodies ); - std::cout<<"Create filtered residuals"< > filteredResidualObservationCollection = createResidualCollection( filteredObservedObservationCollection, filteredComputedObservationCollection ); - - Eigen::VectorXd numericalTimeBiasPartials = getNumericalObservationTimePartial< long double, Time >( - filteredObservationSimulationSettings, observationSimulators, bodies, 5.0 ); - - - input_output::writeMatrixToFile( numericalTimeBiasPartials, "grailTestTimeDerivative.dat", 16, "/home/dominic/Tudat/Data/GRAIL_TestResults/"); - - std::vector< double > timeBiases; - std::vector< Eigen::VectorXd > polynomialCoefficientsList; - Eigen::VectorXd correctedResiduals; - estimateTimeBiasAndPolynomialFitPerSet< long double, Time >( - filteredResidualObservationCollection, numericalTimeBiasPartials, timeBiases, polynomialCoefficientsList, correctedResiduals ); - - Eigen::VectorXd startTimes; - Eigen::VectorXd durations; - Eigen::VectorXd meanValues; - Eigen::VectorXd rmsValues; - - getResidualStatistics( - filteredResidualObservationCollection, correctedResiduals, startTimes, durations, meanValues, rmsValues ); - -// for( unsigned int i = 0; i < timeBiases.size( ); i++ ) -// { -// std::cout< residualCutoffValuePerObservable; -// residualCutoffValuePerObservable[ dsn_n_way_averaged_doppler ] = 0.3; -// std::shared_ptr< observation_models::ObservationCollection< long double, Time > > filteredObservedObservationCollection = -// filterResidualOutliers( observedObservationCollection, residualObservationCollection, residualCutoffValuePerObservable ); -// -// -// std::cout<<"Computed filtered observations"< > > filteredObservationSimulationSettings = -// getObservationSimulationSettingsFromObservations( filteredObservedObservationCollection ); -// std::shared_ptr< observation_models::ObservationCollection< long double, Time > > filteredComputedObservationCollection = -// simulateObservations( filteredObservationSimulationSettings, observationSimulators, bodies ); -// -// std::cout<<"Create filtered residuals"< > filteredResidualObservationCollection = -// createResidualCollection( filteredObservedObservationCollection, filteredComputedObservationCollection ); - - - { Eigen::VectorXd residuals = filteredResidualObservationCollection->getObservationVector( ).template cast< double >( ); - input_output::writeMatrixToFile( residuals, "grailTestResiduals.dat", 16, "/home/dominic/Tudat/Data/GRAIL_TestResults/"); + input_output::writeMatrixToFile( residuals, "grailTestResiduals.dat", 16, "/home/dominic/Tudat/Data/GRAIL_TestResults_30s_timebias/"); - input_output::writeMatrixToFile( correctedResiduals, "grailTestCorrectedResiduals.dat", 16, "/home/dominic/Tudat/Data/GRAIL_TestResults/"); +// input_output::writeMatrixToFile( correctedResiduals, "grailTestCorrectedResiduals.dat", 16, "/home/dominic/Tudat/Data/GRAIL_TestResults/"); Eigen::VectorXd observationTimes = utilities::convertStlVectorToEigenVector( filteredResidualObservationCollection->getConcatenatedTimeVector( ) ).template cast< double >( ); - input_output::writeMatrixToFile( observationTimes, "grailTestTimes.dat", 16, "/home/dominic/Tudat/Data/GRAIL_TestResults/"); + input_output::writeMatrixToFile( observationTimes, "grailTestTimes.dat", 16, "/home/dominic/Tudat/Data/GRAIL_TestResults_30s_timebias/"); Eigen::VectorXd observationLinkEndsIds = utilities::convertStlVectorToEigenVector( filteredResidualObservationCollection->getConcatenatedLinkEndIds( ) ).template cast< double >( ); - input_output::writeMatrixToFile(observationLinkEndsIds , "grailTestLinkEnds.dat", 16, "/home/dominic/Tudat/Data/GRAIL_TestResults/"); + input_output::writeMatrixToFile(observationLinkEndsIds , "grailTestLinkEnds.dat", 16, "/home/dominic/Tudat/Data/GRAIL_TestResults_30s_timebias/"); + } + + { + // Set accelerations between bodies that are to be taken into account. + SelectedAccelerationMap accelerationMap; + std::map > > accelerationsOfSpacecraft; + accelerationsOfSpacecraft[ "Sun" ].push_back( std::make_shared( point_mass_gravity )); + accelerationsOfSpacecraft[ "Sun" ].push_back( std::make_shared( radiation_pressure )); + accelerationsOfSpacecraft[ "Earth" ].push_back( std::make_shared( point_mass_gravity )); + accelerationsOfSpacecraft[ "Moon" ].push_back( std::make_shared( 256, 256 )); +// accelerationsOfSpacecraft[ "Moon" ].push_back( std::make_shared( radiation_pressure )); + accelerationsOfSpacecraft[ "Moon" ].push_back( empiricalAcceleration( )); + accelerationsOfSpacecraft[ "Mars" ].push_back( std::make_shared( point_mass_gravity )); + accelerationsOfSpacecraft[ "Jupiter" ].push_back( std::make_shared( point_mass_gravity )); + accelerationsOfSpacecraft[ "Saturn" ].push_back( std::make_shared( point_mass_gravity )); + + accelerationMap[ "GRAIL-A" ] =accelerationsOfSpacecraft; + + // Set bodies for which initial state is to be estimated and integrated. + std::vector bodiesToEstimate = { "GRAIL-A" }; + std::vector centralBodies = { "Moon" }; + + AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodies, accelerationMap, bodiesToEstimate, centralBodies ); + + Eigen::VectorXd initialState = getInitialStatesOfBodies( bodiesToEstimate, centralBodies, bodies, initialTime ); + + std::shared_ptr< TranslationalStatePropagatorSettings< long double, Time > > propagatorSettings = + std::make_shared< TranslationalStatePropagatorSettings< long double, Time > > + ( centralBodies, accelerationModelMap, bodiesToEstimate, initialState.template cast< long double >( ), Time( initialTime ), + numerical_integrators::rungeKuttaFixedStepSettings< Time >( 30.0, + numerical_integrators::rungeKuttaFehlberg78 ), + std::make_shared< PropagationTimeTerminationSettings >( finalTime ) ); + + std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames = + getInitialStateParameterSettings< long double, Time >( propagatorSettings, bodies ); + + std::vector > additionalParameterNames; + + parameterNames.push_back( estimatable_parameters::radiationPressureCoefficient( "GRAIL-A" )); + + std::map > empiricalComponentsToEstimate; + empiricalComponentsToEstimate[ radial_empirical_acceleration_component ].push_back( constant_empirical ); + empiricalComponentsToEstimate[ radial_empirical_acceleration_component ].push_back( sine_empirical ); + empiricalComponentsToEstimate[ radial_empirical_acceleration_component ].push_back( cosine_empirical ); + + empiricalComponentsToEstimate[ along_track_empirical_acceleration_component ].push_back( constant_empirical ); + empiricalComponentsToEstimate[ along_track_empirical_acceleration_component ].push_back( sine_empirical ); + empiricalComponentsToEstimate[ along_track_empirical_acceleration_component ].push_back( cosine_empirical ); + + empiricalComponentsToEstimate[ across_track_empirical_acceleration_component ].push_back( constant_empirical ); + empiricalComponentsToEstimate[ across_track_empirical_acceleration_component ].push_back( sine_empirical ); + empiricalComponentsToEstimate[ across_track_empirical_acceleration_component ].push_back( cosine_empirical ); + + parameterNames.push_back( std::make_shared( + "GRAIL-A", "Moon", empiricalComponentsToEstimate )); + for( auto it : linkEndIds ) + { + parameterNames.push_back( timeObservationBias( it.second, dsn_n_way_averaged_doppler ) ); + } + + std::shared_ptr< estimatable_parameters::EstimatableParameterSet< long double > > parametersToEstimate = + createParametersToEstimate< long double, Time >( parameterNames, bodies, propagatorSettings ); + + OrbitDeterminationManager< long double, Time > orbitDeterminationManager = OrbitDeterminationManager< long double, Time >( + bodies, parametersToEstimate, observationModelSettingsList, propagatorSettings ); + + std::shared_ptr< EstimationInput< long double, Time > > estimationInput = std::make_shared< EstimationInput< long double, Time > >( + filteredObservedObservationCollection ); + estimationInput->setConvergenceChecker( std::make_shared< EstimationConvergenceChecker >( 4 ) ); + estimationInput->defineEstimationSettings( + 0, 1, 0, 1, 1, 1 ); + std::shared_ptr< EstimationOutput< long double, Time > > estimationOutput = orbitDeterminationManager.estimateParameters( estimationInput ); + + input_output::writeMatrixToFile(estimationOutput->residuals_ , "grailPostFitResiduals.dat", 16, "/home/dominic/Tudat/Data/GRAIL_TestResults_30s_timebias/"); + + auto estimatedStateHistory = + std::dynamic_pointer_cast< SingleArcVariationalSimulationResults< long double, Time > >( estimationOutput->getSimulationResults( ).back( ) )->getDynamicsResults( )->getEquationsOfMotionNumericalSolution( ); + + std::map< double, Eigen::VectorXd > finalStateHistory; + std::map< double, Eigen::VectorXd > finalStateDifference; + std::map< double, Eigen::VectorXd > finalStateDifferenceRsw; + for( auto it : estimatedStateHistory ) + { + finalStateHistory[ it.first ] = it.second.cast< double >( ); + Eigen::Matrix3d rotationMatrix = getInertialToRswSatelliteCenteredFrameRotationMatrix( it.second.cast< double >( ) ); + Eigen::VectorXd stateDifference = it.second.cast< double >( ) - spice_interface::getBodyCartesianStateAtEpoch( + "GRAIL-A", "Moon", globalFrameOrientation, "None", it.first ); + Eigen::VectorXd rswStateDifference = Eigen::Vector6d::Zero( ); + rswStateDifference.segment( 0, 3 ) = rotationMatrix * stateDifference.segment( 0, 3 ); + rswStateDifference.segment( 3, 3 ) = rotationMatrix * stateDifference.segment( 3, 3 ); + + finalStateDifference[ it.first ] = stateDifference; + finalStateDifferenceRsw[ it.first ] = rswStateDifference; + } + + input_output::writeMatrixToFile(estimationOutput->getCorrelationMatrix( ) , "grailTestCorrelations.dat", 16, "/home/dominic/Tudat/Data/GRAIL_TestResults_30s_timebias/"); + + input_output::writeDataMapToTextFile( finalStateDifference, + "stateDifference.dat", + "/home/dominic/Tudat/Data/GRAIL_TestResults_30s_timebias/", + "", std::numeric_limits< double >::digits10, std::numeric_limits< double >::digits10, "," ); + + input_output::writeDataMapToTextFile( finalStateDifferenceRsw, + "stateDifferenceRsw.dat", + "/home/dominic/Tudat/Data/GRAIL_TestResults_30s_timebias/", + "", std::numeric_limits< double >::digits10, std::numeric_limits< double >::digits10, "," ); + + + + + } } \ No newline at end of file diff --git a/tests/src/astro/orbit_determination/CMakeLists.txt b/tests/src/astro/orbit_determination/CMakeLists.txt index e885eeacfe..750aa3c29e 100644 --- a/tests/src/astro/orbit_determination/CMakeLists.txt +++ b/tests/src/astro/orbit_determination/CMakeLists.txt @@ -102,6 +102,7 @@ TUDAT_ADD_TEST_CASE(NonSequentialStateEstimation # PRIVATE_LINKS # ${Tudat_ESTIMATION_LIBRARIES} # ) + TUDAT_ADD_TEST_CASE(ConsiderParameters PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES} @@ -112,6 +113,11 @@ TUDAT_ADD_TEST_CASE(NonSequentialStateEstimation ${Tudat_ESTIMATION_LIBRARIES} ) + TUDAT_ADD_TEST_CASE(FitToSpiceGrail + PRIVATE_LINKS + ${Tudat_ESTIMATION_LIBRARIES} + ) + TUDAT_ADD_TEST_CASE(CustomAccelerationVariationalEquations PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES} diff --git a/tests/src/astro/orbit_determination/unitTestFitToSpice.cpp b/tests/src/astro/orbit_determination/unitTestFitToSpice.cpp index 21417ea486..6940047ac1 100644 --- a/tests/src/astro/orbit_determination/unitTestFitToSpice.cpp +++ b/tests/src/astro/orbit_determination/unitTestFitToSpice.cpp @@ -112,17 +112,17 @@ int main( ) std::map > empiricalComponentsToEstimate; -// empiricalComponentsToEstimate[ radial_empirical_acceleration_component ].push_back( constant_empirical ); -// empiricalComponentsToEstimate[ radial_empirical_acceleration_component ].push_back( sine_empirical ); -// empiricalComponentsToEstimate[ radial_empirical_acceleration_component ].push_back( cosine_empirical ); -// + empiricalComponentsToEstimate[ radial_empirical_acceleration_component ].push_back( constant_empirical ); + empiricalComponentsToEstimate[ radial_empirical_acceleration_component ].push_back( sine_empirical ); + empiricalComponentsToEstimate[ radial_empirical_acceleration_component ].push_back( cosine_empirical ); + empiricalComponentsToEstimate[ along_track_empirical_acceleration_component ].push_back( constant_empirical ); -// empiricalComponentsToEstimate[ along_track_empirical_acceleration_component ].push_back( sine_empirical ); -// empiricalComponentsToEstimate[ along_track_empirical_acceleration_component ].push_back( cosine_empirical ); -// -// empiricalComponentsToEstimate[ across_track_empirical_acceleration_component ].push_back( constant_empirical ); -// empiricalComponentsToEstimate[ across_track_empirical_acceleration_component ].push_back( sine_empirical ); -// empiricalComponentsToEstimate[ across_track_empirical_acceleration_component ].push_back( cosine_empirical ); + empiricalComponentsToEstimate[ along_track_empirical_acceleration_component ].push_back( sine_empirical ); + empiricalComponentsToEstimate[ along_track_empirical_acceleration_component ].push_back( cosine_empirical ); + + empiricalComponentsToEstimate[ across_track_empirical_acceleration_component ].push_back( constant_empirical ); + empiricalComponentsToEstimate[ across_track_empirical_acceleration_component ].push_back( sine_empirical ); + empiricalComponentsToEstimate[ across_track_empirical_acceleration_component ].push_back( cosine_empirical ); additionalParameterNames.push_back( std::make_shared( "MGS", "Mars", empiricalComponentsToEstimate )); @@ -130,9 +130,9 @@ int main( ) std::shared_ptr > estimationOutput = createBestFitToCurrentEphemeris( bodies, accelerationModelMap, bodiesToEstimate, centralBodies, - numerical_integrators::rungeKuttaFixedStepSettings( 120.0, + numerical_integrators::rungeKuttaFixedStepSettings( 60.0, numerical_integrators::rungeKuttaFehlberg78 ), - initialTime, finalTime, 600.0, additionalParameterNames ); + initialTime, finalTime, 120.0, additionalParameterNames ); } diff --git a/tests/src/astro/orbit_determination/unitTestFitToSpiceGrail.cpp b/tests/src/astro/orbit_determination/unitTestFitToSpiceGrail.cpp new file mode 100644 index 0000000000..7ee83ec1c5 --- /dev/null +++ b/tests/src/astro/orbit_determination/unitTestFitToSpiceGrail.cpp @@ -0,0 +1,158 @@ +/* Copyright (c) 2010-2019, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + */ +// +//#define BOOST_TEST_DYN_LINK +//#define BOOST_TEST_MAIN + +#include +#include + +#include + +#include + +#include "tudat/simulation/estimation_setup/fitOrbitToEphemeris.h" +// +// +//namespace tudat +//{ +//namespace unit_tests +//{ +//BOOST_AUTO_TEST_SUITE( test_fit_to_spice ) + +//Using declarations. +using namespace tudat; +using namespace tudat::observation_models; +using namespace tudat::orbit_determination; +using namespace tudat::estimatable_parameters; +using namespace tudat::simulation_setup; +using namespace tudat::basic_astrodynamics; +// +//BOOST_AUTO_TEST_CASE( test_FitToSpice ) +//{ +int main( ) +{ + + // Load spice kernels + spice_interface::loadStandardSpiceKernels( ); + spice_interface::loadSpiceKernelInTudat( "/home/dominic/Tudat/Data/GRAIL_Spice/grail_120301_120529_sci_v02.bsp" ); + spice_interface::loadSpiceKernelInTudat( get_spice_kernels_path( ) + + "/moon_de440_200625.tf"); + spice_interface::loadSpiceKernelInTudat( get_spice_kernels_path( ) + + "/moon_pa_de440_200625.bpc"); + + double initialTimeEnvironment = DateTime( 2012, 03, 02, 0, 0, 0.0 ).epoch< double >(); + double finalTimeEnvironment = DateTime( 2012, 05, 29, 0, 0, 0.0 ).epoch< double >(); + + DateTime initialDateTime = basic_astrodynamics::getCalendarDateFromTime< double >( initialTimeEnvironment ); + DateTime finalDateTime = basic_astrodynamics::getCalendarDateFromTime< double >( finalTimeEnvironment ); + + double initialPropagationTime = DateTime( 2012, 04, 07, 0, 0, 0.0 ).epoch< double >(); + double finalPropagtionTime = initialPropagationTime + 86400.0; + std::vector< std::string > bodyNames; + bodyNames.push_back( "Earth" ); + bodyNames.push_back( "Mars" ); + bodyNames.push_back( "Sun" ); + bodyNames.push_back( "Moon" ); + bodyNames.push_back( "Jupiter" ); + bodyNames.push_back( "Saturn" ); + + BodyListSettings bodySettings = + getDefaultBodySettings( bodyNames, "Mars" ); + bodySettings.at( "Moon" )->rotationModelSettings = spiceRotationModelSettings( + bodySettings.getFrameOrientation( ), "MOON_PA_DE440", "MOON_PA_DE440" ); + bodySettings.at( "Moon" )->gravityFieldSettings = + std::make_shared< FromFileSphericalHarmonicsGravityFieldSettings >( gggrx1200, 500 ); + std::dynamic_pointer_cast< SphericalHarmonicsGravityFieldSettings >( + bodySettings.at( "Moon" )->gravityFieldSettings )->resetAssociatedReferenceFrame( "MOON_PA_DE440" ); + bodySettings.at( "Moon" )->gravityFieldVariationSettings.push_back( + fixedSingleDegreeLoveNumberGravityFieldVariationSettings( "Earth", 0.02405, 2 ) ); + bodySettings.at( "Moon" )->gravityFieldVariationSettings.push_back( + fixedSingleDegreeLoveNumberGravityFieldVariationSettings( "Sun", 0.02405, 2 ) ); + + bodySettings.addSettings( "GRAIL-A" ); + bodySettings.at( "GRAIL-A" )->ephemerisSettings = std::make_shared< InterpolatedSpiceEphemerisSettings >( + initialTimeEnvironment + 3600.0, finalTimeEnvironment - 3600.0, 30.0, "Moon" ); + + // Create bodies needed in simulation + SystemOfBodies bodies = createSystemOfBodies( bodySettings ); + + // Create radiation pressure settings + double referenceAreaRadiation = 16.0; + double radiationPressureCoefficient = 1.5; + std::vector occultingBodies; + occultingBodies.push_back( "Moon" ); + std::shared_ptr grailRadiationPressureSettings = + std::make_shared( + "Sun", referenceAreaRadiation, radiationPressureCoefficient, occultingBodies ); + + // Create and set radiation pressure settings + bodies.at( "GRAIL-A" )->setRadiationPressureInterface( + "Sun", createRadiationPressureInterface( + grailRadiationPressureSettings, "GRAIL-A", bodies )); + bodies.at( "GRAIL-A" )->setConstantBodyMass( 2000.0 ); + + // Set accelerations between bodies that are to be taken into account. + SelectedAccelerationMap accelerationMap; + std::map > > accelerationsOfSpacecraft; + accelerationsOfSpacecraft[ "Sun" ].push_back( std::make_shared( point_mass_gravity )); + accelerationsOfSpacecraft[ "Sun" ].push_back( std::make_shared( cannon_ball_radiation_pressure )); + accelerationsOfSpacecraft[ "Earth" ].push_back( std::make_shared( point_mass_gravity )); + accelerationsOfSpacecraft[ "Moon" ].push_back( std::make_shared( 256, 256 )); + accelerationsOfSpacecraft[ "Moon" ].push_back( empiricalAcceleration( )); + accelerationsOfSpacecraft[ "Mars" ].push_back( std::make_shared( point_mass_gravity )); + accelerationsOfSpacecraft[ "Jupiter" ].push_back( std::make_shared( point_mass_gravity )); + accelerationsOfSpacecraft[ "Saturn" ].push_back( std::make_shared( point_mass_gravity )); + + accelerationMap[ "GRAIL-A" ] =accelerationsOfSpacecraft; + + // Set bodies for which initial state is to be estimated and integrated. + std::vector bodiesToEstimate = { "GRAIL-A" }; + std::vector centralBodies = { "Moon" }; + + AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodies, accelerationMap, bodiesToEstimate, centralBodies ); + + std::vector > additionalParameterNames; + + additionalParameterNames.push_back( estimatable_parameters::radiationPressureCoefficient( "GRAIL-A" )); + + std::map > empiricalComponentsToEstimate; +// empiricalComponentsToEstimate[ radial_empirical_acceleration_component ].push_back( constant_empirical ); +// empiricalComponentsToEstimate[ radial_empirical_acceleration_component ].push_back( sine_empirical ); +// empiricalComponentsToEstimate[ radial_empirical_acceleration_component ].push_back( cosine_empirical ); +// +// empiricalComponentsToEstimate[ along_track_empirical_acceleration_component ].push_back( constant_empirical ); +// empiricalComponentsToEstimate[ along_track_empirical_acceleration_component ].push_back( sine_empirical ); +// empiricalComponentsToEstimate[ along_track_empirical_acceleration_component ].push_back( cosine_empirical ); +// +// empiricalComponentsToEstimate[ across_track_empirical_acceleration_component ].push_back( constant_empirical ); +// empiricalComponentsToEstimate[ across_track_empirical_acceleration_component ].push_back( sine_empirical ); +// empiricalComponentsToEstimate[ across_track_empirical_acceleration_component ].push_back( cosine_empirical ); + + additionalParameterNames.push_back( std::make_shared( + "GRAIL-A", "Moon", empiricalComponentsToEstimate )); + + + std::shared_ptr > estimationOutput = + createBestFitToCurrentEphemeris( bodies, accelerationModelMap, bodiesToEstimate, centralBodies, + numerical_integrators::rungeKuttaFixedStepSettings( 60.0, + numerical_integrators::rungeKuttaFehlberg78 ), + initialPropagationTime, finalPropagtionTime, 60.0, additionalParameterNames ); + + + + +} +// +//BOOST_AUTO_TEST_SUITE_END( ) +// +//} +// +//}