/*
* calculate lookangle between the observer and the passed in Eci object
*/
CoordTopocentric Observer::GetLookAngle(const Eci &eci)
{
/*
* update the observers Eci to match the time of the Eci passed in
* if necessary
*/
Update(eci.GetDateTime());
/*
* calculate differences
*/
Vector range_rate = eci.Velocity() - m_eci.Velocity();
Vector range = eci.Position() - m_eci.Position();
range.w = range.Magnitude();
/*
* Calculate Local Mean Sidereal Time for observers longitude
*/
double theta = eci.GetDateTime().ToLocalMeanSiderealTime(m_geo.longitude);
double sin_lat = sin(m_geo.latitude);
double cos_lat = cos(m_geo.latitude);
double sin_theta = sin(theta);
double cos_theta = cos(theta);
double top_s = sin_lat * cos_theta * range.x
+ sin_lat * sin_theta * range.y - cos_lat * range.z;
double top_e = -sin_theta * range.x
+ cos_theta * range.y;
double top_z = cos_lat * cos_theta * range.x
+ cos_lat * sin_theta * range.y + sin_lat * range.z;
double az = atan(-top_e / top_s);
if (top_s > 0.0)
{
az += kPI;
}
if (az < 0.0)
{
az += 2.0 * kPI;
}
double el = asin(top_z / range.w);
double rate = range.Dot(range_rate) / range.w;
/*
* azimuth in radians
* elevation in radians
* range in km
* range rate in km/s
*/
return CoordTopocentric(az,
el,
range.w,
rate);
}
int computeCartesianToTwoLineElementResiduals( const gsl_vector* independentVariables,
void* parameters,
gsl_vector* residuals )
{
const Vector6 targetState
= static_cast< CartesianToTwoLineElementsParameters< Vector6 >* >( parameters )->targetState;
const Tle templateTle
= static_cast< CartesianToTwoLineElementsParameters< Vector6 >* >( parameters )->templateTle;
// Create a TLE object with the mean TLE elements generated by the root-finder.
Tle tle = templateTle;
Real meanInclination = sml::computeModulo( std::fabs( gsl_vector_get( independentVariables, 0 ) ), 180.0 );
Real meanRightAscendingNode = sml::computeModulo( gsl_vector_get( independentVariables, 1 ), 360.0 );
Real meanEccentricity = gsl_vector_get( independentVariables, 2 );
if ( meanEccentricity < 0.0 )
{
meanEccentricity = std::fabs( gsl_vector_get( independentVariables, 2 ) );
}
if ( meanEccentricity > 0.999 )
{
meanEccentricity = 0.99;
}
Real meanArgumentPerigee = sml::computeModulo( gsl_vector_get( independentVariables, 3 ), 360.0 );
Real meanMeanAnomaly = sml::computeModulo( gsl_vector_get( independentVariables, 4 ), 360.0 );
Real meanMeanMotion = std::fabs( gsl_vector_get( independentVariables, 5 ) );
tle.updateMeanElements( meanInclination,
meanRightAscendingNode,
meanEccentricity,
meanArgumentPerigee,
meanMeanAnomaly,
meanMeanMotion );
// Propagate the TLE object to the specified epoch using the SGP4 propagator.
SGP4 sgp4( tle );
Eci cartesianState = sgp4.FindPosition( 0.0 );
// Compute residuals by computing the difference between the Cartesian state generated by the
// SGP4 propagator and the target Cartesian state.
gsl_vector_set( residuals, astro::xPositionIndex, cartesianState.Position( ).x - targetState[ astro::xPositionIndex ] );
gsl_vector_set( residuals, astro::yPositionIndex, cartesianState.Position( ).y - targetState[ astro::yPositionIndex ] );
gsl_vector_set( residuals, astro::zPositionIndex, cartesianState.Position( ).z - targetState[ astro::zPositionIndex ] );
gsl_vector_set( residuals, astro::xVelocityIndex, cartesianState.Velocity( ).x - targetState[ astro::xVelocityIndex ] );
gsl_vector_set( residuals, astro::yVelocityIndex, cartesianState.Velocity( ).y - targetState[ astro::yVelocityIndex ] );
gsl_vector_set( residuals, astro::zVelocityIndex, cartesianState.Velocity( ).z - targetState[ astro::zVelocityIndex ] );
return GSL_SUCCESS;
}
void tle::eph_impl(double mjd2000, array3D &r, array3D &v) const {
Vector position;
Vector velocity;
double minutes_since = (mjd2000-m_ref_mjd2000)*24*60;
try
{
Eci eci = m_sgp4_propagator.FindPosition(minutes_since);
position = eci.Position();
velocity = eci.Velocity();
r[0] = position.x*1000; r[1] = position.y*1000; r[2] = position.z*1000;
v[0] = velocity.x*1000; v[1] = velocity.y*1000; v[2] = velocity.z*1000;
}
catch (SatelliteException& e)
{
std::cout << "SatelliteException caught while computing ephemerides" << std::endl;
throw_value_error(e.what());
}
catch (DecayedException& e)
{
std::cout << "DecayedException caught while computing ephemerides" << std::endl;
throw_value_error(e.what());
}
}
int computeAtomResiduals( const gsl_vector* independentVariables,
void* parameters,
gsl_vector* residuals )
{
// Store parameters locally.
const Vector3 departurePosition = static_cast< AtomParameters< Real, Vector3 >* >(
parameters )->departurePosition;
const DateTime departureEpoch
= static_cast< AtomParameters< Real, Vector3 >* >( parameters )->departureEpoch;
const Vector3 targetPosition = static_cast< AtomParameters< Real, Vector3 >* >(
parameters )->targetPosition;
const Real timeOfFlight
= static_cast< AtomParameters< Real, Vector3 >* >(
parameters )->timeOfFlight;
const Real earthGravitationalParameter
= static_cast< AtomParameters< Real, Vector3 >* >(
parameters )->earthGravitationalParameter;
const Real earthMeanRadius
= static_cast< AtomParameters< Real, Vector3 >* >( parameters )->earthMeanRadius;
const Tle referenceTle
= static_cast< AtomParameters< Real, Vector3 >* >( parameters )->referenceTle;
const Real absoluteTolerance
= static_cast< AtomParameters< Real, Vector3 >* >( parameters )->absoluteTolerance;
const Real relativeTolerance
= static_cast< AtomParameters< Real, Vector3 >* >( parameters )->relativeTolerance;
const int maximumIterations
= static_cast< AtomParameters< Real, Vector3 >* >( parameters )->maximumIterations;
// Set Departure state [km; km/s].
std::vector< Real > departureVelocity( 3 );
for ( int i = 0; i < 3; i++ )
{
departureVelocity[ i ] = gsl_vector_get( independentVariables, i );
}
std::vector< Real > departureState( 6 );
for ( int i = 0; i < 3; i++ )
{
departureState[ i ] = departurePosition[ i ];
}
for ( int i = 0; i < 3; i++ )
{
departureState[ i + 3 ] = departureVelocity[ i ];
}
// Convert departure state to TLE.
std::string dummyString = "";
int dummyint = 0;
const Tle departureTle = convertCartesianStateToTwoLineElements(
departureState,
departureEpoch,
dummyString,
dummyint,
referenceTle,
earthGravitationalParameter,
earthMeanRadius,
absoluteTolerance,
relativeTolerance,
maximumIterations );
// Propagate departure TLE by time-of-flight using SGP4 propagator.
SGP4 sgp4( departureTle );
DateTime arrivalEpoch = departureEpoch.AddSeconds( timeOfFlight );
Eci arrivalState = sgp4.FindPosition( arrivalEpoch );
// Evaluate system of non-linear equations and store residuals.
gsl_vector_set( residuals, 0,
( arrivalState.Position( ).x - targetPosition[ 0 ] ) / earthMeanRadius );
gsl_vector_set( residuals, 1,
( arrivalState.Position( ).y - targetPosition[ 1 ] ) / earthMeanRadius );
gsl_vector_set( residuals, 2,
( arrivalState.Position( ).z - targetPosition[ 2 ] ) / earthMeanRadius );
return GSL_SUCCESS;
}
const std::pair< Vector3, Vector3 > executeAtomSolver(
const Vector3& departurePosition,
const DateTime& departureEpoch,
const Vector3& arrivalPosition,
const Real timeOfFlight,
const Vector3& departureVelocityGuess,
std::string& solverStatusSummary,
int& numberOfIterations,
const Tle& referenceTle,
const Real earthGravitationalParameter,
const Real earthMeanRadius,
const Real absoluteTolerance,
const Real relativeTolerance,
const int maximumIterations )
{
// Set up parameters for residual function.
AtomParameters< Real, Vector3 > parameters( departurePosition,
departureEpoch,
arrivalPosition,
timeOfFlight,
earthGravitationalParameter,
earthMeanRadius,
referenceTle,
absoluteTolerance,
relativeTolerance,
maximumIterations );
// Set up residual function.
gsl_multiroot_function atomFunction
= {
&computeAtomResiduals< Real, Vector3 >, 3, ¶meters
};
// Set initial guess.
gsl_vector* initialGuess = gsl_vector_alloc( 3 );
for ( int i = 0; i < 3; i++ )
{
gsl_vector_set( initialGuess, i, departureVelocityGuess[ i ] );
}
// Set up solver type (derivative free).
const gsl_multiroot_fsolver_type* solverType = gsl_multiroot_fsolver_hybrids;
// Allocate memory for solver.
gsl_multiroot_fsolver* solver = gsl_multiroot_fsolver_alloc( solverType, 3 );
// Set solver to use residual function with initial guess.
gsl_multiroot_fsolver_set( solver, &atomFunction, initialGuess );
// Declare current solver status and iteration counter.
int solverStatus = false;
int counter = 0;
// Set up buffer to store solver status summary table.
std::ostringstream summary;
// Print header for summary table to buffer.
summary << printAtomSolverStateTableHeader( );
do
{
// Print current state of solver for summary table.
summary << printAtomSolverState( counter, solver );
// Increment iteration counter.
++counter;
// Execute solver iteration.
solverStatus = gsl_multiroot_fsolver_iterate( solver );
// Check if solver is stuck; if it is stuck, break from loop.
if ( solverStatus )
{
std::cerr << "GSL solver status: " << solverStatus << std::endl;
std::cerr << summary.str( ) << std::endl;
std::cerr << std::endl;
throw std::runtime_error( "ERROR: Non-linear solver is stuck!" );
}
// Check if root has been found (within tolerance).
solverStatus = gsl_multiroot_test_delta(
solver->dx, solver->x, absoluteTolerance, relativeTolerance );
} while ( solverStatus == GSL_CONTINUE && counter < maximumIterations );
// Save number of iterations.
numberOfIterations = counter - 1;
// Print final status of solver to buffer.
summary << std::endl;
summary << "Status of non-linear solver: " << gsl_strerror( solverStatus ) << std::endl;
summary << std::endl;
// Write buffer contents to solver status summary string.
solverStatusSummary = summary.str( );
// Store final departure velocity.
Vector3 departureVelocity = departureVelocityGuess;
for ( int i = 0; i < 3; i++ )
{
departureVelocity[ i ] = gsl_vector_get( solver->x, i );
}
// Set departure state [km/s].
std::vector< Real > departureState( 6 );
for ( int i = 0; i < 3; i++ )
{
departureState[ i ] = departurePosition[ i ];
}
for ( int i = 0; i < 3; i++ )
{
departureState[ i + 3 ] = departureVelocity[ i ];
}
// Convert departure state to TLE.
std::string dummyString = "";
int dummyint = 0;
const Tle departureTle = convertCartesianStateToTwoLineElements< Real >(
departureState,
departureEpoch,
dummyString,
dummyint,
referenceTle,
earthGravitationalParameter,
earthMeanRadius,
absoluteTolerance,
relativeTolerance,
maximumIterations );
// Propagate departure TLE by time-of-flight using SGP4 propagator.
SGP4 sgp4( departureTle );
DateTime arrivalEpoch = departureEpoch.AddSeconds( timeOfFlight );
Eci arrivalState = sgp4.FindPosition( arrivalEpoch );
Vector3 arrivalVelocity = departureVelocity;
arrivalVelocity[ 0 ] = arrivalState.Velocity( ).x;
arrivalVelocity[ 1 ] = arrivalState.Velocity( ).y;
arrivalVelocity[ 2 ] = arrivalState.Velocity( ).z;
// Free up memory.
gsl_multiroot_fsolver_free( solver );
gsl_vector_free( initialGuess );
// Return departure and arrival velocities.
return std::make_pair< Vector3, Vector3 >( departureVelocity, arrivalVelocity );
}