#include "fsfw/coordinates/CoordinateTransformations.h" #include #include #include "fsfw/globalfunctions/constants.h" #include "fsfw/globalfunctions/math/MatrixOperations.h" #include "fsfw/globalfunctions/math/VectorOperations.h" #include "fsfw/globalfunctions/sign.h" #include "fsfw/serviceinterface.h" void CoordinateTransformations::positionEcfToEci(const double* ecfPosition, double* eciPosition, timeval* timeUTC) { ecfToEci(ecfPosition, eciPosition, NULL, timeUTC); } void CoordinateTransformations::velocityEcfToEci(const double* ecfVelocity, const double* ecfPosition, double* eciVelocity, timeval* timeUTC) { ecfToEci(ecfVelocity, eciVelocity, ecfPosition, timeUTC); } void CoordinateTransformations::positionEciToEcf(const double* eciCoordinates, double* ecfCoordinates, timeval* timeUTC) { eciToEcf(eciCoordinates, ecfCoordinates, NULL, timeUTC); }; void CoordinateTransformations::velocityEciToEcf(const double* eciVelocity, const double* eciPosition, double* ecfVelocity, timeval* timeUTC) { eciToEcf(eciVelocity, ecfVelocity, eciPosition, timeUTC); } double CoordinateTransformations::getEarthRotationAngle(timeval timeUTC) { double jD2000UTC; Clock::convertTimevalToJD2000(timeUTC, &jD2000UTC); double TTt2000 = getJuleanCenturiesTT(timeUTC); double theta = 2 * Math::PI * (0.779057273264 + 1.00273781191135448 * jD2000UTC); // Correct theta according to IAU 2000 precession-nutation model theta = theta + 7.03270725817493E-008 + 0.0223603701 * TTt2000 + 6.77128219501896E-006 * TTt2000 * TTt2000 + 4.5300990362875E-010 * TTt2000 * TTt2000 * TTt2000 + 9.12419347848147E-011 * TTt2000 * TTt2000 * TTt2000 * TTt2000; return theta; } void CoordinateTransformations::getEarthRotationMatrix(timeval timeUTC, double matrix[][3]) { double theta = getEarthRotationAngle(timeUTC); matrix[0][0] = cos(theta); matrix[0][1] = sin(theta); matrix[0][2] = 0; matrix[1][0] = -sin(theta); matrix[1][1] = cos(theta); matrix[1][2] = 0; matrix[2][0] = 0; matrix[2][1] = 0; matrix[2][2] = 1; } void CoordinateTransformations::ecfToEci(const double* ecfCoordinates, double* eciCoordinates, const double* ecfPositionIfCoordinatesAreVelocity, timeval* timeUTCin) { timeval timeUTC; if (timeUTCin != NULL) { timeUTC = *timeUTCin; } else { Clock::getClock_timeval(&timeUTC); } double Tfi[3][3]; double Tif[3][3]; getTransMatrixECITOECF(timeUTC, Tfi); MatrixOperations::transpose(Tfi[0], Tif[0], 3); MatrixOperations::multiply(Tif[0], ecfCoordinates, eciCoordinates, 3, 3, 1); if (ecfPositionIfCoordinatesAreVelocity != NULL) { double Tdotfi[3][3]; double Tdotif[3][3]; double Trot[3][3] = {{0, Earth::OMEGA, 0}, {0 - Earth::OMEGA, 0, 0}, {0, 0, 0}}; MatrixOperations::multiply(Trot[0], Tfi[0], Tdotfi[0], 3, 3, 3); MatrixOperations::transpose(Tdotfi[0], Tdotif[0], 3); double velocityCorrection[3]; MatrixOperations::multiply(Tdotif[0], ecfPositionIfCoordinatesAreVelocity, velocityCorrection, 3, 3, 1); VectorOperations::add(velocityCorrection, eciCoordinates, eciCoordinates, 3); } } double CoordinateTransformations::getJuleanCenturiesTT(timeval timeUTC) { timeval timeTT; ReturnValue_t result = Clock::convertUTCToTT(timeUTC, &timeTT); if (result != returnvalue::OK) { // i think it is better to continue here than to abort timeTT = timeUTC; #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "CoordinateTransformations::Conversion from UTC to TT failed. Continuing " "calculations with UTC." << std::endl; #endif } double jD2000TT; Clock::convertTimevalToJD2000(timeTT, &jD2000TT); return jD2000TT / 36525.; } void CoordinateTransformations::eciToEcf(const double* eciCoordinates, double* ecfCoordinates, const double* eciPositionIfCoordinatesAreVelocity, timeval* timeUTCin) { timeval timeUTC; if (timeUTCin != NULL) { timeUTC = *timeUTCin; } else { Clock::getClock_timeval(&timeUTC); } double Tfi[3][3]; getTransMatrixECITOECF(timeUTC, Tfi); MatrixOperations::multiply(Tfi[0], eciCoordinates, ecfCoordinates, 3, 3, 1); if (eciPositionIfCoordinatesAreVelocity != NULL) { double Tdotfi[3][3]; double Trot[3][3] = {{0, Earth::OMEGA, 0}, {0 - Earth::OMEGA, 0, 0}, {0, 0, 0}}; MatrixOperations::multiply(Trot[0], Tfi[0], Tdotfi[0], 3, 3, 3); double velocityCorrection[3]; MatrixOperations::multiply(Tdotfi[0], eciPositionIfCoordinatesAreVelocity, velocityCorrection, 3, 3, 1); VectorOperations::add(ecfCoordinates, velocityCorrection, ecfCoordinates, 3); } }; void CoordinateTransformations::getTransMatrixECITOECF(timeval timeUTC, double Tfi[3][3]) { double TTt2000 = getJuleanCenturiesTT(timeUTC); ////////////////////////////////////////////////////////// // Calculate Precession Matrix double zeta = 0.0111808609 * TTt2000 + 1.46355554053347E-006 * TTt2000 * TTt2000 + 8.72567663260943E-008 * TTt2000 * TTt2000 * TTt2000; double theta_p = 0.0097171735 * TTt2000 - 2.06845757045384E-006 * TTt2000 * TTt2000 - 2.02812107218552E-007 * TTt2000 * TTt2000 * TTt2000; double z = zeta + 3.8436028638364E-006 * TTt2000 * TTt2000 + 0.000000001 * TTt2000 * TTt2000 * TTt2000; double mPrecession[3][3]; mPrecession[0][0] = -sin(z) * sin(zeta) + cos(z) * cos(theta_p) * cos(zeta); mPrecession[1][0] = cos(z) * sin(zeta) + sin(z) * cos(theta_p) * cos(zeta); mPrecession[2][0] = sin(theta_p) * cos(zeta); mPrecession[0][1] = -sin(z) * cos(zeta) - cos(z) * cos(theta_p) * sin(zeta); mPrecession[1][1] = cos(z) * cos(zeta) - sin(z) * cos(theta_p) * sin(zeta); mPrecession[2][1] = -sin(theta_p) * sin(zeta); mPrecession[0][2] = -cos(z) * sin(theta_p); mPrecession[1][2] = -sin(z) * sin(theta_p); mPrecession[2][2] = cos(theta_p); ////////////////////////////////////////////////////////// // Calculate Nutation Matrix double omega_moon = 2.1824386244 - 33.7570459338 * TTt2000 + 3.61428599267159E-005 * TTt2000 * TTt2000 + 3.87850944887629E-008 * TTt2000 * TTt2000 * TTt2000; double deltaPsi = -0.000083388 * sin(omega_moon); double deltaEpsilon = 4.46174030725106E-005 * cos(omega_moon); double epsilon = 0.4090928042 - 0.0002269655 * TTt2000 - 2.86040071854626E-009 * TTt2000 * TTt2000 + 8.78967203851589E-009 * TTt2000 * TTt2000 * TTt2000; double mNutation[3][3]; mNutation[0][0] = cos(deltaPsi); mNutation[1][0] = cos(epsilon + deltaEpsilon) * sin(deltaPsi); mNutation[2][0] = sin(epsilon + deltaEpsilon) * sin(deltaPsi); mNutation[0][1] = -cos(epsilon) * sin(deltaPsi); mNutation[1][1] = cos(epsilon) * cos(epsilon + deltaEpsilon) * cos(deltaPsi) + sin(epsilon) * sin(epsilon + deltaEpsilon); mNutation[2][1] = cos(epsilon) * sin(epsilon + deltaEpsilon) * cos(deltaPsi) - sin(epsilon) * cos(epsilon + deltaEpsilon); mNutation[0][2] = -sin(epsilon) * sin(deltaPsi); mNutation[1][2] = sin(epsilon) * cos(epsilon + deltaEpsilon) * cos(deltaPsi) - cos(epsilon) * sin(epsilon + deltaEpsilon); mNutation[2][2] = sin(epsilon) * sin(epsilon + deltaEpsilon) * cos(deltaPsi) + cos(epsilon) * cos(epsilon + deltaEpsilon); ////////////////////////////////////////////////////////// // Calculate Earth rotation matrix // calculate theta double mTheta[3][3]; double Ttemp[3][3]; getEarthRotationMatrix(timeUTC, mTheta); // polar motion is neglected MatrixOperations::multiply(mNutation[0], mPrecession[0], Ttemp[0], 3, 3, 3); MatrixOperations::multiply(mTheta[0], Ttemp[0], Tfi[0], 3, 3, 3); }; void CoordinateTransformations::cartesianFromLatLongAlt(const double lat, const double longi, const double alt, double* cartesianOutput) { /* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude, * longitude and altitude * @param: lat geodetic latitude [rad] * longi longitude [rad] * alt altitude [m] * cartesianOutput Cartesian Coordinates in ECEF (3x1) * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff * Landis Markley and John L. Crassidis*/ double radiusPolar = 6356752.314; double radiusEqua = 6378137; double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2)); double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2)); cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi); cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi); cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat); }; void CoordinateTransformations::latLongAltFromCartesian(const double* vector, double& latitude, double& longitude, double& altitude) { /* @brief: latLongAltFromCartesian() - calculates latitude, longitude and altitude from * cartesian coordinates in ECEF * @param: x x-value of position vector [m] * y y-value of position vector [m] * z z-value of position vector [m] * latitude geodetic latitude [rad] * longitude longitude [rad] * altitude altitude [m] * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.35 f * Landis Markley and John L. Crassidis*/ // From World Geodetic System the Earth Radii double a = 6378137.0; // semimajor axis [m] double b = 6356752.3142; // semiminor axis [m] // Calculation double e2 = 1 - pow(b, 2) / pow(a, 2); double epsilon2 = pow(a, 2) / pow(b, 2) - 1; double rho = sqrt(pow(vector[0], 2) + pow(vector[1], 2)); double p = std::abs(vector[2]) / epsilon2; double s = pow(rho, 2) / (e2 * epsilon2); double q = pow(p, 2) - pow(b, 2) + s; double u = p / sqrt(q); double v = pow(b, 2) * pow(u, 2) / q; double P = 27 * v * s / q; double Q = pow(sqrt(P + 1) + sqrt(P), 2. / 3.); double t = (1 + Q + 1 / Q) / 6; double c = sqrt(pow(u, 2) - 1 + 2 * t); double w = (c - u) / 2; double d = sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1. / 4.)); double N = a * sqrt(1 + epsilon2 * pow(d, 2) / pow(b, 2)); latitude = asin((epsilon2 + 1) * d / N); altitude = rho * cos(latitude) + vector[2] * sin(latitude) - pow(a, 2) / N; longitude = atan2(vector[1], vector[0]); }