From e34f190fef2d425ba556fcf4fde230844dc8e7b5 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Mon, 2 Oct 2023 13:21:32 +0200 Subject: [PATCH] Generalizing unit test of observation dependent variables --- include/tudat/basics/utilities.h | 13 + .../unitTestObservationDependentVariables.cpp | 498 ++++++++++++------ 2 files changed, 357 insertions(+), 154 deletions(-) diff --git a/include/tudat/basics/utilities.h b/include/tudat/basics/utilities.h index cc9aa1d564..85ec8c361f 100755 --- a/include/tudat/basics/utilities.h +++ b/include/tudat/basics/utilities.h @@ -897,6 +897,19 @@ Eigen::Matrix< T, Eigen::Dynamic, 1 > getSuccesivelyConcatenatedVector( return concatenatedVector; } +template +int countNumberOfOccurencesInVector( const std::vector< T >& vector, const T& value ) +{ + int counter = 0; + for( unsigned int i = 0; i < vector.size( ); i++ ) + { + if( vector.at( i ) == value ) + { + counter++; + } + } + return counter; +} } // namespace utilities diff --git a/tests/src/astro/observation_models/unitTestObservationDependentVariables.cpp b/tests/src/astro/observation_models/unitTestObservationDependentVariables.cpp index d090f2eea1..4bfe7bc3a4 100644 --- a/tests/src/astro/observation_models/unitTestObservationDependentVariables.cpp +++ b/tests/src/astro/observation_models/unitTestObservationDependentVariables.cpp @@ -7,7 +7,7 @@ * 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 @@ -16,9 +16,10 @@ #include +#include "tudat/basics/utilities.h" #include "tudat/simulation/estimation.h" - +// //namespace tudat //{ //namespace unit_tests @@ -38,16 +39,11 @@ using namespace tudat::propagators; using namespace tudat::basic_astrodynamics; using namespace tudat::coordinate_conversions; using namespace tudat::statistics; - -//BOOST_AUTO_TEST_SUITE( test_observation_noise_models ) - -//// Function to conver -//double ignoreInputeVariable( std::function< double( ) > inputFreeFunction, const double dummyInput ) -//{ -// return inputFreeFunction( ); -//} - -//! Test whether observation noise is correctly added when simulating noisy observations +// +//BOOST_AUTO_TEST_SUITE( test_observation_dependent_variables ) +// +////! Test whether observation noise is correctly added when simulating noisy observations +//BOOST_AUTO_TEST_CASE( testObservationDependentVariables ) int main( ) { //Load spice kernels. @@ -79,184 +75,378 @@ int main( ) std::vector< std::string > groundStationNames; groundStationNames.push_back( "Station1" ); groundStationNames.push_back( "Station2" ); - groundStationNames.push_back( "Station3" ); + groundStationNames.push_back( "Station3" );// relative_angular_position = 9, + createGroundStation( bodies.at( "Earth" ), "Station1", ( Eigen::Vector3d( ) << 0.0, 0.35, 0.0 ).finished( ), geodetic_position ); - createGroundStation( bodies.at( "Earth" ), "Station2", ( Eigen::Vector3d( ) << 0.0, -0.55, 2.0 ).finished( ), geodetic_position ); - createGroundStation( bodies.at( "Earth" ), "Station3", ( Eigen::Vector3d( ) << 0.0, 0.05, 4.0 ).finished( ), geodetic_position ); + createGroundStation( bodies.at( "Earth" ), "Station2", ( Eigen::Vector3d( ) << 0.0, -0.55, 1.0 ).finished( ), geodetic_position ); // Define parameters. - std::vector< LinkEnds > stationReceiverLinkEnds; - std::vector< LinkEnds > stationTransmitterLinkEnds; + std::vector< LinkEnds > stationTransmitterOneWayLinkEnds; + std::vector< LinkEnds > stationReceiverOneWayLinkEnds; + + std::vector< LinkEnds > stationReceiverTwoWayLinkEnds; + std::vector< LinkEnds > stationRetransmitterTwoWayLinkEnds; + std::vector< LinkEnds > stationReceiverThreeWayLinkEnds; + std::vector< LinkEnds > stationTransmitterThreeWayLinkEnds; // Define link ends to/from ground stations to Moon + LinkEnds linkEnds; for( unsigned int i = 0; i < groundStationNames.size( ); i++ ) { - LinkEnds linkEnds; + linkEnds[ transmitter ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( i ) ) ); linkEnds[ receiver ] = LinkEndId( std::make_pair( "Moon", "" ) ); - stationTransmitterLinkEnds.push_back( linkEnds ); + stationTransmitterOneWayLinkEnds.push_back( linkEnds ); linkEnds.clear( ); linkEnds[ receiver ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( i ) ) ); linkEnds[ transmitter ] = LinkEndId( std::make_pair( "Moon", "" ) ); - stationReceiverLinkEnds.push_back( linkEnds ); - } + stationReceiverOneWayLinkEnds.push_back( linkEnds ); - // Define (arbitrary) link ends for each observable - std::map< ObservableType, std::vector< LinkEnds > > linkEndsPerObservable; - linkEndsPerObservable[ one_way_range ].push_back( stationReceiverLinkEnds[ 0 ] ); - linkEndsPerObservable[ one_way_range ].push_back( stationReceiverLinkEnds[ 1 ] ); + linkEnds.clear( ); + linkEnds[ receiver ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( i ) ) ); + linkEnds[ retransmitter ] = LinkEndId( std::make_pair( "Moon", "" ) ); + linkEnds[ transmitter ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( i ) ) ); + stationReceiverTwoWayLinkEnds.push_back( linkEnds ); - linkEndsPerObservable[ one_way_doppler ].push_back( stationReceiverLinkEnds[ 0 ] ); - linkEndsPerObservable[ one_way_doppler ].push_back( stationReceiverLinkEnds[ 1 ] ); + linkEnds.clear( ); + linkEnds[ receiver ] = LinkEndId( std::make_pair( "Moon", "" ) ); + linkEnds[ retransmitter ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( i ) ) ); + linkEnds[ transmitter ] = LinkEndId( std::make_pair( "Moon", "" ) ); + stationRetransmitterTwoWayLinkEnds.push_back( linkEnds ); + } - // Define observation settings for each observable/link ends combination - std::vector< std::shared_ptr< ObservationModelSettings > > observationSettingsList; - for( std::map< ObservableType, std::vector< LinkEnds > >::iterator linkEndIterator = linkEndsPerObservable.begin( ); - linkEndIterator != linkEndsPerObservable.end( ); linkEndIterator++ ) - { - ObservableType currentObservable = linkEndIterator->first; - std::vector< LinkEnds > currentLinkEndsList = linkEndIterator->second; + linkEnds.clear( ); + linkEnds[ receiver ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( 0 ) ) ); + linkEnds[ retransmitter ] = LinkEndId( std::make_pair( "Moon", "" ) ); + linkEnds[ transmitter ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( 1 ) ) ); + stationReceiverThreeWayLinkEnds.push_back( linkEnds ); - for( unsigned int i = 0; i < currentLinkEndsList.size( ); i++ ) - { - // Create observation settings - observationSettingsList.push_back( std::make_shared< ObservationModelSettings >( - currentObservable, currentLinkEndsList.at( i ) ) ); - } - } + linkEnds.clear( ); + linkEnds[ receiver ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( 1 ) ) ); + linkEnds[ retransmitter ] = LinkEndId( std::make_pair( "Moon", "" ) ); + linkEnds[ transmitter ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( 0 ) ) ); + stationTransmitterThreeWayLinkEnds.push_back( linkEnds ); - // Create observation simulators - std::vector< std::shared_ptr< ObservationSimulatorBase< double, double > > > observationSimulators = - createObservationSimulators( observationSettingsList, bodies ); + std::map referenceElevationAngles; + std::map referenceAzimuthAngles; + std::map referenceTargetRanges; - // Define osbervation times. - std::vector< double > baseTimeList; - double observationTimeStart = initialEphemerisTime + 1000.0; - double observationInterval = 10.0; - for( unsigned int i = 0; i < 14; i++ ) + for( unsigned int observableTestCase = 0; observableTestCase < 1; observableTestCase++ ) { - for( unsigned int j = 0; j < 4320; j++ ) + int geometryType = -1; + ObservableType currentObservableType; + if( observableTestCase == 0 ) { - baseTimeList.push_back( observationTimeStart + static_cast< double >( i ) * 86400.0 + - static_cast< double >( j ) * observationInterval ); + currentObservableType = one_way_range; + geometryType = 0; } - } - - // Define observation simulation settings (observation type, link end, times and reference link end) - std::vector< std::shared_ptr< ObservationSimulationSettings< double > > > measurementSimulationInput; - for( std::map< ObservableType, std::vector< LinkEnds > >::iterator linkEndIterator = linkEndsPerObservable.begin( ); - linkEndIterator != linkEndsPerObservable.end( ); linkEndIterator++ ) - { - ObservableType currentObservable = linkEndIterator->first; - std::vector< LinkEnds > currentLinkEndsList = linkEndIterator->second; - for( unsigned int i = 0; i < currentLinkEndsList.size( ); i++ ) + else if( observableTestCase == 1 ) { - measurementSimulationInput.push_back( - std::make_shared< TabulatedObservationSimulationSettings< > >( - currentObservable, currentLinkEndsList.at( i ), baseTimeList, receiver ) ); + currentObservableType = angular_position; + geometryType = 0; } - } - - std::vector< std::shared_ptr< observation_models::ObservationViabilitySettings > > viabilitySettingsList; - viabilitySettingsList.push_back( elevationAngleViabilitySettings( - std::make_pair( "Earth", "Station1" ), 25.0 * mathematical_constants::PI / 180.0 ) ); - viabilitySettingsList.push_back( elevationAngleViabilitySettings( - std::make_pair( "Earth", "Station2" ), 25.0 * mathematical_constants::PI / 180.0 ) ); - - addViabilityToObservationSimulationSettings( - measurementSimulationInput, viabilitySettingsList ); - - // Define settings for dependent variables - std::vector< std::shared_ptr< ObservationDependentVariableSettings > > dependentVariableList; - - std::shared_ptr< ObservationDependentVariableSettings > elevationAngleSettings1 = - std::make_shared< StationAngleObservationDependentVariableSettings >( - station_elevation_angle, LinkEndId( std::make_pair( "Earth", "Station1" ) ) ); - std::shared_ptr< ObservationDependentVariableSettings > azimuthAngleSettings1 = - std::make_shared< StationAngleObservationDependentVariableSettings >( - station_azimuth_angle, LinkEndId( std::make_pair( "Earth", "Station1" ) ) ); - std::shared_ptr< ObservationDependentVariableSettings > elevationAngleSettings2 = - std::make_shared< StationAngleObservationDependentVariableSettings >( - station_elevation_angle, LinkEndId( std::make_pair( "Earth", "Station2" ) ) ); - - dependentVariableList.push_back( elevationAngleSettings1 ); - dependentVariableList.push_back( elevationAngleSettings2 ); - dependentVariableList.push_back( azimuthAngleSettings1 ); - - addDependentVariablesToObservationSimulationSettings( - measurementSimulationInput, dependentVariableList, bodies ); - - - - // Simulate noise-free observations - std::shared_ptr< ObservationCollection< > > idealObservationsAndTimes = simulateObservations< double, double >( - measurementSimulationInput, observationSimulators, bodies ); - - std::cout<<"Simulated observations "< elevationAngles; - std::map< double, Eigen::VectorXd > azimuthAngles; - - std::cout<<"Getting elevation angle"< dependentVariables_; +// position_observable = 2, +// velocity_observable = 8, +// relative_position_observable = 11, +// euler_angle_313_observable = 7, -// std::vector< std::shared_ptr< ObservationDependentVariableSettings > > dependentVariableList; + int numberOfLinkEndCases = -1; + if( geometryType == 0 ) + { + numberOfLinkEndCases = 1; + } + else if( geometryType == 1 ) + { + numberOfLinkEndCases = 4; + } -// std::vector< int > sizeIndices_; + for( int currentLinkEndCase = 0; currentLinkEndCase < numberOfLinkEndCases; currentLinkEndCase++ ) + { -// std::map< double, Eigen::VectorXd > getSingleDependentVariables( std::shared_ptr< ObservationDependentVariableSettings > ); + LinkEnds currentLinkEnds; + switch( geometryType ) + { + case 0: + { + switch ( currentLinkEndCase ) + { + case 0: + currentLinkEnds = stationReceiverOneWayLinkEnds.at( 0 ); + break; + case 1: + currentLinkEnds = stationTransmitterOneWayLinkEnds.at( 0 ); + break; + default: + throw std::runtime_error( "Error in observation dependent variable unit test A " ); + } + break; + } + case 1: + { + switch ( currentLinkEndCase ) + { + case 0: + currentLinkEnds = stationReceiverTwoWayLinkEnds.at( 0 ); + break; + case 1: + currentLinkEnds = stationReceiverThreeWayLinkEnds.at( 0 ); + break; + case 2: + currentLinkEnds = stationTransmitterThreeWayLinkEnds.at( 0 ); + break; + case 3: + currentLinkEnds = stationRetransmitterTwoWayLinkEnds.at( 0 ); + break; + default: + throw std::runtime_error( "Error in observation dependent variable unit test A " ); + } + break; + } + } + std::cout< > linkEndsPerObservable; + linkEndsPerObservable[ currentObservableType ].push_back( currentLinkEnds ); + + // Define observation settings for each observable/link ends combination + std::vector > observationSettingsList; + for ( std::map >::iterator + linkEndIterator = linkEndsPerObservable.begin( ); + linkEndIterator != linkEndsPerObservable.end( ); linkEndIterator++ ) + { + ObservableType currentObservable = linkEndIterator->first; + std::vector currentLinkEndsList = linkEndIterator->second; + + for ( unsigned int i = 0; i < currentLinkEndsList.size( ); i++ ) + { + // Create observation settings + observationSettingsList.push_back( std::make_shared( + currentObservable, currentLinkEndsList.at( i ))); + } + } + + // Create observation simulators + std::vector > > observationSimulators = + createObservationSimulators( observationSettingsList, bodies ); + + // Define osbervation times. + std::vector baseTimeList; + double observationTimeStart = initialEphemerisTime + 1000.0; + double observationInterval = 10.0; + for ( unsigned int i = 0; i < 14; i++ ) + { + for ( unsigned int j = 0; j < 4320; j++ ) + { + baseTimeList.push_back( observationTimeStart + static_cast< double >( i ) * 86400.0 + + static_cast< double >( j ) * observationInterval ); + } + } + + // Define observation simulation settings (observation type, link end, times and reference link end) + std::vector > > measurementSimulationInput; + for ( + std::map >::iterator + linkEndIterator = linkEndsPerObservable.begin( ); + linkEndIterator != linkEndsPerObservable.end( ); linkEndIterator++ ) + { + ObservableType currentObservable = linkEndIterator->first; + std::vector currentLinkEndsList = linkEndIterator->second; + for ( unsigned int i = 0; i < currentLinkEndsList.size( ); i++ ) + { + measurementSimulationInput.push_back( + std::make_shared >( + currentObservable, currentLinkEndsList.at( i ), baseTimeList, receiver )); + } + } + + std::vector > viabilitySettingsList; + viabilitySettingsList.push_back( elevationAngleViabilitySettings( + std::make_pair( "Earth", "Station1" ), 25.0 * mathematical_constants::PI / 180.0 )); + viabilitySettingsList.push_back( elevationAngleViabilitySettings( + std::make_pair( "Earth", "Station2" ), 25.0 * mathematical_constants::PI / 180.0 )); + + addViabilityToObservationSimulationSettings( + measurementSimulationInput, viabilitySettingsList ); + + // Define settings for dependent variables + std::vector > dependentVariableList; + + std::shared_ptr elevationAngleSettings1 = + std::make_shared( + station_elevation_angle, LinkEndId( std::make_pair( "Earth", "Station1" ))); + std::shared_ptr azimuthAngleSettings1 = + std::make_shared( + station_azimuth_angle, LinkEndId( std::make_pair( "Earth", "Station1" ))); + std::shared_ptr targetRangeSettings1 = + std::make_shared( + target_range, transmitter, receiver ); + std::shared_ptr targetInverseRangeSettings1 = + std::make_shared( + target_range, receiver, transmitter ); + + std::shared_ptr elevationAngleSettings2 = + std::make_shared( + station_elevation_angle, LinkEndId( std::make_pair( "Earth", "Station2" ))); + + dependentVariableList.push_back( elevationAngleSettings1 ); + dependentVariableList.push_back( azimuthAngleSettings1 ); + dependentVariableList.push_back( elevationAngleSettings2 ); + + addDependentVariablesToObservationSimulationSettings( + measurementSimulationInput, dependentVariableList, bodies ); + addDependentVariablesToObservationSimulationSettings( + measurementSimulationInput, { targetRangeSettings1 }, bodies, currentObservableType, currentLinkEnds ); + addDependentVariablesToObservationSimulationSettings( + measurementSimulationInput, { targetInverseRangeSettings1 }, bodies, currentObservableType, currentLinkEnds ); -//}; + // Simulate noise-free observations + std::shared_ptr > idealObservationsAndTimes = simulateObservations( + measurementSimulationInput, observationSimulators, bodies ); + if ( observableTestCase == 0 ) + { + + std::map elevationAngles1 = getDependentVariableResultList( + idealObservationsAndTimes, elevationAngleSettings1, currentObservableType ); + std::map elevationAngles2 = getDependentVariableResultList( + idealObservationsAndTimes, elevationAngleSettings2, currentObservableType ); + std::map azimuthAngles1 = getDependentVariableResultList( + idealObservationsAndTimes, azimuthAngleSettings1, currentObservableType ); + std::map targetRanges1 = getDependentVariableResultList( + idealObservationsAndTimes, targetRangeSettings1, currentObservableType ); + std::map targetInverseRanges1 = getDependentVariableResultList( + idealObservationsAndTimes, targetInverseRangeSettings1, currentObservableType ); + + referenceElevationAngles = elevationAngles1; + referenceAzimuthAngles = azimuthAngles1; + referenceTargetRanges = targetRanges1; + + std::map + linkEndIdentifiers = idealObservationsAndTimes->getLinkEndIdentifierMap( ); + std::vector linkEndIds = idealObservationsAndTimes->getConcatenatedLinkEndIds( ); + + int numberOfLinkEnds1Observations = utilities::countNumberOfOccurencesInVector( + linkEndIds, linkEndIdentifiers.at( stationReceiverOneWayLinkEnds[ 0 ] )); + int numberOfLinkEnds2Observations = utilities::countNumberOfOccurencesInVector( + linkEndIds, linkEndIdentifiers.at( stationReceiverOneWayLinkEnds[ 1 ] )); + + BOOST_CHECK_EQUAL( elevationAngles1.size( ), numberOfLinkEnds1Observations ); + BOOST_CHECK_EQUAL( azimuthAngles1.size( ), numberOfLinkEnds1Observations ); + BOOST_CHECK_EQUAL( targetRanges1.size( ), numberOfLinkEnds1Observations ); + BOOST_CHECK_EQUAL( targetInverseRanges1.size( ), numberOfLinkEnds1Observations ); + BOOST_CHECK_EQUAL( elevationAngles2.size( ), numberOfLinkEnds2Observations ); + + std::shared_ptr pointingAnglesCalculator1 = + bodies.at( "Earth" )->getGroundStation( "Station1" )->getPointingAnglesCalculator( ); + std::shared_ptr pointingAnglesCalculator2 = + bodies.at( "Earth" )->getGroundStation( "Station2" )->getPointingAnglesCalculator( ); + + std::shared_ptr > observationModel1 = + std::dynamic_pointer_cast >( + observationSimulators.at( 0 ))->getObservationModel( stationReceiverOneWayLinkEnds[ 0 ] ); + std::shared_ptr > observationModel2 = + std::dynamic_pointer_cast >( + observationSimulators.at( 0 ))->getObservationModel( stationReceiverOneWayLinkEnds[ 1 ] ); + + std::vector linkEndTimes; + std::vector > linkEndStates; + + for ( auto it: elevationAngles1 ) + { + double currentTime = it.first; + double currentElevation = elevationAngles1.at( currentTime )( 0 ); + double currentAzimuth = azimuthAngles1.at( currentTime )( 0 ); + double targetRange = targetRanges1.at( currentTime )( 0 ); + double targetInverseRange = targetInverseRanges1.at( currentTime )( 0 ); + + observationModel1->computeIdealObservationsWithLinkEndData( + currentTime, receiver, linkEndTimes, linkEndStates ); + Eigen::Vector3d vectorToTarget = ( linkEndStates.at( 0 ) - linkEndStates.at( 1 )).segment( 0, 3 ); + + double elevationAngle = pointingAnglesCalculator1->calculateElevationAngleFromInertialVector( + vectorToTarget, linkEndTimes.at( 1 )); + BOOST_CHECK_SMALL(( elevationAngle - currentElevation ), std::numeric_limits::epsilon( )); + + double azimuthAngle = pointingAnglesCalculator1->calculateAzimuthAngleFromInertialVector( + vectorToTarget, linkEndTimes.at( 1 )); + BOOST_CHECK_SMALL(( azimuthAngle - currentAzimuth ), std::numeric_limits::epsilon( )); + + BOOST_CHECK_SMALL(( targetRange - vectorToTarget.norm( )), + std::numeric_limits::epsilon( ) * vectorToTarget.norm( )); + BOOST_CHECK_SMALL(( targetInverseRange - targetRange ), + std::numeric_limits::epsilon( ) * vectorToTarget.norm( )); + BOOST_CHECK_SMALL(( targetInverseRange - vectorToTarget.norm( )), + std::numeric_limits::epsilon( ) * vectorToTarget.norm( )); + + } + } + else if ( observableTestCase < 3 ) + { + std::map elevationAngles = getDependentVariableResultList( + idealObservationsAndTimes, elevationAngleSettings1, currentObservableType ); + std::map azimuthAngles = getDependentVariableResultList( + idealObservationsAndTimes, azimuthAngleSettings1, currentObservableType ); + std::map targetRanges = getDependentVariableResultList( + idealObservationsAndTimes, targetRangeSettings1, currentObservableType ); + + for ( auto it: referenceElevationAngles ) + { + BOOST_CHECK(( elevationAngles.count( it.first ) > 0 )); + BOOST_CHECK(( azimuthAngles.count( it.first ) > 0 )); + BOOST_CHECK(( targetRanges.count( it.first ) > 0 )); + + double currentTime = it.first; + + BOOST_CHECK_SMALL( std::fabs( + elevationAngles.at( currentTime )( 0 ) - referenceElevationAngles.at( currentTime )( 0 )), + std::numeric_limits::epsilon( )); + BOOST_CHECK_SMALL( std::fabs( + azimuthAngles.at( currentTime )( 0 ) - referenceAzimuthAngles.at( currentTime )( 0 )), + std::numeric_limits::epsilon( )); + BOOST_CHECK_SMALL( + std::fabs( targetRanges.at( currentTime )( 0 ) - referenceTargetRanges.at( currentTime )( 0 )), + std::numeric_limits::epsilon( ) * referenceTargetRanges.at( currentTime )( 0 )); + + } + } + } + } +} +// //BOOST_AUTO_TEST_SUITE_END( ) - +// //} - +// //} - +//