diff --git a/s2e-ff/CMakeLists.txt b/s2e-ff/CMakeLists.txt index 12d83540..105bb5a7 100644 --- a/s2e-ff/CMakeLists.txt +++ b/s2e-ff/CMakeLists.txt @@ -95,6 +95,9 @@ set(SOURCE_FILES src/Library/math/DualQuaternion.cpp src/Library/math/RotationFirstDualQuaternion.cpp src/Library/math/TranslationFirstDualQuaternion.cpp + src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp + src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp + src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp src/Simulation/Case/FfCase.cpp src/Simulation/Spacecraft/FfSat.cpp src/Simulation/Spacecraft/FfComponents.cpp @@ -199,12 +202,21 @@ if(GOOGLE_TEST) ## Unit test set(TEST_PROJECT_NAME ${PROJECT_NAME}_TEST) set(TEST_FILES + # Dual Quaternion src/Library/math/DualQuaternion.cpp test/TestDualQuaternion.cpp src/Library/math/RotationFirstDualQuaternion.cpp test/TestRotationFirstDualQuaternion.cpp src/Library/math/TranslationFirstDualQuaternion.cpp test/TestTranslationFirstDualQuaternion.cpp + # Orbital elements + src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp + test/TestQuasiNonsingularOrbitalElements.cpp + # Relative orbital elements + src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp + test/TestQuasiNonsingularOrbitalElementDifferences.cpp + src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp + test/TestQuasiNonsingularRelativeOrbitalElements.cpp ) add_executable(${TEST_PROJECT_NAME} ${TEST_FILES}) target_link_libraries(${TEST_PROJECT_NAME} gtest gtest_main) diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp new file mode 100644 index 00000000..2d6b02f6 --- /dev/null +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp @@ -0,0 +1,99 @@ +/** + * @file QuasiNonsingularOrbitalElements.cpp + * @brief Orbital elements avoid singularity when the eccentricity is near zero. + */ + +#include "QuasiNonsingularOrbitalElements.hpp" + +#include + +QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements() { + semi_major_axis_m_ = 0.0; + true_latitude_angle_rad_ = 0.0; + eccentricity_x_ = 0.0; + eccentricity_y_ = 0.0; + inclination_rad_ = 0.0; + raan_rad_ = 0.0; + CalcOrbitParameters(); +} + +QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const double semi_major_axis_m, const double eccentricity_x, + const double eccentricity_y, const double inclination_rad, const double raan_rad, + const double mean_arg_latitude_rad) + : semi_major_axis_m_(semi_major_axis_m), + eccentricity_x_(eccentricity_x), + eccentricity_y_(eccentricity_y), + inclination_rad_(inclination_rad), + raan_rad_(raan_rad), + true_latitude_angle_rad_(mean_arg_latitude_rad) { + CalcOrbitParameters(); +} + +QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const double mu_m3_s2, const libra::Vector<3> position_i_m, + const libra::Vector<3> velocity_i_m_s) { + // common variables + double r_m = norm(position_i_m); + double v2_m2_s2 = inner_product(velocity_i_m_s, velocity_i_m_s); + libra::Vector<3> h; + h = outer_product(position_i_m, velocity_i_m_s); + double h_norm = norm(h); + + // semi major axis + semi_major_axis_m_ = mu_m3_s2 / (2.0 * mu_m3_s2 / r_m - v2_m2_s2); + + // inclination + libra::Vector<3> h_direction = h; + h_direction = normalize(h_direction); + inclination_rad_ = acos(h_direction[2]); + inclination_rad_ = libra::WrapTo2Pi(inclination_rad_); + + // RAAN + double norm_h = sqrt(h[0] * h[0] + h[1] * h[1]); + if (norm_h < 0.0 + DBL_EPSILON) { + // We cannot define raan when i = 0 + raan_rad_ = 0.0; + } else { + raan_rad_ = asin(h[0] / sqrt(h[0] * h[0] + h[1] * h[1])); + } + raan_rad_ = libra::WrapTo2Pi(raan_rad_); + + // position in plane + double x_p_m = position_i_m[0] * cos(raan_rad_) + position_i_m[1] * sin(raan_rad_); + double tmp_m = -position_i_m[0] * sin(raan_rad_) + position_i_m[1] * cos(raan_rad_); + double y_p_m = tmp_m * cos(inclination_rad_) + position_i_m[2] * sin(inclination_rad_); + + // velocity in plane + double dx_p_m_s = velocity_i_m_s[0] * cos(raan_rad_) + velocity_i_m_s[1] * sin(raan_rad_); + double dtmp_m_s = -velocity_i_m_s[0] * sin(raan_rad_) + velocity_i_m_s[1] * cos(raan_rad_); + double dy_p_m_s = dtmp_m_s * cos(inclination_rad_) + velocity_i_m_s[2] * sin(inclination_rad_); + + // eccentricity + eccentricity_x_ = (h_norm / mu_m3_s2) * dy_p_m_s - x_p_m / r_m; + eccentricity_y_ = -(h_norm / mu_m3_s2) * dx_p_m_s - y_p_m / r_m; + + // true anomaly f_rad and eccentric anomaly u_rad + true_latitude_angle_rad_ = atan2(y_p_m, x_p_m); + true_latitude_angle_rad_ = libra::WrapTo2Pi(true_latitude_angle_rad_); + + CalcOrbitParameters(); +} + +QuasiNonsingularOrbitalElements ::~QuasiNonsingularOrbitalElements() {} + +QuasiNonsingularOrbitalElements operator-(const QuasiNonsingularOrbitalElements lhs, const QuasiNonsingularOrbitalElements rhs) { + double semi_major_axis_m = lhs.GetSemiMajor_m() - rhs.GetSemiMajor_m(); + double eccentricity_x = lhs.GetEccentricityX() - rhs.GetEccentricityX(); + double eccentricity_y = lhs.GetEccentricityY() - rhs.GetEccentricityY(); + double inclination_rad = lhs.GetInclination_rad() - rhs.GetInclination_rad(); + double raan_rad = lhs.GetRaan_rad() - rhs.GetRaan_rad(); + double true_latitude_angle_rad = lhs.GetTrueLatAng_rad() - rhs.GetTrueLatAng_rad(); + + QuasiNonsingularOrbitalElements out(semi_major_axis_m, eccentricity_x, eccentricity_y, inclination_rad, raan_rad, true_latitude_angle_rad); + + return out; +} + +void QuasiNonsingularOrbitalElements::CalcOrbitParameters() { + semi_latus_rectum_m_ = semi_major_axis_m_ * (1.0 - (eccentricity_x_ * eccentricity_x_ + eccentricity_y_ * eccentricity_y_)); + radius_m_ = semi_latus_rectum_m_ / (1.0 + eccentricity_x_ * cos(true_latitude_angle_rad_) + eccentricity_y_ * sin(true_latitude_angle_rad_)); +} diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp new file mode 100644 index 00000000..30d70aa7 --- /dev/null +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp @@ -0,0 +1,123 @@ +/** + * @file QuasiNonsingularOrbitalElements.hpp + * @brief Orbital elements avoid singularity when the eccentricity is near zero. + */ + +#ifndef QUASI_NONSINGULAR_ORBITAL_ELEMENTS_H_ +#define QUASI_NONSINGULAR_ORBITAL_ELEMENTS_H_ + +#include +#include + +/** + * @class QuasiNonsingularOrbitalElements + * @brief Orbital elements avoid singularity when the eccentricity is near zero. + */ +class QuasiNonsingularOrbitalElements { + public: + /** + * @fn QuasiNonsingularOrbitalElements + * @brief Default Constructor + */ + QuasiNonsingularOrbitalElements(); + /** + * @fn QuasiNonsingularOrbitalElements + * @brief Constructor initialized with values + * @param[in] semi_major_axis_m: Semi major axis [m] + * @param[in] eccentricity_x: Eccentricity X component + * @param[in] eccentricity_y: Eccentricity Y component + * @param[in] inclination_rad: Inclination [rad] + * @param[in] raan_rad: Right Ascension of the Ascending Node [rad] + * @param[in] true_latitude_angle_rad: True latitude angle [rad] + */ + QuasiNonsingularOrbitalElements(const double semi_major_axis_m, const double eccentricity_x, const double eccentricity_y, + const double inclination_rad, const double raan_rad, const double true_latitude_angle_rad); + /** + * @fn QuasiNonsingularOrbitalElements + * @brief Constructor initialized with position and velocity + * @param[in] mu_m3_s2: Gravity constant [m3/s2] + * @param[in] position_i_m: Position vector in the inertial frame [m] + * @param[in] velocity_i_m_s: Velocity vector in the inertial frame [m/s] + */ + QuasiNonsingularOrbitalElements(const double mu_m3_s2, const libra::Vector<3> position_i_m, const libra::Vector<3> velocity_i_m_s); + /** + * @fn ~QuasiNonsingularOrbitalElements + * @brief Destructor + */ + ~QuasiNonsingularOrbitalElements(); + + // Getter + /** + * @fn GetSemiMajor_m + * @brief Return semi-major axis [m] + */ + inline double GetSemiMajor_m() const { return semi_major_axis_m_; } + /** + * @fn GetEccentricityX + * @brief Return X component of eccentricity vector + */ + inline double GetEccentricityX() const { return eccentricity_x_; } + /** + * @fn GetEccentricityY + * @brief Return Y component of eccentricity vector + */ + inline double GetEccentricityY() const { return eccentricity_y_; } + /** + * @fn GetInclination_rad + * @brief Return inclination [rad] + */ + inline double GetInclination_rad() const { return inclination_rad_; } + /** + * @fn GetRaan_rad + * @brief Return Right Ascension of the Ascending Node [rad] + */ + inline double GetRaan_rad() const { return raan_rad_; } + /** + * @fn GetTrueLatAng_rad + * @brief Return True latitude angle [rad] + */ + inline double GetTrueLatAng_rad() const { return true_latitude_angle_rad_; } + /** + * @fn GetSemiLatusRectum_m + * @brief Return Semi-latus rectum [m] + */ + inline double GetSemiLatusRectum_m() const { return semi_latus_rectum_m_; } + /** + * @fn GetRadius_m + * @brief Return Current radius [m] + */ + inline double GetRadius_m() const { return radius_m_; } + + // Setter + /** + * @fn SetTrueLAtAng_rad + * @brief Set True latitude angle [rad] + */ + inline void GetTrueLatAng_rad(const double true_latitude_angle_rad) { true_latitude_angle_rad_ = libra::WrapTo2Pi(true_latitude_angle_rad); } + + private: + double semi_major_axis_m_; //!< Semi major axis [m] + double eccentricity_x_; //!< e * cos(arg_peri) + double eccentricity_y_; //!< e * sin(arg_peri) + double inclination_rad_; //!< Inclination [rad] + double raan_rad_; //!< Right Ascension of the Ascending Node [rad] + double true_latitude_angle_rad_; //!< True latitude angle (argment of periapsis + true anomaly) [rad] + + // Orbit states + double semi_latus_rectum_m_; //!< Semi-latus rectum [m] + double radius_m_; //!< Current radius [m] + + /** + * @fn CalcOrbitParameters + * @brief Calculate parameters for orbit + */ + void CalcOrbitParameters(); +}; + +/** + * @fn Operator - + * @brief Calculate subtract of two quasi-nonsingular orbital elements + */ +QuasiNonsingularOrbitalElements operator-(const QuasiNonsingularOrbitalElements lhs, const QuasiNonsingularOrbitalElements rhs); + +#endif diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp new file mode 100644 index 00000000..27bedce0 --- /dev/null +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp @@ -0,0 +1,162 @@ +/** + * @file QuasiNonsingularOrbitalElementDifferences.cpp + * @brief Orbital elements differences to avoid singularity when the eccentricity is near zero. + */ + +#include "QuasiNonsingularOrbitalElementDifferences.hpp" + +#include +#include + +QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDifferences(const QuasiNonsingularOrbitalElements qns_oe_reference, + const QuasiNonsingularOrbitalElements qns_oe_target) + : qns_oe_reference_(qns_oe_reference) { + diff_qns_oe_ = qns_oe_target - qns_oe_reference; +} + +QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDifferences(const QuasiNonsingularOrbitalElements qns_oe_reference, + const libra::Vector<3> relative_position_rtn_m, + const libra::Vector<3> relative_velocity_rtn_m_s, + const double mu_m3_s2) + : qns_oe_reference_(qns_oe_reference) { + // Reference orbit variables + const double a = qns_oe_reference_.GetSemiMajor_m(); + const double ex = qns_oe_reference_.GetEccentricityX(); + const double ey = qns_oe_reference_.GetEccentricityY(); + const double i = qns_oe_reference_.GetInclination_rad(); + const double sin_i = sin(i); + const double cot_i = cos(i) / sin_i; + const double theta = qns_oe_reference_.GetTrueLatAng_rad(); + const double cos_theta = cos(theta); + const double sin_theta = sin(theta); + const double cos_2theta = cos(2.0 * theta); + const double sin_2theta = sin(2.0 * theta); + const double p = qns_oe_reference_.GetSemiLatusRectum_m(); + const double h = sqrt(mu_m3_s2 * p); //!< Orbit angular momentum + const double r = qns_oe_reference_.GetRadius_m(); + + // Calculation + const double v_r = h / p * (ex * sin_theta - ey * cos_theta); + const double v_t = h / p * (1.0 + ex * cos_theta + ey * sin_theta); + + const double alpha = a / r; + const double nu = v_r / v_t; + const double rho = r / p; + const double kappa_1 = alpha * (1.0 / rho - 1.0); + const double kappa_2 = alpha * nu * nu / rho; + + libra::Matrix<6, 6> conversion_to_oed(0.0); + // For semi-major axis + conversion_to_oed[0][0] = 2.0 * alpha * (2.0 + 3.0 * kappa_1 + 2.0 * kappa_2); + conversion_to_oed[0][1] = -2.0 * alpha * nu * (1.0 + 2.0 * kappa_1 + kappa_2); + conversion_to_oed[0][3] = 2.0 * alpha * alpha * nu * p / v_t; + conversion_to_oed[0][4] = 2.0 * alpha * (1.0 + 2.0 * kappa_1 + kappa_2) / v_t; + // For true latitude angle + conversion_to_oed[1][1] = 1.0 / r; + conversion_to_oed[1][2] = cot_i / r * (cos_theta + nu * sin_theta); + conversion_to_oed[1][5] = -1.0 * sin_theta * cot_i / v_t; + // for inclination + conversion_to_oed[2][2] = (sin_theta - nu * cos_theta) / r; + conversion_to_oed[2][5] = cos_theta / v_t; + // For eccentricity vector X + conversion_to_oed[3][0] = (3.0 * cos_theta + 2.0 * nu * sin_theta) / (rho * r); + conversion_to_oed[3][1] = -1.0 * (nu * nu * sin_theta / rho + ex * sin_2theta - ey * cos_2theta) / r; + conversion_to_oed[3][2] = -1.0 * ey * cot_i * (cos_theta + nu * sin_theta) / r; + conversion_to_oed[3][3] = sin_theta / (rho * v_t); + conversion_to_oed[3][4] = (2.0 * cos_theta + nu * sin_theta) / (rho * v_t); + conversion_to_oed[3][5] = ey * cot_i * sin_theta / v_t; + // For eccentricity vector Y + conversion_to_oed[4][0] = (3.0 * sin_theta - 2.0 * nu * cos_theta) / (rho * r); + conversion_to_oed[4][1] = (nu * nu * cos_theta / rho + ey * sin_2theta + ex * cos_2theta) / r; + conversion_to_oed[4][2] = ex * cot_i * (cos_theta + nu * sin_theta) / r; + conversion_to_oed[4][3] = -1.0 * cos_theta / (rho * v_t); + conversion_to_oed[4][4] = (2.0 * sin_theta - nu * cos_theta) / (rho * v_t); + conversion_to_oed[4][5] = -1.0 * ex * cot_i * sin_theta / v_t; + // For RAAN + conversion_to_oed[5][2] = -1.0 * (cos_theta + nu * sin_theta) / (r * sin_i); + conversion_to_oed[5][5] = sin_theta / (v_t * sin_i); + + // Output + libra::Vector<6> position_and_velocity; + for (size_t i = 0; i < 3; i++) { + position_and_velocity[i] = relative_position_rtn_m[i]; + position_and_velocity[i + 3] = relative_velocity_rtn_m_s[i]; + } + libra::Vector<6> relative_oed; + relative_oed = conversion_to_oed * position_and_velocity; + QuasiNonsingularOrbitalElements relative_oed_tmp(relative_oed[0], relative_oed[3], relative_oed[4], relative_oed[2], relative_oed[5], + relative_oed[1]); + diff_qns_oe_ = relative_oed_tmp; +} + +QuasiNonsingularOrbitalElementDifferences::~QuasiNonsingularOrbitalElementDifferences() {} + +libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePosition_rtn_m() { + // Reference orbit variables + const double a = qns_oe_reference_.GetSemiMajor_m(); + const double ex = qns_oe_reference_.GetEccentricityX(); + const double ey = qns_oe_reference_.GetEccentricityY(); + const double i = qns_oe_reference_.GetInclination_rad(); + const double theta = qns_oe_reference_.GetTrueLatAng_rad(); + const double cos_theta = cos(theta); + const double sin_theta = sin(theta); + const double p = qns_oe_reference_.GetSemiLatusRectum_m(); + const double r = qns_oe_reference_.GetRadius_m(); + + // Relative orbit variables + const double d_a = diff_qns_oe_.GetSemiMajor_m(); + const double d_ex = diff_qns_oe_.GetEccentricityX(); + const double d_ey = diff_qns_oe_.GetEccentricityY(); + const double d_theta = diff_qns_oe_.GetTrueLatAng_rad(); + const double d_i = diff_qns_oe_.GetInclination_rad(); + const double d_raan = diff_qns_oe_.GetRaan_rad(); + + // Calculation + const double v_r = (ex * sin_theta - ey * cos_theta); // without h/p since it will be cancelled in v_r / v_t + const double v_t = (1.0 + ex * cos_theta + ey * sin_theta); + const double d_r = r / a * d_a + v_r / v_t * r * d_theta - r / p * ((2.0 * a * ex + r * cos_theta) * d_ex + (2.0 * a * ey + r * sin_theta) * d_ey); + + // Output + libra::Vector<3> relative_position_rtn_m; + relative_position_rtn_m[0] = d_r; + relative_position_rtn_m[1] = r * (d_theta + d_raan * cos(i)); + relative_position_rtn_m[2] = r * (d_i * sin_theta - d_raan * cos_theta * sin(i)); + + return relative_position_rtn_m; +} + +libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativeVelocity_rtn_m_s(const double mu_m3_s2) { + // Reference orbit variables + const double a = qns_oe_reference_.GetSemiMajor_m(); + const double ex = qns_oe_reference_.GetEccentricityX(); + const double ey = qns_oe_reference_.GetEccentricityY(); + const double i = qns_oe_reference_.GetInclination_rad(); + const double theta = qns_oe_reference_.GetTrueLatAng_rad(); + const double cos_theta = cos(theta); + const double sin_theta = sin(theta); + const double p = qns_oe_reference_.GetSemiLatusRectum_m(); + const double h = sqrt(mu_m3_s2 * p); //!< Orbit angular momentum + const double r = qns_oe_reference_.GetRadius_m(); + + // Relative orbit variables + const double d_a = diff_qns_oe_.GetSemiMajor_m(); + const double d_ex = diff_qns_oe_.GetEccentricityX(); + const double d_ey = diff_qns_oe_.GetEccentricityY(); + const double d_theta = diff_qns_oe_.GetTrueLatAng_rad(); + const double d_i = diff_qns_oe_.GetInclination_rad(); + const double d_raan = diff_qns_oe_.GetRaan_rad(); + + // Calculation + const double v_r = h / p * (ex * sin_theta - ey * cos_theta); + const double v_t = h / p * (1.0 + ex * cos_theta + ey * sin_theta); + + // Output + libra::Vector<3> relative_velocity_rtn_m_s; + relative_velocity_rtn_m_s[0] = -v_r / (2.0 * a) * d_a + (1.0 / r - 1.0 / p) * h * d_theta + (v_r * a * ex + h * sin_theta) * d_ex / p + + (v_r * a * ey - h * cos_theta) * d_ey / p; + relative_velocity_rtn_m_s[1] = -3.0 * v_t / (2.0 * a) * d_a - v_r * d_theta + (3.0 * v_t * a * ex + 2.0 * h * cos_theta) * d_ex / p + + (3.0 * v_t * a * ey + 2.0 * h * sin_theta) * d_ey / p + v_r * cos(i) * d_raan; + relative_velocity_rtn_m_s[2] = (v_t * cos_theta + v_r * sin_theta) * d_i + (v_t * sin_theta - v_r * cos_theta) * sin(i) * d_raan; + + return relative_velocity_rtn_m_s; +} diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp new file mode 100644 index 00000000..21e202ca --- /dev/null +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp @@ -0,0 +1,71 @@ +/** + * @file QuasiNonsingularOrbitalElementDifferences.hpp + * @brief Orbital elements differences to avoid singularity when the eccentricity is near zero. + */ + +#ifndef QUASI_NONSINGULAR_ORBITAL_ELEMENT_DIFFERENCES_H_ +#define QUASI_NONSINGULAR_ORBITAL_ELEMENT_DIFFERENCES_H_ + +#include + +#include "../Orbit/QuasiNonsingularOrbitalElements.hpp" + +/** + * @class QuasiNonsingularOrbitalElementDifferences + * @brief Orbital element differences defined by eccentricity/inclination vectors to avoid singularity when the eccentricity is near zero. + * @note Orbital element differences(OEDs) is defined as the arithmetic difference between two orbital elements + */ +class QuasiNonsingularOrbitalElementDifferences { + public: + /** + * @fn QuasiNonsingularOrbitalElementDifferences + * @brief Constructor initialized with tow quasi-nonsingular orbital elements + * @param [in] qns_oe_reference: Quasi-nonsingular orbital elements of the reference spacecraft + * @param [in] qns_oe_target: Quasi-nonsingular orbital elements of the target spacecraft + */ + QuasiNonsingularOrbitalElementDifferences(const QuasiNonsingularOrbitalElements qns_oe_reference, + const QuasiNonsingularOrbitalElements qns_oe_target); + /** + * @fn QuasiNonsingularOrbitalElementDifferences + * @brief Constructor initialized with tow quasi-nonsingular orbital elements + * @param [in] qns_oe_reference: Quasi-nonsingular orbital elements of the reference spacecraft + * @param [in] relative_position_rtn_m: Relative position of target satellite in the reference satellite's RTN frame [m] + * @param [in] relative_velocity_rtn_m_s: Relative velocity of target satellite in the reference satellite's RTN frame [m/s] + * @param [in] mu_m3_s2: Gravity constant of the center body [m3/s2] + */ + QuasiNonsingularOrbitalElementDifferences(const QuasiNonsingularOrbitalElements qns_oe_reference, const libra::Vector<3> relative_position_rtn_m, + const libra::Vector<3> relative_velocity_rtn_m_s, const double mu_m3_s2); + /** + * @fn ~QuasiNonsingularOrbitalElementDifferences + * @brief Destructor + */ + ~QuasiNonsingularOrbitalElementDifferences(); + + // Calculation + /** + * @fn CalcRelativePosition_rtn_m + * @brief Calculate the relative position of target spacecraft with short distance approximation + * @return Relative position vector in RTN frame of reference spacecraft [m] + */ + libra::Vector<3> CalcRelativePosition_rtn_m(); + /** + * @fn CalcRelativeVelocity_rtn_m_s + * @brief Calculate the relative velocity of target spacecraft with short distance approximation + * @param [in] mu_m3_s2: Gravity constant of the center body [m3/s2] + * @return Relative position vector in RTN frame of reference spacecraft [m/s] + */ + libra::Vector<3> CalcRelativeVelocity_rtn_m_s(const double mu_m3_s2); + + // Getter + /** + * @fn GetDiffQuasiNonSingularOrbitalElements + * @brief Return difference of quasi-nonsingular orbital elements + */ + inline QuasiNonsingularOrbitalElements GetDiffQuasiNonSingularOrbitalElements() const { return diff_qns_oe_; } + + private: + QuasiNonsingularOrbitalElements qns_oe_reference_; //!< Quasi-nonsingular orbital elements of reference spacecraft + QuasiNonsingularOrbitalElements diff_qns_oe_; //!< Difference of quasi-nonsingular orbital elements +}; + +#endif diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp new file mode 100644 index 00000000..256bbdd4 --- /dev/null +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp @@ -0,0 +1,112 @@ +/** + * @file QuasiNonsingularRelativeOrbitalElements.cpp + * @brief Relative orbital elements defined by eccentricity/inclination vectors to avoid singularity when the eccentricity is near zero. + */ + +#include "QuasiNonsingularRelativeOrbitalElements.hpp" + +#include + +QuasiNonsingularRelativeOrbitalElements::QuasiNonsingularRelativeOrbitalElements(const QuasiNonsingularOrbitalElements qns_oe_reference, + const QuasiNonsingularOrbitalElements qns_oe_target) { + semi_major_axis_ref_m_ = qns_oe_reference.GetSemiMajor_m(); + + d_semi_major_axis_ = (qns_oe_target.GetSemiMajor_m() - semi_major_axis_ref_m_) / semi_major_axis_ref_m_; + d_eccentricity_x_ = qns_oe_target.GetEccentricityX() - qns_oe_reference.GetEccentricityX(); + d_eccentricity_y_ = qns_oe_target.GetEccentricityY() - qns_oe_reference.GetEccentricityY(); + d_inclination_x_ = qns_oe_target.GetInclination_rad() - qns_oe_reference.GetInclination_rad(); + + const double diff_raan = qns_oe_target.GetRaan_rad() - qns_oe_reference.GetRaan_rad(); + d_inclination_y_ = diff_raan * sin(qns_oe_reference.GetInclination_rad()); + + // Calc difference of argument of latitude + const double d_mean_arg_lat_rad = CalcDiffMeanArgLat_rad(qns_oe_reference, qns_oe_target); + + d_mean_longitude_ = d_mean_arg_lat_rad + diff_raan * cos(qns_oe_reference.GetInclination_rad()); +} + +QuasiNonsingularRelativeOrbitalElements::QuasiNonsingularRelativeOrbitalElements(const double semi_major_axis_ref_m, + const libra::Vector<3> relative_position_rtn_m, + const libra::Vector<3> relative_velocity_rtn_m_s, + const double mu_m3_s2) { + const double a = semi_major_axis_ref_m; + const double n = sqrt(mu_m3_s2 / (a * a * a)); + const libra::Vector<3> dv = (1.0 / n) * relative_velocity_rtn_m_s; + + // Relative info + d_semi_major_axis_ = (4.0 * relative_position_rtn_m[0] + 2.0 * dv[1]) / a; + d_mean_longitude_ = (relative_position_rtn_m[1] - 2.0 * dv[0]) / a; + d_eccentricity_x_ = (3.0 * relative_position_rtn_m[0] + 2.0 * dv[1]) / a; + d_eccentricity_y_ = (-1.0 * dv[0]) / a; + d_inclination_x_ = (dv[2]) / a; + d_inclination_y_ = (-1.0 * relative_position_rtn_m[2]) / a; + + // Reference info + semi_major_axis_ref_m_ = a; +} + +QuasiNonsingularRelativeOrbitalElements::~QuasiNonsingularRelativeOrbitalElements() {} + +libra::Vector<3> QuasiNonsingularRelativeOrbitalElements::CalcRelativePositionCircularApprox_rtn_m(const double mean_arg_lat_rad) { + libra::Vector<3> relative_position_rtn_m; + const double cos_u = cos(mean_arg_lat_rad); + const double sin_u = sin(mean_arg_lat_rad); + + relative_position_rtn_m[0] = d_semi_major_axis_ - (d_eccentricity_x_ * cos_u + d_eccentricity_y_ * sin_u); + relative_position_rtn_m[1] = + -1.5 * d_semi_major_axis_ * mean_arg_lat_rad + d_mean_longitude_ + 2.0 * (d_eccentricity_x_ * sin_u - d_eccentricity_y_ * cos_u); + relative_position_rtn_m[2] = d_inclination_x_ * sin_u - d_inclination_y_ * cos_u; + + relative_position_rtn_m *= semi_major_axis_ref_m_; + return relative_position_rtn_m; +} + +libra::Vector<3> QuasiNonsingularRelativeOrbitalElements::CalcRelativeVelocityCircularApprox_rtn_m_s(const double mean_arg_lat_rad, + const double mu_m3_s2) { + libra::Vector<3> relative_velocity_rtn_m_s; + const double cos_u = cos(mean_arg_lat_rad); + const double sin_u = sin(mean_arg_lat_rad); + + const double a = semi_major_axis_ref_m_; + const double n = sqrt(mu_m3_s2 / (a * a * a)); + + relative_velocity_rtn_m_s[0] = d_eccentricity_x_ * sin_u - d_eccentricity_y_ * cos_u; + relative_velocity_rtn_m_s[1] = -1.5 * d_semi_major_axis_ + 2.0 * (d_eccentricity_x_ * cos_u + d_eccentricity_y_ * sin_u); + relative_velocity_rtn_m_s[2] = d_inclination_x_ * cos_u + d_inclination_y_ * sin_u; + + relative_velocity_rtn_m_s *= (semi_major_axis_ref_m_ * n); + return relative_velocity_rtn_m_s; +} + +double QuasiNonsingularRelativeOrbitalElements::CalcDiffMeanArgLat_rad(const QuasiNonsingularOrbitalElements qns_oe_reference, + const QuasiNonsingularOrbitalElements qns_oe_target) { + // Reference info + const double q1 = qns_oe_reference.GetEccentricityX(); + const double q2 = qns_oe_reference.GetEccentricityY(); + const double e2 = q1 * q1 + q2 * q2; + const double e = sqrt(e2); + const double eta2 = 1.0 - e2; + const double eta = sqrt(eta2); + + const double cos_theta = cos(qns_oe_reference.GetTrueLatAng_rad()); + const double sin_theta = sin(qns_oe_reference.GetTrueLatAng_rad()); + const double e_cos_f = q1 * cos_theta + q2 * sin_theta; + const double denominator = (1.0 + e_cos_f) * (1.0 + e_cos_f); + + const double arg_peri_ref_rad = atan2(qns_oe_reference.GetEccentricityY(), qns_oe_reference.GetEccentricityX()); + const double true_anomaly_ref_rad = qns_oe_reference.GetTrueLatAng_rad() - arg_peri_ref_rad; + const double sin_f = sin(true_anomaly_ref_rad); + + // Difference Info + const double arg_peri_target_rad = atan2(qns_oe_target.GetEccentricityY(), qns_oe_target.GetEccentricityX()); + const double true_anomaly_target_rad = qns_oe_target.GetTrueLatAng_rad() - arg_peri_target_rad; + const double d_true_anomaly_rad = true_anomaly_target_rad - true_anomaly_ref_rad; + + const double q1_target = qns_oe_reference.GetEccentricityX(); + const double q2_target = qns_oe_reference.GetEccentricityY(); + const double e_target = sqrt(q1_target * q1_target + q2_target * q2_target); + const double d_e = e_target - e; + + const double d_mean_arg_lat_rad = (eta / denominator) * (eta2 * d_true_anomaly_rad - sin_f * (2.0 + e_cos_f) * d_e); + return (arg_peri_target_rad - arg_peri_ref_rad) + d_mean_arg_lat_rad; +} diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp new file mode 100644 index 00000000..627c1117 --- /dev/null +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp @@ -0,0 +1,116 @@ +/** + * @file QuasiNonsingularRelativeOrbitalElements.hpp + * @brief Relative orbital elements defined by eccentricity/inclination vectors to avoid singularity when the eccentricity is near zero. + */ + +#ifndef QUASI_NONSINGULAR_RELATIVE_ORBITAL_ELEMENTS_H_ +#define QUASI_NONSINGULAR_RELATIVE_ORBITAL_ELEMENTS_H_ + +#include + +#include "../Orbit/QuasiNonsingularOrbitalElements.hpp" + +/** + * @class QuasiNonsingularRelativeOrbitalElements + * @brief Relative orbital elements defined by eccentricity/inclination vectors to avoid singularity when the eccentricity is near zero. + * @note Relative orbital elements(ROEs) is defined as a set of six unique linear or nonlinear combinations of two orbital elements + */ +class QuasiNonsingularRelativeOrbitalElements { + public: + /** + * @fn QuasiNonsingularRelativeOrbitalElements + * @brief Constructor initialized with tow quasi-nonsingular orbital elements + * @param [in] qns_oe_reference: Quasi-nonsingular orbital elements of the reference spacecraft + * @param [in] qns_oe_target: Quasi-nonsingular orbital elements of the target spacecraft + */ + QuasiNonsingularRelativeOrbitalElements(const QuasiNonsingularOrbitalElements qns_oe_reference, + const QuasiNonsingularOrbitalElements qns_oe_target); + /** + * @fn QuasiNonsingularRelativeOrbitalElements + * @brief Constructor initialized with relative position and velocity + * @param [in] semi_major_axis_ref_m: Semi-major axis of the reference satellite orbit [m] + * @param [in] relative_position_rtn_m: Relative position of target satellite in the reference satellite's RTN frame [m] + * @param [in] relative_velocity_rtn_m_s: Relative velocity of target satellite in the reference satellite's RTN frame [m/s] + * @param [in] mu_m3_s2: Gravity constant of the center body [m3/s2] + */ + QuasiNonsingularRelativeOrbitalElements(const double semi_major_axis_ref_m, const libra::Vector<3> relative_position_rtn_m, + const libra::Vector<3> relative_velocity_rtn_m_s, const double mu_m3_s2); + /** + * @fn ~QuasiNonsingularRelativeOrbitalElements + * @brief Destructor + */ + ~QuasiNonsingularRelativeOrbitalElements(); + + // Calculation + /** + * @fn CalcRelativePositionCircularApprox_rtn_m + * @brief Calculate the relative position of target spacecraft with circular approximation + * @param [in] mean_arg_lat_rad: Mean argument of latitude [rad] + * @return Relative position vector in RTN frame of reference spacecraft [m] + */ + libra::Vector<3> CalcRelativePositionCircularApprox_rtn_m(const double mean_arg_lat_rad); + /** + * @fn CalcRelativeVelocityCircularApprox_rtn_m_s + * @brief Calculate the relative position of target spacecraft with circular approximation + * @param [in] mean_arg_lat_rad: Mean argument of latitude [rad] + * @param [in] mu_m3_s2: Gravity constant of the center body [m3/s2] + * @return Relative velocity vector in RTN frame of reference spacecraft [m/s] + */ + libra::Vector<3> CalcRelativeVelocityCircularApprox_rtn_m_s(const double mean_arg_lat_rad, const double mu_m3_s2); + + // Getter + /** + * @fn GetDeltaSemiMajor + * @brief Return Relative semi major axis [-] + */ + inline double GetDeltaSemiMajor() const { return d_semi_major_axis_; } + /** + * @fn GetDeltaSemiMajor + * @brief Return Relative mean longitude [-] + */ + inline double GetDeltaMeanLongitude() const { return d_mean_longitude_; } + /** + * @fn GetDeltaSemiMajor + * @brief Return Relative eccentricity vector X component [-] + */ + inline double GetDeltaEccentricityX() const { return d_eccentricity_x_; } + /** + * @fn GetDeltaSemiMajor + * @brief Return Relative eccentricity vector Y component [-] + */ + inline double GetDeltaEccentricityY() const { return d_eccentricity_y_; } + /** + * @fn GetDeltaSemiMajor + * @brief Return Relative inclination vector X component [-] + */ + inline double GetDeltaInclinationX() const { return d_inclination_x_; } + /** + * @fn GetDeltaSemiMajor + * @brief Return Relative inclination vector Y component [-] + */ + inline double GetDeltaInclinationY() const { return d_inclination_y_; } + + private: + // Reference orbit information + double semi_major_axis_ref_m_; //!< Semi major axis of reference orbit [m] + + // Relative Orbital Elements + double d_semi_major_axis_; //!< Relative semi major axis [-] + double d_mean_longitude_; //!< Relative mean longitude [-] + double d_eccentricity_x_; //!< Relative eccentricity vector X component [-] + double d_eccentricity_y_; //!< Relative eccentricity vector Y component [-] + double d_inclination_x_; //!< Relative inclination vector X component [-] + double d_inclination_y_; //!< Relative inclination vector Y component [-] + + /** + * @fn CalcDiffMeanArgLat_rad + * @brief Calculate difference of mean argument of latitude [rad] + * @note Mean argument of latitude = argument of periapsis + mean anomaly + * @param [in] qns_oe_reference: Quasi-nonsingular orbital elements of the reference spacecraft + * @param [in] qns_oe_target: Quasi-nonsingular orbital elements of the target spacecraft + * @return Difference of mean argument of latitude [rad] + */ + double CalcDiffMeanArgLat_rad(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target); +}; + +#endif diff --git a/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp new file mode 100644 index 00000000..d0feb5de --- /dev/null +++ b/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp @@ -0,0 +1,68 @@ +#include + +#include "../src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp" + +TEST(QuasiNonsingularOrbitalElementDifferences, ConstructorWithOe) { + // reference + const double reference_semi_major_axis_m = 6896e3; + const double reference_eccentricity_x = 0.0; // Test singular point + const double reference_eccentricity_y = 0.0; // Test singular point + const double reference_inclination_rad = 1.7; + const double reference_raan_rad = 5.93; + const double reference_true_latitude_angle_rad = 0.5; + QuasiNonsingularOrbitalElements reference_qn_oe(reference_semi_major_axis_m, reference_eccentricity_x, reference_eccentricity_y, + reference_inclination_rad, reference_raan_rad, reference_true_latitude_angle_rad); + // target + const double target_semi_major_axis_m = 6896e3; + const double target_eccentricity_x = 0.05; + const double target_eccentricity_y = 0.03; + const double target_inclination_rad = 1.7; + const double target_raan_rad = 5.93; + const double target_true_latitude_angle_rad = 0.5; + QuasiNonsingularOrbitalElements target_qn_oe(target_semi_major_axis_m, target_eccentricity_x, target_eccentricity_y, target_inclination_rad, + target_raan_rad, target_true_latitude_angle_rad); + + QuasiNonsingularOrbitalElementDifferences qn_oe(reference_qn_oe, target_qn_oe); + // OEs + EXPECT_NEAR(target_semi_major_axis_m - reference_semi_major_axis_m, qn_oe.GetDiffQuasiNonSingularOrbitalElements().GetSemiMajor_m(), 1); + EXPECT_NEAR(target_eccentricity_x - reference_eccentricity_x, qn_oe.GetDiffQuasiNonSingularOrbitalElements().GetEccentricityX(), 1e-6); + EXPECT_NEAR(target_eccentricity_y - reference_eccentricity_y, qn_oe.GetDiffQuasiNonSingularOrbitalElements().GetEccentricityY(), 1e-6); + EXPECT_NEAR(target_inclination_rad - reference_inclination_rad, qn_oe.GetDiffQuasiNonSingularOrbitalElements().GetInclination_rad(), 1e-3); + EXPECT_NEAR(target_raan_rad - reference_raan_rad, qn_oe.GetDiffQuasiNonSingularOrbitalElements().GetRaan_rad(), 1e-3); + EXPECT_NEAR(target_true_latitude_angle_rad - reference_true_latitude_angle_rad, qn_oe.GetDiffQuasiNonSingularOrbitalElements().GetTrueLatAng_rad(), + 1e-3); +} + +TEST(QuasiNonsingularOrbitalElementDifferences, ConstructorWithPositionVelocity) { + const double mu_m3_s2 = 3.986008e14; + // reference satellite + const double reference_semi_major_axis_m = 6896e3; + const double reference_eccentricity_x = 0.002; + const double reference_eccentricity_y = 0.004; + const double reference_inclination_rad = 1.7; + const double reference_raan_rad = 5.93; + const double reference_true_latitude_angle_rad = 0.5; + QuasiNonsingularOrbitalElements reference_qn_oe(reference_semi_major_axis_m, reference_eccentricity_x, reference_eccentricity_y, + reference_inclination_rad, reference_raan_rad, reference_true_latitude_angle_rad); + // target relative position and velocity + libra::Vector<3> position_rtn_m; + position_rtn_m[0] = -0.000213852; + position_rtn_m[1] = -11.4122; + position_rtn_m[2] = 4.65733; + libra::Vector<3> velocity_rtn_m_s; + velocity_rtn_m_s[0] = 1.1448E-06; + velocity_rtn_m_s[1] = 0.0; + velocity_rtn_m_s[2] = 0.005298; + + QuasiNonsingularOrbitalElementDifferences qn_oe(reference_qn_oe, position_rtn_m, velocity_rtn_m_s, mu_m3_s2); + libra::Vector<3> calc_position_rtn_m = qn_oe.CalcRelativePosition_rtn_m(); + libra::Vector<3> calc_velocity_rtn_m_s = qn_oe.CalcRelativeVelocity_rtn_m_s(mu_m3_s2); + + // Compare position and velocity + EXPECT_NEAR(calc_position_rtn_m[0], position_rtn_m[0], 1e-5); + EXPECT_NEAR(calc_position_rtn_m[1], position_rtn_m[1], 1e-5); + EXPECT_NEAR(calc_position_rtn_m[2], position_rtn_m[2], 1e-5); + EXPECT_NEAR(calc_velocity_rtn_m_s[0], velocity_rtn_m_s[0], 1e-5); + EXPECT_NEAR(calc_velocity_rtn_m_s[1], velocity_rtn_m_s[1], 1e-5); + EXPECT_NEAR(calc_velocity_rtn_m_s[2], velocity_rtn_m_s[2], 1e-5); +} diff --git a/s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp b/s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp new file mode 100644 index 00000000..d1732b3e --- /dev/null +++ b/s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp @@ -0,0 +1,115 @@ +#include + +#include "../src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp" + +TEST(QuasiNonsingularOrbitalElements, DefaultConstructor) { + QuasiNonsingularOrbitalElements qn_oe; + + // OEs + EXPECT_DOUBLE_EQ(0.0, qn_oe.GetSemiMajor_m()); + EXPECT_DOUBLE_EQ(0.0, qn_oe.GetEccentricityX()); + EXPECT_DOUBLE_EQ(0.0, qn_oe.GetEccentricityY()); + EXPECT_DOUBLE_EQ(0.0, qn_oe.GetInclination_rad()); + EXPECT_DOUBLE_EQ(0.0, qn_oe.GetRaan_rad()); + EXPECT_DOUBLE_EQ(0.0, qn_oe.GetTrueLatAng_rad()); + // Parameters + EXPECT_DOUBLE_EQ(0.0, qn_oe.GetSemiLatusRectum_m()); + EXPECT_DOUBLE_EQ(0.0, qn_oe.GetRadius_m()); +} + +TEST(QuasiNonsingularOrbitalElements, ConstructorWithSingularOe) { + const double semi_major_axis_m = 6896e3; + const double eccentricity_x = 0.0; // Test singular point + const double eccentricity_y = 0.0; // Test singular point + const double inclination_rad = 1.7; + const double raan_rad = 5.93; + const double true_latitude_angle_rad = 0.5; + QuasiNonsingularOrbitalElements qn_oe(semi_major_axis_m, eccentricity_x, eccentricity_y, inclination_rad, raan_rad, true_latitude_angle_rad); + + // OEs + EXPECT_DOUBLE_EQ(semi_major_axis_m, qn_oe.GetSemiMajor_m()); + EXPECT_DOUBLE_EQ(eccentricity_x, qn_oe.GetEccentricityX()); + EXPECT_DOUBLE_EQ(eccentricity_y, qn_oe.GetEccentricityY()); + EXPECT_DOUBLE_EQ(inclination_rad, qn_oe.GetInclination_rad()); + EXPECT_DOUBLE_EQ(raan_rad, qn_oe.GetRaan_rad()); + EXPECT_DOUBLE_EQ(true_latitude_angle_rad, qn_oe.GetTrueLatAng_rad()); + // Parameters + EXPECT_DOUBLE_EQ(semi_major_axis_m, qn_oe.GetSemiLatusRectum_m()); + EXPECT_DOUBLE_EQ(semi_major_axis_m, qn_oe.GetRadius_m()); +} + +TEST(QuasiNonsingularOrbitalElements, ConstructorWithOe) { + const double semi_major_axis_m = 6896e3; + const double eccentricity_x = 0.05; + const double eccentricity_y = 0.03; + const double inclination_rad = 1.7; + const double raan_rad = 5.93; + const double true_latitude_angle_rad = 0.5; + QuasiNonsingularOrbitalElements qn_oe(semi_major_axis_m, eccentricity_x, eccentricity_y, inclination_rad, raan_rad, true_latitude_angle_rad); + + // OEs + EXPECT_DOUBLE_EQ(semi_major_axis_m, qn_oe.GetSemiMajor_m()); + EXPECT_DOUBLE_EQ(eccentricity_x, qn_oe.GetEccentricityX()); + EXPECT_DOUBLE_EQ(eccentricity_y, qn_oe.GetEccentricityY()); + EXPECT_DOUBLE_EQ(inclination_rad, qn_oe.GetInclination_rad()); + EXPECT_DOUBLE_EQ(raan_rad, qn_oe.GetRaan_rad()); + EXPECT_DOUBLE_EQ(true_latitude_angle_rad, qn_oe.GetTrueLatAng_rad()); + // Parameters + EXPECT_NEAR(6872553.6, qn_oe.GetSemiLatusRectum_m(), 1e-1); + EXPECT_NEAR(6494189.8, qn_oe.GetRadius_m(), 1e-1); +} + +TEST(QuasiNonsingularOrbitalElements, ConstructorWithPositionVelocity) { + const double mu_m3_s2 = 3.986008e14; + libra::Vector<3> position_i_m; + position_i_m[0] = -5659121.225; + position_i_m[1] = 2467374.064; + position_i_m[2] = -3072838.471; + const double r_norm_m = norm(position_i_m); + libra::Vector<3> velocity_i_m_s; + velocity_i_m_s[0] = 3517.005128; + velocity_i_m_s[1] = -323.2731889; + velocity_i_m_s[2] = -6734.568005; + QuasiNonsingularOrbitalElements qn_oe(mu_m3_s2, position_i_m, velocity_i_m_s); + + // OEs + EXPECT_NEAR(6.8993234545e6, qn_oe.GetSemiMajor_m(), 1); + EXPECT_NEAR(-3.6369e-4, qn_oe.GetEccentricityX(), 1e-6); + EXPECT_NEAR(-3.2297e-4, qn_oe.GetEccentricityY(), 1e-6); + EXPECT_NEAR(1.7017, qn_oe.GetInclination_rad(), 1e-3); + EXPECT_NEAR(5.9376, qn_oe.GetRaan_rad(), 1e-3); + EXPECT_NEAR(3.6077, qn_oe.GetTrueLatAng_rad(), 1e-3); + // Parameters + EXPECT_NEAR(6.8993218223e6, qn_oe.GetSemiLatusRectum_m(), 1e-1); + EXPECT_NEAR(r_norm_m, qn_oe.GetRadius_m(), 1e-1); +} + +TEST(QuasiNonsingularOrbitalElements, Subtract) { + // lhs + const double lhs_semi_major_axis_m = 6896e3; + const double lhs_eccentricity_x = 0.0; // Test singular point + const double lhs_eccentricity_y = 0.0; // Test singular point + const double lhs_inclination_rad = 1.7; + const double lhs_raan_rad = 5.93; + const double lhs_true_latitude_angle_rad = 0.5; + QuasiNonsingularOrbitalElements lhs_qn_oe(lhs_semi_major_axis_m, lhs_eccentricity_x, lhs_eccentricity_y, lhs_inclination_rad, lhs_raan_rad, + lhs_true_latitude_angle_rad); + // rhs + const double rhs_semi_major_axis_m = 6896e3; + const double rhs_eccentricity_x = 0.05; + const double rhs_eccentricity_y = 0.03; + const double rhs_inclination_rad = 1.7; + const double rhs_raan_rad = 5.93; + const double rhs_true_latitude_angle_rad = 0.5; + QuasiNonsingularOrbitalElements rhs_qn_oe(rhs_semi_major_axis_m, rhs_eccentricity_x, rhs_eccentricity_y, rhs_inclination_rad, rhs_raan_rad, + rhs_true_latitude_angle_rad); + + QuasiNonsingularOrbitalElements qn_oe = lhs_qn_oe - rhs_qn_oe; + // OEs + EXPECT_NEAR(lhs_semi_major_axis_m - rhs_semi_major_axis_m, qn_oe.GetSemiMajor_m(), 1); + EXPECT_NEAR(lhs_eccentricity_x - rhs_eccentricity_x, qn_oe.GetEccentricityX(), 1e-6); + EXPECT_NEAR(lhs_eccentricity_y - rhs_eccentricity_y, qn_oe.GetEccentricityY(), 1e-6); + EXPECT_NEAR(lhs_inclination_rad - rhs_inclination_rad, qn_oe.GetInclination_rad(), 1e-3); + EXPECT_NEAR(lhs_raan_rad - rhs_raan_rad, qn_oe.GetRaan_rad(), 1e-3); + EXPECT_NEAR(lhs_true_latitude_angle_rad - rhs_true_latitude_angle_rad, qn_oe.GetTrueLatAng_rad(), 1e-3); +} diff --git a/s2e-ff/test/TestQuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/test/TestQuasiNonsingularRelativeOrbitalElements.cpp new file mode 100644 index 00000000..af597407 --- /dev/null +++ b/s2e-ff/test/TestQuasiNonsingularRelativeOrbitalElements.cpp @@ -0,0 +1,66 @@ +#include + +#include "../src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp" + +TEST(QuasiNonsingularRelativeOrbitalElements, ConstructorWithOe) { + // reference + const double reference_semi_major_axis_m = 6896e3; + const double reference_eccentricity_x = 0.0; // Test singular point + const double reference_eccentricity_y = 0.0; // Test singular point + const double reference_inclination_rad = 1.7; + const double reference_raan_rad = 5.93; + const double reference_true_latitude_angle_rad = 0.5; + QuasiNonsingularOrbitalElements reference_qn_oe(reference_semi_major_axis_m, reference_eccentricity_x, reference_eccentricity_y, + reference_inclination_rad, reference_raan_rad, reference_true_latitude_angle_rad); + // target + const double target_semi_major_axis_m = 6896e3; + const double target_eccentricity_x = 0.05; + const double target_eccentricity_y = 0.03; + const double target_inclination_rad = 1.7; + const double target_raan_rad = 5.93; + const double target_true_latitude_angle_rad = 0.5; + QuasiNonsingularOrbitalElements target_qn_oe(target_semi_major_axis_m, target_eccentricity_x, target_eccentricity_y, target_inclination_rad, + target_raan_rad, target_true_latitude_angle_rad); + + QuasiNonsingularRelativeOrbitalElements qn_roe(reference_qn_oe, target_qn_oe); + + EXPECT_NEAR((target_semi_major_axis_m - reference_semi_major_axis_m) / reference_semi_major_axis_m, qn_roe.GetDeltaSemiMajor(), 1); + // When reference is circular orbit + EXPECT_NEAR(target_true_latitude_angle_rad - reference_true_latitude_angle_rad, qn_roe.GetDeltaMeanLongitude(), 1e-6); + + EXPECT_NEAR(target_eccentricity_x - reference_eccentricity_x, qn_roe.GetDeltaEccentricityX(), 1e-6); + EXPECT_NEAR(target_eccentricity_y - reference_eccentricity_y, qn_roe.GetDeltaEccentricityY(), 1e-6); + + EXPECT_NEAR(target_inclination_rad - reference_inclination_rad, qn_roe.GetDeltaInclinationX(), 1e-3); + EXPECT_NEAR((target_raan_rad - reference_raan_rad) * sin(reference_inclination_rad), qn_roe.GetDeltaInclinationY(), 1e-3); +} + +TEST(QuasiNonsingularRelativeOrbitalElements, ConstructorWithPositionVelocity) { + const double mu_m3_s2 = 3.986008e14; + // reference satellite + const double reference_semi_major_axis_m = 6896e3; + + // target relative position and velocity + libra::Vector<3> position_rtn_m; + position_rtn_m[0] = -0.000213852; + position_rtn_m[1] = -11.4122; + position_rtn_m[2] = 4.65733; + libra::Vector<3> velocity_rtn_m_s; + velocity_rtn_m_s[0] = 1.1448E-06; + velocity_rtn_m_s[1] = 0.0; + velocity_rtn_m_s[2] = 0.005298; + + QuasiNonsingularRelativeOrbitalElements qn_roe(reference_semi_major_axis_m, position_rtn_m, velocity_rtn_m_s, mu_m3_s2); + + double u = 0; + libra::Vector<3> calc_position_rtn_m = qn_roe.CalcRelativePositionCircularApprox_rtn_m(u); + libra::Vector<3> calc_velocity_rtn_m_s = qn_roe.CalcRelativeVelocityCircularApprox_rtn_m_s(u, mu_m3_s2); + + // Compare position and velocity + EXPECT_NEAR(calc_position_rtn_m[0], position_rtn_m[0], 1e-5); + EXPECT_NEAR(calc_position_rtn_m[1], position_rtn_m[1], 1e-5); + EXPECT_NEAR(calc_position_rtn_m[2], position_rtn_m[2], 1e-5); + EXPECT_NEAR(calc_velocity_rtn_m_s[0], velocity_rtn_m_s[0], 1e-5); + EXPECT_NEAR(calc_velocity_rtn_m_s[1], velocity_rtn_m_s[1], 1e-5); + EXPECT_NEAR(calc_velocity_rtn_m_s[2], velocity_rtn_m_s[2], 1e-5); +}