//! Function determine the numerical partial derivative of the true anomaly wrt the elements of the Cartesian state
Eigen::Matrix< double, 1, 6 > calculateNumericalPartialOfTrueAnomalyWrtState(
        const Eigen::Vector6d& cartesianElements, const double gravitationalParameter,
        const Eigen::Vector6d& cartesianStateElementPerturbations )
{
    using namespace tudat::orbital_element_conversions;

    // Initialize partial to zero
    Eigen::Matrix< double, 1, 6 > partial = Eigen::Matrix< double, 1, 6 >::Zero( );

    // Iterate over all six elements and calculate partials.
    Eigen::Vector6d perturbedCartesianElements;
    double upPerturbedTrueAnomaly, downPerturbedTrueAnomaly;
    for( int i = 0; i < 6; i++ )
    {
        // Calculate true anomaly at up-perturbed cartesian element i
        perturbedCartesianElements = cartesianElements;
        perturbedCartesianElements( i ) += cartesianStateElementPerturbations( i );
        upPerturbedTrueAnomaly = convertCartesianToKeplerianElements( perturbedCartesianElements, gravitationalParameter )( trueAnomalyIndex );

        // Check validity of result
        if( upPerturbedTrueAnomaly != upPerturbedTrueAnomaly )
        {
            std::cout << "Error 1 in partial of true anomaly wrt cartesian state, element" << i
                      << ", keplerian state: " << std::endl
                      << convertCartesianToKeplerianElements( perturbedCartesianElements, gravitationalParameter ).transpose( ) << std::endl
                      << perturbedCartesianElements.transpose( ) << std::endl;
        }

        // Calculate true anomaly at down-perturbed cartesian element i
        perturbedCartesianElements = cartesianElements;
        perturbedCartesianElements( i ) -= cartesianStateElementPerturbations( i );
        downPerturbedTrueAnomaly = convertCartesianToKeplerianElements( perturbedCartesianElements, gravitationalParameter )( trueAnomalyIndex );

        // Check validity of result
        if( downPerturbedTrueAnomaly != downPerturbedTrueAnomaly )
        {
            std::cout << "Error 2 in partial of true anomaly wrt cartesian state, element" << i
                      << ", keplerian state: " << std::endl
                      << convertCartesianToKeplerianElements( perturbedCartesianElements, gravitationalParameter ).transpose( ) << std::endl
                      << perturbedCartesianElements.transpose( ) << std::endl;
        }

        // Calculate central difference of term i
        partial( 0, i ) = ( upPerturbedTrueAnomaly - downPerturbedTrueAnomaly ) / ( 2.0 * cartesianStateElementPerturbations( i ) );

        // Check validity of result
        if( partial( 0, i ) != partial( 0, i ) )
        {
            std::cout << "Error 2 in partial of true anomaly wrt cartesian state, element" << i << std::endl;
        }
    }

    return partial;
}
Пример #2
0
//==============================================================================
std::string toString(const Eigen::Vector6d& _v)
{
  return boost::lexical_cast<std::string>(_v.transpose());
}