diff --git a/include/tudat/simulation/estimation_setup/observationOutputSettings.h b/include/tudat/simulation/estimation_setup/observationOutputSettings.h index 9fc5dfa386..bfdf061054 100644 --- a/include/tudat/simulation/estimation_setup/observationOutputSettings.h +++ b/include/tudat/simulation/estimation_setup/observationOutputSettings.h @@ -115,13 +115,14 @@ class InterlinkObservationDependentVariableSettings: public ObservationDependent const std::string relativeBody = "" ): ObservationDependentVariableSettings( variableType ), startLinkEnd_( startLinkEnd ), endLinkEnd_( endLinkEnd ), - integratedObservableHandling_( integratedObservableHandling ){ } + integratedObservableHandling_( integratedObservableHandling ), + relativeBody_( relativeBody ){ } ~InterlinkObservationDependentVariableSettings( ){ } std::string getIdentifier( ) { - std::string identifier = ", link from " + observation_models::getLinkEndTypeString( startLinkEnd_ ) + + std::string identifier = ", link from " + observation_models::getLinkEndTypeString( startLinkEnd_ ) + " to " + observation_models::getLinkEndTypeString( endLinkEnd_ ); if( relativeBody_ != "" ) { diff --git a/src/astro/observation_models/observationViabilityCalculator.cpp b/src/astro/observation_models/observationViabilityCalculator.cpp index de36f19ff5..efd425d556 100755 --- a/src/astro/observation_models/observationViabilityCalculator.cpp +++ b/src/astro/observation_models/observationViabilityCalculator.cpp @@ -80,6 +80,7 @@ double computeMinimumLinkDistanceToPoint( const Eigen::Vector3d& observingBody, Eigen::Vector3d observerToPoint = relativePoint - observingBody; Eigen::Vector3d transmitterToPoint = relativePoint - transmittingBody; Eigen::Vector3d transmitterToObserver = observingBody - transmittingBody; + Eigen::Vector3d observerToTransmitter = transmittingBody - observingBody; double transmitterAngle = transmitterToObserver.dot( transmitterToPoint ); double observerAngle = transmitterToObserver.dot( observerToPoint ); @@ -91,18 +92,20 @@ double computeMinimumLinkDistanceToPoint( const Eigen::Vector3d& observingBody, { if( std::fabs( transmitterAngle ) < std::fabs( observerAngle ) ) { - minimumDistance = transmitterAngle; + minimumDistance = transmitterToPoint.norm( ); } else { - minimumDistance = observerAngle; + minimumDistance = observerToPoint.norm( ); } } // Minimum distance is impact parameter else { - minimumDistance = ( transmitterToObserver.cross( transmitterToPoint ) ).norm( ) / transmitterToObserver.norm( ); + // Compute as average value with both points as reference, to minimuze numerical errors + minimumDistance = ( ( transmitterToObserver.cross( transmitterToPoint ) ).norm( ) / transmitterToObserver.norm( ) + + ( observerToTransmitter.cross( observerToPoint ) ).norm( ) / observerToTransmitter.norm( ) ) / 2.0; } return minimumDistance; diff --git a/src/simulation/estimation_setup/observationOutput.cpp b/src/simulation/estimation_setup/observationOutput.cpp index 152bd7ce0a..99ec92d4cc 100644 --- a/src/simulation/estimation_setup/observationOutput.cpp +++ b/src/simulation/estimation_setup/observationOutput.cpp @@ -225,7 +225,7 @@ ObservationDependentVariableFunction getInterlinkObservationVariableFunction( } case body_avoidance_angle_variable: { - if ( bodies.count( variableSettings->relativeBody_ ) != 0 ) + if ( bodies.count( variableSettings->relativeBody_ ) == 0 ) { throw std::runtime_error( "Error when parsing body avoidance observation dependent variable w.r.t. " + variableSettings->relativeBody_ + ", body is not defined" ); @@ -246,7 +246,7 @@ ObservationDependentVariableFunction getInterlinkObservationVariableFunction( linkEndStates.at( linkEndIndicesToUse.first ).segment( 0, 3 ), linkEndStates.at( linkEndIndicesToUse.second ).segment( 0, 3 ), bodies.at( variableSettings->relativeBody_ )->getStateInBaseFrameFromEphemeris( - ( linkEndIndicesToUse.first + linkEndIndicesToUse.second ) / 2.0 ).segment( 0, 3 )); + ( linkEndTimes.at( linkEndIndicesToUse.first ) + linkEndTimes.at( linkEndIndicesToUse.second ) ) / 2.0 ).segment( 0, 3 )); if ( std::fabs( cosineOfAvoidanceAngle ) >= 1.0 ) { return ( Eigen::Vector1d( ) @@ -261,7 +261,7 @@ ObservationDependentVariableFunction getInterlinkObservationVariableFunction( } case link_body_center_distance: { - if ( bodies.count( variableSettings->relativeBody_ ) != 0 ) + if ( bodies.count( variableSettings->relativeBody_ ) == 0 ) { throw std::runtime_error( "Error when parsing link-body distance observation dependent variable w.r.t. " + variableSettings->relativeBody_ + ", body is not defined" ); @@ -282,20 +282,24 @@ ObservationDependentVariableFunction getInterlinkObservationVariableFunction( linkEndStates.at( linkEndIndicesToUse.first ).segment( 0, 3 ), linkEndStates.at( linkEndIndicesToUse.second ).segment( 0, 3 ), bodies.at( variableSettings->relativeBody_ )->getStateInBaseFrameFromEphemeris( - ( linkEndIndicesToUse.first + linkEndIndicesToUse.second ) / 2.0 ).segment( 0, 3 )); + ( linkEndTimes.at( linkEndIndicesToUse.first ) + linkEndTimes.at( linkEndIndicesToUse.second ) ) / 2.0 ).segment( 0, 3 )); return ( Eigen::Vector1d( ) << minimumDistance ).finished( ); }; break; } case link_limb_distance: { - std::shared_ptr< InterlinkObservationDependentVariableSettings > bodyCenterDistanceSettings = - std::make_shared< InterlinkObservationDependentVariableSettings >( - link_body_center_distance, - variableSettings->startLinkEnd_, variableSettings->endLinkEnd_, - variableSettings->integratedObservableHandling_, variableSettings->relativeBody_ ); - ObservationDependentVariableFunction linkCenterFunction = getInterlinkObservationVariableFunction( - bodies, variableSettings, observableType, linkEnds ); + if ( bodies.count( variableSettings->relativeBody_ ) == 0 ) + { + throw std::runtime_error( "Error when parsing link-body distance observation dependent variable w.r.t. " + + variableSettings->relativeBody_ + ", body is not defined" ); + } + + if ( bodies.at( variableSettings->relativeBody_ )->getEphemeris( ) == nullptr ) + { + throw std::runtime_error( "Error when parsing link-body distance observation dependent variable w.r.t. " + + variableSettings->relativeBody_ + ", body has no ephemeris" ); + } if ( bodies.at( variableSettings->relativeBody_ )->getShapeModel( ) == nullptr ) { @@ -308,14 +312,18 @@ ObservationDependentVariableFunction getInterlinkObservationVariableFunction( const Eigen::VectorXd &observationValue, const std::shared_ptr ancilliarySimulationSettings ) { - return linkCenterFunction( linkEndTimes, linkEndStates, observationValue, ancilliarySimulationSettings ) - - ( Eigen::Vector1d( ) << shapeModel->getAverageRadius( ) ).finished( ); + double minimumDistance = observation_models::computeMinimumLinkDistanceToPoint( + linkEndStates.at( linkEndIndicesToUse.first ).segment( 0, 3 ), + linkEndStates.at( linkEndIndicesToUse.second ).segment( 0, 3 ), + bodies.at( variableSettings->relativeBody_ )->getStateInBaseFrameFromEphemeris( + ( linkEndTimes.at( linkEndIndicesToUse.first ) + linkEndTimes.at( linkEndIndicesToUse.second ) ) / 2.0 ).segment( 0, 3 )); + return ( Eigen::Vector1d( ) << minimumDistance - shapeModel->getAverageRadius( ) ).finished( ); }; break; } case link_angle_with_orbital_plane: { - if ( bodies.count( variableSettings->relativeBody_ ) != 0 ) + if ( bodies.count( variableSettings->relativeBody_ ) == 0 ) { throw std::runtime_error( "Error when parsing link-orbital plane angle observation dependent variable w.r.t. " + variableSettings->relativeBody_ + ", body is not defined" ); @@ -336,9 +344,10 @@ ObservationDependentVariableFunction getInterlinkObservationVariableFunction( linkEndStates.at( linkEndIndicesToUse.second ).segment( 0, 3 ) - linkEndStates.at( linkEndIndicesToUse.first ).segment( 0, 3 ); Eigen::Vector6d targetStateWrtCentralBody = linkEndStates.at( linkEndIndicesToUse.second ) - - bodies.at( variableSettings->relativeBody_ )->getStateInBaseFrameFromEphemeris( linkEndIndicesToUse.second ); + bodies.at( variableSettings->relativeBody_ )->getStateInBaseFrameFromEphemeris( + linkEndTimes.at( linkEndIndicesToUse.second ) ); Eigen::Vector3d orbitNormal = targetStateWrtCentralBody.segment< 3 >( 0 ).cross( targetStateWrtCentralBody.segment< 3 >( 3 ) ); - return ( Eigen::Vector1d( ) << linear_algebra::computeAngleBetweenVectors( orbitNormal, vectorToTarget ) ).finished( ); + return ( Eigen::Vector1d( ) << linear_algebra::computeAngleBetweenVectors( orbitNormal, vectorToTarget ) - mathematical_constants::PI / 2.0 ).finished( ); }; break; diff --git a/src/simulation/estimation_setup/observationOutputSettings.cpp b/src/simulation/estimation_setup/observationOutputSettings.cpp index 04de562855..c9e9d3a3ce 100644 --- a/src/simulation/estimation_setup/observationOutputSettings.cpp +++ b/src/simulation/estimation_setup/observationOutputSettings.cpp @@ -73,7 +73,7 @@ std::string getObservationDependentVariableName( } case link_angle_with_orbital_plane: { - dependentVariableName = "Angle between link vector and orbit normal vector "; + dependentVariableName = "Angle between link vector and orbital plane "; break; } case doppler_integration_time_dependent_variable: @@ -89,7 +89,7 @@ std::string getObservationDependentVariableName( default: throw std::runtime_error( "Error when checking observation dependent variable. Type " + std::to_string( variableType ) + - " not found when checking fif variable is vectorial." ); + " not found when retrieving variable name." ); } return dependentVariableName; } @@ -97,7 +97,7 @@ std::string getObservationDependentVariableName( std::string getObservationDependentVariableId( const std::shared_ptr< ObservationDependentVariableSettings > variableSettings ) { - return getObservationDependentVariableName( variableSettings->variableType_ ) + ". " + + return getObservationDependentVariableName( variableSettings->variableType_ ) + variableSettings->getIdentifier( ); } @@ -126,10 +126,16 @@ bool isObservationDependentVariableVectorial( case link_body_center_distance: isVariableVectorial = false; break; + case link_limb_distance: + isVariableVectorial = false; + break; + case link_angle_with_orbital_plane: + isVariableVectorial = false; + break; default: throw std::runtime_error( "Error when checking observation dependent variable. Type " + getObservationDependentVariableId( variableSettings ) + - " not found when checking fif variable is vectorial." ); + " not found when checking if variable is vectorial." ); } return isVariableVectorial; diff --git a/tests/src/astro/observation_models/unitTestObservationDependentVariables.cpp b/tests/src/astro/observation_models/unitTestObservationDependentVariables.cpp index ea18531290..ad0600684d 100644 --- a/tests/src/astro/observation_models/unitTestObservationDependentVariables.cpp +++ b/tests/src/astro/observation_models/unitTestObservationDependentVariables.cpp @@ -75,8 +75,7 @@ void compareAgainstReference( // double currentTime = it.first; -// std::cout<second<<" "<second< 0 ) + { + if( startToPoint.norm( ) < endToPoint.norm( ) ) + { + distance = startToPoint.norm( ); + } + else + { + distance = endToPoint.norm( ); + } + } + else + { + double angle = linear_algebra::computeAngleBetweenVectors( startToPoint, lineDirection ); + distance = std::sin( angle ) * startToPoint.norm( ); + } + return distance; +} + //! Test whether observation noise is correctly added when simulating noisy observations BOOST_AUTO_TEST_CASE( testObservationDependentVariables ) //int main( ) @@ -105,19 +136,28 @@ BOOST_AUTO_TEST_CASE( testObservationDependentVariables ) // Specify initial time double initialEphemerisTime = double( 1.0E7 ); - double finalEphemerisTime = double( 1.0E7 + 3.0 * physical_constants::JULIAN_DAY ); // Create bodies needed in simulation BodyListSettings bodySettings = - getDefaultBodySettings( bodyNames, initialEphemerisTime - 3600.0, finalEphemerisTime + 3600.0 ); + getDefaultBodySettings( bodyNames ); bodySettings.at( "Earth" )->rotationModelSettings = std::make_shared< SimpleRotationModelSettings >( "ECLIPJ2000", "IAU_Earth", spice_interface::computeRotationQuaternionBetweenFrames( "ECLIPJ2000", "IAU_Earth", initialEphemerisTime ), initialEphemerisTime, 2.0 * mathematical_constants::PI / ( physical_constants::JULIAN_DAY ) ); + bodySettings.addSettings( "MoonOrbiter" ); + bodySettings.resetFrames( "Earth" ); + Eigen::Vector6d keplerElements = Eigen::Vector6d::Zero( ); + keplerElements( 0 ) = 2.0E6; + keplerElements( 1 ) = 0.1; + keplerElements( 2 ) = 1.0; + + bodySettings.at( "MoonOrbiter" )->ephemerisSettings = keplerEphemerisSettings( + keplerElements, 0.0, spice_interface::getBodyGravitationalParameter( "Moon" ), "Moon" ); SystemOfBodies bodies = createSystemOfBodies( bodySettings ); + // Creatre ground stations @@ -137,8 +177,8 @@ BOOST_AUTO_TEST_CASE( testObservationDependentVariables ) bodies.at( "Earth" )->getGroundStation( "Station2" )->getVehicleSystems( )->setTransponderTurnaroundRatio( ); bodies.at( "Earth" )->getGroundStation( "Station2" )->setTransmittingFrequencyCalculator( std::make_shared< ConstantFrequencyInterpolator >( 3.0E9 ) ); - bodies.at( "Moon" )->setVehicleSystems( std::make_shared< system_models::VehicleSystems >( ) ); - bodies.at( "Moon" )->getVehicleSystems( )->setTransponderTurnaroundRatio( ); + bodies.at( "MoonOrbiter" )->setVehicleSystems( std::make_shared< system_models::VehicleSystems >( ) ); + bodies.at( "MoonOrbiter" )->getVehicleSystems( )->setTransponderTurnaroundRatio( ); // Define parameters. std::vector< LinkEnds > stationTransmitterOneWayLinkEnds; @@ -163,61 +203,61 @@ BOOST_AUTO_TEST_CASE( testObservationDependentVariables ) { linkEnds[ transmitter ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( i ) ) ); - linkEnds[ receiver ] = LinkEndId( std::make_pair( "Moon", "" ) ); + linkEnds[ receiver ] = LinkEndId( std::make_pair( "MoonOrbiter", "" ) ); stationTransmitterOneWayLinkEnds.push_back( linkEnds ); linkEnds.clear( ); linkEnds[ receiver ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( i ) ) ); - linkEnds[ transmitter ] = LinkEndId( std::make_pair( "Moon", "" ) ); + linkEnds[ transmitter ] = LinkEndId( std::make_pair( "MoonOrbiter", "" ) ); stationReceiverOneWayLinkEnds.push_back( linkEnds ); linkEnds.clear( ); linkEnds[ receiver ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( i ) ) ); - linkEnds[ retransmitter ] = LinkEndId( std::make_pair( "Moon", "" ) ); + linkEnds[ retransmitter ] = LinkEndId( std::make_pair( "MoonOrbiter", "" ) ); linkEnds[ transmitter ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( i ) ) ); stationReceiverTwoWayLinkEnds.push_back( linkEnds ); linkEnds.clear( ); - linkEnds[ receiver ] = LinkEndId( std::make_pair( "Moon", "" ) ); + linkEnds[ receiver ] = LinkEndId( std::make_pair( "MoonOrbiter", "" ) ); linkEnds[ retransmitter ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( i ) ) ); - linkEnds[ transmitter ] = LinkEndId( std::make_pair( "Moon", "" ) ); + linkEnds[ transmitter ] = LinkEndId( std::make_pair( "MoonOrbiter", "" ) ); stationRetransmitterTwoWayLinkEnds.push_back( linkEnds ); linkEnds.clear( ); linkEnds[ receiver ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( i ) ) ); - linkEnds[ transmitter ] = LinkEndId( std::make_pair( "Moon", "" ) ); + linkEnds[ transmitter ] = LinkEndId( std::make_pair( "MoonOrbiter", "" ) ); linkEnds[ transmitter2 ] = LinkEndId( std::make_pair( "Mars", "" ) ); stationReceiverRelativeLinkEnds.push_back( linkEnds ); linkEnds.clear( ); linkEnds[ receiver ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( i ) ) ); - linkEnds[ transmitter2 ] = LinkEndId( std::make_pair( "Moon", "" ) ); + linkEnds[ transmitter2 ] = LinkEndId( std::make_pair( "MoonOrbiter", "" ) ); linkEnds[ transmitter ] = LinkEndId( std::make_pair( "Mars", "" ) ); stationReceiverOppositeRelativeLinkEnds.push_back( linkEnds ); // linkEnds.clear( ); // linkEnds[ transmitter ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( i ) ) ); -// linkEnds[ receiver ] = LinkEndId( std::make_pair( "Moon", "" ) ); +// linkEnds[ receiver ] = LinkEndId( std::make_pair( "MoonOrbiter", "" ) ); // linkEnds[ transmitter2 ] = LinkEndId( std::make_pair( "Mars", "" ) ); // stationTransmitterRelativeLinkEnds.push_back( linkEnds ); // // linkEnds.clear( ); // linkEnds[ transmitter2 ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( i ) ) ); -// linkEnds[ receiver ] = LinkEndId( std::make_pair( "Moon", "" ) ); +// linkEnds[ receiver ] = LinkEndId( std::make_pair( "MoonOrbiter", "" ) ); // linkEnds[ transmitter ] = LinkEndId( std::make_pair( "Mars", "" ) ); // stationTransmitter2RelativeLinkEnds.push_back( linkEnds ); } linkEnds.clear( ); linkEnds[ receiver ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( 0 ) ) ); - linkEnds[ retransmitter ] = LinkEndId( std::make_pair( "Moon", "" ) ); + linkEnds[ retransmitter ] = LinkEndId( std::make_pair( "MoonOrbiter", "" ) ); linkEnds[ transmitter ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( 1 ) ) ); stationReceiverThreeWayLinkEnds.push_back( linkEnds ); linkEnds.clear( ); linkEnds[ receiver ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( 1 ) ) ); - linkEnds[ retransmitter ] = LinkEndId( std::make_pair( "Moon", "" ) ); + linkEnds[ retransmitter ] = LinkEndId( std::make_pair( "MoonOrbiter", "" ) ); linkEnds[ transmitter ] = LinkEndId( std::make_pair( "Earth", groundStationNames.at( 0 ) ) ); stationTransmitterThreeWayLinkEnds.push_back( linkEnds ); @@ -310,7 +350,7 @@ BOOST_AUTO_TEST_CASE( testObservationDependentVariables ) numberOfLinkEndCases *= 2; } - for( int currentLinkEndCase = 0; currentLinkEndCase < numberOfLinkEndCases; currentLinkEndCase++ ) + for( int currentLinkEndCase = 0; currentLinkEndCase < numberOfLinkEndCases; currentLinkEndCase++ ) //numberOfLinkEndCases { bool compareAgainstReceiver = -1; LinkEndType referenceLinkEnd = unidentified_link_end; @@ -436,7 +476,7 @@ BOOST_AUTO_TEST_CASE( testObservationDependentVariables ) if( !( currentObservableType == dsn_n_way_averaged_doppler && referenceLinkEnd != receiver ) ) { - std::cout< > linkEndsPerObservable; linkEndsPerObservable[ currentObservableType ].push_back( currentLinkEnds ); @@ -545,26 +585,50 @@ BOOST_AUTO_TEST_CASE( testObservationDependentVariables ) // Define settings for dependent variables std::vector > dependentVariableList; - std::shared_ptr elevationAngleSettings1 = + std::shared_ptr elevationAngleSettings = std::make_shared( station_elevation_angle, LinkEndId( std::make_pair( "Earth", "Station1" )), referenceLinkEnd, integratedObservableHandling, originatingLinkEndRole ); - std::shared_ptr azimuthAngleSettings1 = + std::shared_ptr azimuthAngleSettings = std::make_shared( station_azimuth_angle, LinkEndId( std::make_pair( "Earth", "Station1" )), referenceLinkEnd, integratedObservableHandling, originatingLinkEndRole ); - std::shared_ptr targetRangeSettings1 = + std::shared_ptr targetRangeSettings = std::make_shared( target_range, referenceLinkEnd, originatingLinkEndRole, integratedObservableHandling ); - std::shared_ptr targetInverseRangeSettings1 = + std::shared_ptr targetInverseRangeSettings = + std::make_shared( + target_range, originatingLinkEndRole, referenceLinkEnd, integratedObservableHandling ); + std::shared_ptr linkBodyCenterDistanceSettings = + std::make_shared( + link_body_center_distance, referenceLinkEnd, originatingLinkEndRole, integratedObservableHandling, "Moon" ); + std::shared_ptr linkBodyCenterDistanceInverseSettings = + std::make_shared( + link_body_center_distance, originatingLinkEndRole, referenceLinkEnd, integratedObservableHandling, "Moon" ); + std::shared_ptr linkLimbDistanceSettings = + std::make_shared( + link_limb_distance, referenceLinkEnd, originatingLinkEndRole, integratedObservableHandling, "Moon" ); + std::shared_ptr linkLimbDistanceInverseSettings = std::make_shared( - target_range, originatingLinkEndRole, referenceLinkEnd, integratedObservableHandling ); + link_limb_distance, originatingLinkEndRole, referenceLinkEnd, integratedObservableHandling, "Moon" ); + std::shared_ptr moonAvoidanceAngleSettings = + std::make_shared( + body_avoidance_angle_variable, referenceLinkEnd, originatingLinkEndRole, integratedObservableHandling, "Moon" ); + std::shared_ptr orbitalPlaneAngleSettings = + std::make_shared( + link_angle_with_orbital_plane, referenceLinkEnd, originatingLinkEndRole, integratedObservableHandling, "Moon" ); - dependentVariableList.push_back( elevationAngleSettings1 ); - dependentVariableList.push_back( azimuthAngleSettings1 ); - dependentVariableList.push_back( targetRangeSettings1 ); - dependentVariableList.push_back( targetInverseRangeSettings1 ); + dependentVariableList.push_back( elevationAngleSettings ); + dependentVariableList.push_back( azimuthAngleSettings ); + dependentVariableList.push_back( targetRangeSettings ); + dependentVariableList.push_back( targetInverseRangeSettings ); + dependentVariableList.push_back( linkBodyCenterDistanceSettings ); + dependentVariableList.push_back( linkBodyCenterDistanceInverseSettings ); + dependentVariableList.push_back( linkLimbDistanceSettings ); + dependentVariableList.push_back( linkLimbDistanceInverseSettings ); + dependentVariableList.push_back( moonAvoidanceAngleSettings ); + dependentVariableList.push_back( orbitalPlaneAngleSettings ); addDependentVariablesToObservationSimulationSettings( @@ -578,13 +642,28 @@ BOOST_AUTO_TEST_CASE( testObservationDependentVariables ) { std::map elevationAngles1 = getDependentVariableResultList( - idealObservationsAndTimes, elevationAngleSettings1, currentObservableType ); + idealObservationsAndTimes, elevationAngleSettings, currentObservableType ); std::map azimuthAngles1 = getDependentVariableResultList( - idealObservationsAndTimes, azimuthAngleSettings1, currentObservableType ); + idealObservationsAndTimes, azimuthAngleSettings, currentObservableType ); + std::map targetRanges1 = getDependentVariableResultList( - idealObservationsAndTimes, targetRangeSettings1, currentObservableType ); + idealObservationsAndTimes, targetRangeSettings, currentObservableType ); std::map targetInverseRanges1 = getDependentVariableResultList( - idealObservationsAndTimes, targetInverseRangeSettings1, currentObservableType ); + idealObservationsAndTimes, targetInverseRangeSettings, currentObservableType ); + + std::map linkBodyDistances = getDependentVariableResultList( + idealObservationsAndTimes, linkBodyCenterDistanceSettings, currentObservableType ); + std::map linkBodyInverseDistances = getDependentVariableResultList( + idealObservationsAndTimes, linkBodyCenterDistanceInverseSettings, currentObservableType ); + + std::map linkLimbDistances = getDependentVariableResultList( + idealObservationsAndTimes, linkLimbDistanceSettings, currentObservableType ); + std::map linkLimbInverseDistances = getDependentVariableResultList( + idealObservationsAndTimes, linkLimbDistanceInverseSettings, currentObservableType ); + std::map moonAvoidanceAngles = getDependentVariableResultList( + idealObservationsAndTimes, moonAvoidanceAngleSettings, currentObservableType ); + std::map orbitalPlaneAngles = getDependentVariableResultList( + idealObservationsAndTimes, orbitalPlaneAngleSettings, currentObservableType ); if( currentLinkEndCase == 0 ) { @@ -592,6 +671,13 @@ BOOST_AUTO_TEST_CASE( testObservationDependentVariables ) referenceReceiverDependentVariableResults.push_back( azimuthAngles1 ); referenceReceiverDependentVariableResults.push_back( targetRanges1 ); referenceReceiverDependentVariableResults.push_back( targetInverseRanges1 ); + referenceReceiverDependentVariableResults.push_back( linkBodyDistances ); + referenceReceiverDependentVariableResults.push_back( linkBodyInverseDistances ); + referenceReceiverDependentVariableResults.push_back( linkLimbDistances ); + referenceReceiverDependentVariableResults.push_back( linkLimbInverseDistances ); + referenceReceiverDependentVariableResults.push_back( moonAvoidanceAngles ); + referenceReceiverDependentVariableResults.push_back( orbitalPlaneAngles ); + } else if( currentLinkEndCase == 1 ) { @@ -599,6 +685,12 @@ BOOST_AUTO_TEST_CASE( testObservationDependentVariables ) referenceTransmitterDependentVariableResults.push_back( azimuthAngles1 ); referenceTransmitterDependentVariableResults.push_back( targetRanges1 ); referenceTransmitterDependentVariableResults.push_back( targetInverseRanges1 ); + referenceTransmitterDependentVariableResults.push_back( linkBodyDistances ); + referenceTransmitterDependentVariableResults.push_back( linkBodyInverseDistances ); + referenceTransmitterDependentVariableResults.push_back( linkLimbDistances ); + referenceTransmitterDependentVariableResults.push_back( linkLimbInverseDistances ); + referenceTransmitterDependentVariableResults.push_back( moonAvoidanceAngles ); + referenceTransmitterDependentVariableResults.push_back( orbitalPlaneAngles ); } std::map @@ -632,14 +724,39 @@ BOOST_AUTO_TEST_CASE( testObservationDependentVariables ) double currentAzimuth = azimuthAngles1.at( currentTime )( 0 ); double targetRange = targetRanges1.at( currentTime )( 0 ); double targetInverseRange = targetInverseRanges1.at( currentTime )( 0 ); + double linkBodyDistance = linkBodyDistances.at( currentTime )( 0 ); + double linkBodyInverseDistance = linkBodyInverseDistances.at( currentTime )( 0 ); + double linkLimbDistance = linkLimbDistances.at( currentTime )( 0 ); + double linkLimbInverseDistance = linkLimbInverseDistances.at( currentTime )( 0 ); + double moonAvoidanceAngle = moonAvoidanceAngles.at( currentTime )( 0 ); + double orbitalPlaneAngle = orbitalPlaneAngles.at( currentTime )( 0 ); observationModel1->computeIdealObservationsWithLinkEndData( currentTime, referenceLinkEnd, linkEndTimes, linkEndStates ); + Eigen::Vector3d vectorToTarget = ( linkEndStates.at( 0 ) - linkEndStates.at( 1 )).segment( 0, 3 ); + Eigen::Vector6d moonState = spice_interface::getBodyCartesianStateAtEpoch( + "Moon", "Earth", "ECLIPJ2000", "None", ( linkEndTimes.at( 0 ) + linkEndTimes.at( 1 ) )/ 2.0 ); + + Eigen::Vector3d stationToMoon = moonState.segment< 3 >( 0 ); + Eigen::Vector6d moonToSpacecraft = -moonState; + if( referenceLinkEnd == transmitter ) { vectorToTarget *= -1.0; + stationToMoon -= linkEndStates.at( 0 ).segment< 3 >( 0 ); + moonToSpacecraft = linkEndStates.at( 1 ) - spice_interface::getBodyCartesianStateAtEpoch( + "Moon", "Earth", "ECLIPJ2000", "None", linkEndTimes.at( 1 ) ); + } + else + { + stationToMoon -= linkEndStates.at( 1 ).segment< 3 >( 0 ); + moonToSpacecraft = linkEndStates.at( 0 ) - spice_interface::getBodyCartesianStateAtEpoch( + "Moon", "Earth", "ECLIPJ2000", "None", linkEndTimes.at( 0 ) ); } + + Eigen::Vector3d orbitalAngularMomentum = moonToSpacecraft.segment< 3 >( 0 ).cross( moonToSpacecraft.segment< 3 >( 3 ) ); + double referenceTime = ( referenceLinkEnd == transmitter ) ? linkEndTimes.at( 0 ) : linkEndTimes.at( 1 ); double elevationAngle = pointingAnglesCalculator1->calculateElevationAngleFromInertialVector( @@ -648,15 +765,35 @@ BOOST_AUTO_TEST_CASE( testObservationDependentVariables ) double azimuthAngle = pointingAnglesCalculator1->calculateAzimuthAngleFromInertialVector( vectorToTarget, referenceTime ); - BOOST_CHECK_SMALL(( azimuthAngle - currentAzimuth ), std::numeric_limits::epsilon( )); - BOOST_CHECK_SMALL(( targetRange - vectorToTarget.norm( )), + double linkDistanceToMoon = computeLineSegmentToCenterOfMassDistance( + linkEndStates.at( 0 ).segment< 3 >( 0 ), linkEndStates.at( 1 ).segment< 3 >( 0 ), + moonState.segment< 3 >( 0 ) ); + + double manualMoonAvoidanceAngle = linear_algebra::computeAngleBetweenVectors( stationToMoon, vectorToTarget ); + double manualOrbitalPlaneAngle = linear_algebra::computeAngleBetweenVectors( orbitalAngularMomentum, vectorToTarget ) - mathematical_constants::PI / 2.0; + + BOOST_CHECK_SMALL( std::fabs( azimuthAngle - currentAzimuth ), std::numeric_limits::epsilon( )); + + BOOST_CHECK_SMALL( std::fabs( targetRange - vectorToTarget.norm( )), std::numeric_limits::epsilon( ) * vectorToTarget.norm( )); - BOOST_CHECK_SMALL(( targetInverseRange - targetRange ), + BOOST_CHECK_SMALL( std::fabs( targetInverseRange - targetRange ), std::numeric_limits::epsilon( ) * vectorToTarget.norm( )); - BOOST_CHECK_SMALL(( targetInverseRange - vectorToTarget.norm( )), + BOOST_CHECK_SMALL( std::fabs( targetInverseRange - vectorToTarget.norm( )), std::numeric_limits::epsilon( ) * vectorToTarget.norm( )); + BOOST_CHECK_SMALL( std::fabs( linkDistanceToMoon - linkBodyDistance ), 1.0E-4 ); + + BOOST_CHECK_SMALL( std::fabs( linkBodyDistance - linkBodyInverseDistance ), + std::numeric_limits::epsilon( ) * 1.0E7 ); + BOOST_CHECK_SMALL( std::fabs( linkLimbDistance - linkLimbInverseDistance ), + std::numeric_limits::epsilon( ) * 1.0E7 ); + BOOST_CHECK_SMALL( std::fabs( linkBodyDistance - linkLimbDistance - spice_interface::getAverageRadius( "Moon" ) ), + std::numeric_limits::epsilon( ) * 1.0E7 ); + BOOST_CHECK_SMALL( std::fabs( moonAvoidanceAngle - manualMoonAvoidanceAngle ), 1.0E-12 ); + BOOST_CHECK_SMALL( std::fabs( orbitalPlaneAngle - manualOrbitalPlaneAngle ), 1.0E-10 ); + + } } if( compareAgainstReceiver )