//! 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;
}
//==============================================================================
std::string toString(const Eigen::Vector6d& _v)
{
return boost::lexical_cast<std::string>(_v.transpose());
}