Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/angularPositionPartial.cpp
//! Update the scaling object to the current times and states
Update the scaling object to the current times and states
Update the scaling object to the current times and states
void AngularPositionScaling::update( const std::vector< Eigen::Vector6d >& linkEndStates,
                                     const std::vector< double >& times,
                                     const observation_models::LinkEndType fixedLinkEnd,
                                     const Eigen::VectorXd currentObservation )
{
    Eigen::Vector3d relativeRangeVector = ( linkEndStates[ 1 ] - linkEndStates[ 0 ] ).segment( 0, 3 );
    Eigen::Vector3d normalizedRelativeRangeVector = relativeRangeVector.normalized( );
    scalingFactor_ = calculatePartialOfAngularPositionWrtLinkEndPosition( relativeRangeVector, true );
    if( fixedLinkEnd == observation_models::receiver )
    {
        referenceLightTimeCorrectionScaling_ = scalingFactor_ * linkEndStates[ 0 ].segment( 3, 3 ) /
                ( physical_constants::SPEED_OF_LIGHT - linkEndStates[ 0 ].segment( 3, 3 ).dot( normalizedRelativeRangeVector ) );
        referenceScalingFactor_ =
                scalingFactor_ *
                ( Eigen::Matrix3d::Identity( ) + linkEndStates[ 0 ].segment( 3, 3 ) * normalizedRelativeRangeVector.transpose( ) /
                ( physical_constants::SPEED_OF_LIGHT - linkEndStates[ 0 ].segment( 3, 3 ).dot( normalizedRelativeRangeVector ) ) );
    }
    else if( fixedLinkEnd == observation_models::transmitter )
    {
        referenceLightTimeCorrectionScaling_ = scalingFactor_ * linkEndStates[ 1 ].segment( 3, 3 ) /
                ( physical_constants::SPEED_OF_LIGHT - linkEndStates[ 1 ].segment( 3, 3 ).dot( normalizedRelativeRangeVector ) );
        referenceScalingFactor_ =
                scalingFactor_ *
                ( Eigen::Matrix3d::Identity( ) + linkEndStates[ 1 ].segment( 3, 3 ) * normalizedRelativeRangeVector.transpose( ) /
                ( physical_constants::SPEED_OF_LIGHT - linkEndStates[ 1 ].segment( 3, 3 ).dot( normalizedRelativeRangeVector ) ) );
    }
    currentLinkEndType_ = fixedLinkEnd;
}
void AngularPositionScaling :: update ( const std :: vector < Eigen :: Vector6d > & linkEndStates , const std :: vector < double > & times , const observation_models :: LinkEndType fixedLinkEnd , const Eigen :: VectorXd currentObservation ) { Eigen :: Vector3d relativeRangeVector = ( linkEndStates [ 1 ] - linkEndStates [ 0 ] ) . segment ( 0 , 3 ) ; Eigen :: Vector3d normalizedRelativeRangeVector = relativeRangeVector . normalized ( ) ; scalingFactor_ = calculatePartialOfAngularPositionWrtLinkEndPosition ( relativeRangeVector , true ) ; if ( fixedLinkEnd == observation_models :: receiver ) { referenceLightTimeCorrectionScaling_ = scalingFactor_ * linkEndStates [ 0 ] . segment ( 3 , 3 ) / ( physical_constants :: SPEED_OF_LIGHT - linkEndStates [ 0 ] . segment ( 3 , 3 ) . dot ( normalizedRelativeRangeVector ) ) ; referenceScalingFactor_ = scalingFactor_ * ( Eigen :: Matrix3d :: Identity ( ) + linkEndStates [ 0 ] . segment ( 3 , 3 ) * normalizedRelativeRangeVector . transpose ( ) / ( physical_constants :: SPEED_OF_LIGHT - linkEndStates [ 0 ] . segment ( 3 , 3 ) . dot ( normalizedRelativeRangeVector ) ) ) ; } else if ( fixedLinkEnd == observation_models :: transmitter ) { referenceLightTimeCorrectionScaling_ = scalingFactor_ * linkEndStates [ 1 ] . segment ( 3 , 3 ) / ( physical_constants :: SPEED_OF_LIGHT - linkEndStates [ 1 ] . segment ( 3 , 3 ) . dot ( normalizedRelativeRangeVector ) ) ; referenceScalingFactor_ = scalingFactor_ * ( Eigen :: Matrix3d :: Identity ( ) + linkEndStates [ 1 ] . segment ( 3 , 3 ) * normalizedRelativeRangeVector . transpose ( ) / ( physical_constants :: SPEED_OF_LIGHT - linkEndStates [ 1 ] . segment ( 3 , 3 ) . dot ( normalizedRelativeRangeVector ) ) ) ; } currentLinkEndType_ = fixedLinkEnd ; }
Update the scaling object to the current times and states
Update the scaling object to the current times and states
// Compute common scaling factor // Compute scaling for receiver reference // Compute scaling for transmitter reference