From c0eb3794756610a2899160b63e604bfbb9aabaf2 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 13 Sep 2022 15:29:09 +0200 Subject: [PATCH 01/54] Add relative orbital elements --- s2e-ff/CMakeLists.txt | 2 ++ .../Orbit/NonsingularOrbitalElements.cpp | 12 +++++++ .../Orbit/NonsingularOrbitalElements.hpp | 28 +++++++++++++++++ .../RelativeOrbit/RelativeOrbitalElements.cpp | 31 +++++++++++++++++++ .../RelativeOrbit/RelativeOrbitalElements.hpp | 27 ++++++++++++++++ 5 files changed, 100 insertions(+) create mode 100644 s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.cpp create mode 100644 s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.hpp create mode 100644 s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.cpp create mode 100644 s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.hpp diff --git a/s2e-ff/CMakeLists.txt b/s2e-ff/CMakeLists.txt index 12d83540..8a901231 100644 --- a/s2e-ff/CMakeLists.txt +++ b/s2e-ff/CMakeLists.txt @@ -95,6 +95,8 @@ set(SOURCE_FILES src/Library/math/DualQuaternion.cpp src/Library/math/RotationFirstDualQuaternion.cpp src/Library/math/TranslationFirstDualQuaternion.cpp + src/Library/Orbit/NonsingularOrbitalElements.cpp + src/Library/RelativeOrbit/RelativeOrbitalElements.cpp src/Simulation/Case/FfCase.cpp src/Simulation/Spacecraft/FfSat.cpp src/Simulation/Spacecraft/FfComponents.cpp diff --git a/s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.cpp b/s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.cpp new file mode 100644 index 00000000..513faf20 --- /dev/null +++ b/s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.cpp @@ -0,0 +1,12 @@ +#include "NonsingularOrbitalElements.hpp" + +NonsingularOrbitalElements::NonsingularOrbitalElements(const OrbitalElements oe) { + double mean_anomaly_rad = 0.0; // since the epoch is the perigee pass time + + semi_major_axis_m_ = oe.GetSemiMajor(); + arg_longitude_epoch_rad_ = oe.GetArgPerigee() + mean_anomaly_rad; + eccentricity_x_ = oe.GetEccentricity() * cos(oe.GetArgPerigee()); + eccentricity_y_ = oe.GetEccentricity() * sin(oe.GetArgPerigee()); + inclination_rad_ = oe.GetInclination(); + raan_rad_ = oe.GetRaan(); +} diff --git a/s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.hpp b/s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.hpp new file mode 100644 index 00000000..d1a0558a --- /dev/null +++ b/s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.hpp @@ -0,0 +1,28 @@ +#ifndef NONSINGULAR_ORBITAL_ELEMENTS_H_ +#define NONSINGULAR_ORBITAL_ELEMENTS_H_ + +#include + +class NonsingularOrbitalElements { + public: + NonsingularOrbitalElements(const OrbitalElements oe); + ~NonsingularOrbitalElements(); + + // Getter + inline double GetSemiMajor() const { return semi_major_axis_m_; } + inline double GetEccentricityX() const { return eccentricity_x_; } + inline double GetEccentricityY() const { return eccentricity_y_; } + inline double GetInclination() const { return inclination_rad_; } + inline double GetRaan() const { return raan_rad_; } + inline double GetArgLonEpoch() const { return arg_longitude_epoch_rad_; } + + private: + double semi_major_axis_m_; + double eccentricity_x_; + double eccentricity_y_; + double inclination_rad_; + double raan_rad_; //!< Right Ascension of the Ascending Node [rad] + double arg_longitude_epoch_rad_; //!< Argument of Latitude at epoch (perigees) [rad] +}; + +#endif diff --git a/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.cpp new file mode 100644 index 00000000..7eebafb4 --- /dev/null +++ b/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.cpp @@ -0,0 +1,31 @@ +#include "RelativeOrbitalElements.hpp" + +#include + +RelativeOrbitalElements::RelativeOrbitalElements(const NonsingularOrbitalElements noe_reference, const NonsingularOrbitalElements noe_target) { + double diff_raan = noe_target.GetRaan() - noe_reference.GetRaan(); + + a_ref_m_ = noe_reference.GetSemiMajor(); + delta_a_ = (noe_target.GetSemiMajor() - a_ref_m_) / a_ref_m_; + delta_lambda_ = (noe_target.GetArgLonEpoch() - noe_reference.GetArgLonEpoch()) + diff_raan * cos(noe_reference.GetInclination()); + + delta_e_x_ = noe_target.GetEccentricityX() - noe_reference.GetEccentricityX(); + delta_e_y_ = noe_target.GetEccentricityY() - noe_reference.GetEccentricityY(); + delta_i_x_ = noe_target.GetInclination() - noe_reference.GetInclination(); + delta_i_y_ = diff_raan * sin(noe_reference.GetInclination()); +} + +RelativeOrbitalElements::~RelativeOrbitalElements() {} + +libra::Vector<3> RelativeOrbitalElements::CalcRelativePositionRtn_m(const double arg_lat_rad) { + libra::Vector<3> relative_position_rtn_m; + double cos_u = cos(arg_lat_rad); + double sin_u = sin(arg_lat_rad); + + relative_position_rtn_m[0] = delta_a_ - (delta_e_x_ * cos_u + delta_e_y_ * sin_u); + relative_position_rtn_m[1] = -1.5 * delta_a_ * arg_lat_rad + delta_lambda_ + 2.0 * (delta_e_x_ * sin_u - delta_e_y_ * cos_u); + relative_position_rtn_m[2] = delta_i_x_ * sin_u - delta_i_y_ * cos_u; + + relative_position_rtn_m *= a_ref_m_; + return relative_position_rtn_m; +} diff --git a/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.hpp b/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.hpp new file mode 100644 index 00000000..21b33d57 --- /dev/null +++ b/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.hpp @@ -0,0 +1,27 @@ +#ifndef RELATIVE_ORBITAL_ELEMENTS_H_ +#define RELATIVE_ORBITAL_ELEMENTS_H_ + +#include + +#include "../Orbit/NonsingularOrbitalElements.hpp" + +class RelativeOrbitalElements { + public: + RelativeOrbitalElements(const NonsingularOrbitalElements noe_chief, const NonsingularOrbitalElements noe_deputy); + ~RelativeOrbitalElements(); + libra::Vector<3> CalcRelativePositionRtn_m(const double arg_lat_rad); //!< Calculate relative position from ROE + + private: + // Reference orbit information + double a_ref_m_; //!< Semi major axis of reference orbit [m] + + // Relative Orbital Elements + double delta_a_; //!< Relative semi major axis [-] + double delta_lambda_; //!< Relative mean longitude [-] + double delta_e_x_; //!< Relative eccentricity vector X component [-] + double delta_e_y_; //!< Relative eccentricity vector Y component [-] + double delta_i_x_; //!< Relative inclination vector X component [-] + double delta_i_y_; //!< Relative inclination vector Y component [-] +}; + +#endif From a27aded60a5d743186575f0168960f2f41a2b0d3 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sun, 2 Oct 2022 16:11:11 +0200 Subject: [PATCH 02/54] fix variable name --- s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.cpp | 2 +- s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.hpp | 6 +++--- .../src/Library/RelativeOrbit/RelativeOrbitalElements.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.cpp b/s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.cpp index 513faf20..e804814d 100644 --- a/s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.cpp +++ b/s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.cpp @@ -4,7 +4,7 @@ NonsingularOrbitalElements::NonsingularOrbitalElements(const OrbitalElements oe) double mean_anomaly_rad = 0.0; // since the epoch is the perigee pass time semi_major_axis_m_ = oe.GetSemiMajor(); - arg_longitude_epoch_rad_ = oe.GetArgPerigee() + mean_anomaly_rad; + mean_arg_latitude_epoch_rad_ = oe.GetArgPerigee() + mean_anomaly_rad; eccentricity_x_ = oe.GetEccentricity() * cos(oe.GetArgPerigee()); eccentricity_y_ = oe.GetEccentricity() * sin(oe.GetArgPerigee()); inclination_rad_ = oe.GetInclination(); diff --git a/s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.hpp b/s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.hpp index d1a0558a..57c863e0 100644 --- a/s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.hpp +++ b/s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.hpp @@ -14,15 +14,15 @@ class NonsingularOrbitalElements { inline double GetEccentricityY() const { return eccentricity_y_; } inline double GetInclination() const { return inclination_rad_; } inline double GetRaan() const { return raan_rad_; } - inline double GetArgLonEpoch() const { return arg_longitude_epoch_rad_; } + inline double GetMeanArgLatEpoch() const { return mean_arg_latitude_epoch_rad_; } private: double semi_major_axis_m_; double eccentricity_x_; double eccentricity_y_; double inclination_rad_; - double raan_rad_; //!< Right Ascension of the Ascending Node [rad] - double arg_longitude_epoch_rad_; //!< Argument of Latitude at epoch (perigees) [rad] + double raan_rad_; //!< Right Ascension of the Ascending Node [rad] + double mean_arg_latitude_epoch_rad_; //!< Mean argument of Latitude at epoch [rad] }; #endif diff --git a/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.cpp index 7eebafb4..8936f3a9 100644 --- a/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.cpp @@ -7,7 +7,7 @@ RelativeOrbitalElements::RelativeOrbitalElements(const NonsingularOrbitalElement a_ref_m_ = noe_reference.GetSemiMajor(); delta_a_ = (noe_target.GetSemiMajor() - a_ref_m_) / a_ref_m_; - delta_lambda_ = (noe_target.GetArgLonEpoch() - noe_reference.GetArgLonEpoch()) + diff_raan * cos(noe_reference.GetInclination()); + delta_lambda_ = (noe_target.GetMeanArgLatEpoch() - noe_reference.GetMeanArgLatEpoch()) + diff_raan * cos(noe_reference.GetInclination()); delta_e_x_ = noe_target.GetEccentricityX() - noe_reference.GetEccentricityX(); delta_e_y_ = noe_target.GetEccentricityY() - noe_reference.GetEccentricityY(); From 0559c10ac9471410002f253b360e4c42db513d6f Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sun, 2 Oct 2022 16:13:54 +0200 Subject: [PATCH 03/54] Rename file --- ...larOrbitalElements.cpp => QuasiNonsingularOrbitalElements.cpp} | 0 ...larOrbitalElements.hpp => QuasiNonsingularOrbitalElements.hpp} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename s2e-ff/src/Library/Orbit/{NonsingularOrbitalElements.cpp => QuasiNonsingularOrbitalElements.cpp} (100%) rename s2e-ff/src/Library/Orbit/{NonsingularOrbitalElements.hpp => QuasiNonsingularOrbitalElements.hpp} (100%) diff --git a/s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.cpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp similarity index 100% rename from s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.cpp rename to s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp diff --git a/s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.hpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp similarity index 100% rename from s2e-ff/src/Library/Orbit/NonsingularOrbitalElements.hpp rename to s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp From 5b7b575d5ce13a73da9c0b5488d1e3575b944b1b Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sun, 2 Oct 2022 16:30:24 +0200 Subject: [PATCH 04/54] Rename nonsingular orbital elements class --- s2e-ff/CMakeLists.txt | 2 +- .../Orbit/QuasiNonsingularOrbitalElements.cpp | 4 ++-- .../Orbit/QuasiNonsingularOrbitalElements.hpp | 10 +++++----- .../RelativeOrbit/RelativeOrbitalElements.cpp | 18 +++++++++--------- .../RelativeOrbit/RelativeOrbitalElements.hpp | 4 ++-- 5 files changed, 19 insertions(+), 19 deletions(-) diff --git a/s2e-ff/CMakeLists.txt b/s2e-ff/CMakeLists.txt index 8a901231..2bea48a1 100644 --- a/s2e-ff/CMakeLists.txt +++ b/s2e-ff/CMakeLists.txt @@ -95,7 +95,7 @@ set(SOURCE_FILES src/Library/math/DualQuaternion.cpp src/Library/math/RotationFirstDualQuaternion.cpp src/Library/math/TranslationFirstDualQuaternion.cpp - src/Library/Orbit/NonsingularOrbitalElements.cpp + src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp src/Library/RelativeOrbit/RelativeOrbitalElements.cpp src/Simulation/Case/FfCase.cpp src/Simulation/Spacecraft/FfSat.cpp diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp index e804814d..42543051 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp @@ -1,6 +1,6 @@ -#include "NonsingularOrbitalElements.hpp" +#include "QuasiNonsingularOrbitalElements.hpp" -NonsingularOrbitalElements::NonsingularOrbitalElements(const OrbitalElements oe) { +QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const OrbitalElements oe) { double mean_anomaly_rad = 0.0; // since the epoch is the perigee pass time semi_major_axis_m_ = oe.GetSemiMajor(); diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp index 57c863e0..8cd24520 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp @@ -1,12 +1,12 @@ -#ifndef NONSINGULAR_ORBITAL_ELEMENTS_H_ -#define NONSINGULAR_ORBITAL_ELEMENTS_H_ +#ifndef QUASI_NONSINGULAR_ORBITAL_ELEMENTS_H_ +#define QUASI_NONSINGULAR_ORBITAL_ELEMENTS_H_ #include -class NonsingularOrbitalElements { +class QuasiNonsingularOrbitalElements { public: - NonsingularOrbitalElements(const OrbitalElements oe); - ~NonsingularOrbitalElements(); + QuasiNonsingularOrbitalElements(const OrbitalElements oe); + ~QuasiNonsingularOrbitalElements(); // Getter inline double GetSemiMajor() const { return semi_major_axis_m_; } diff --git a/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.cpp index 8936f3a9..1ec7f290 100644 --- a/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.cpp @@ -2,17 +2,17 @@ #include -RelativeOrbitalElements::RelativeOrbitalElements(const NonsingularOrbitalElements noe_reference, const NonsingularOrbitalElements noe_target) { - double diff_raan = noe_target.GetRaan() - noe_reference.GetRaan(); +RelativeOrbitalElements::RelativeOrbitalElements(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target) { + double diff_raan = qns_oe_target.GetRaan() - qns_oe_reference.GetRaan(); - a_ref_m_ = noe_reference.GetSemiMajor(); - delta_a_ = (noe_target.GetSemiMajor() - a_ref_m_) / a_ref_m_; - delta_lambda_ = (noe_target.GetMeanArgLatEpoch() - noe_reference.GetMeanArgLatEpoch()) + diff_raan * cos(noe_reference.GetInclination()); + a_ref_m_ = qns_oe_reference.GetSemiMajor(); + delta_a_ = (qns_oe_target.GetSemiMajor() - a_ref_m_) / a_ref_m_; + delta_lambda_ = (qns_oe_target.GetMeanArgLatEpoch() - qns_oe_reference.GetMeanArgLatEpoch()) + diff_raan * cos(qns_oe_reference.GetInclination()); - delta_e_x_ = noe_target.GetEccentricityX() - noe_reference.GetEccentricityX(); - delta_e_y_ = noe_target.GetEccentricityY() - noe_reference.GetEccentricityY(); - delta_i_x_ = noe_target.GetInclination() - noe_reference.GetInclination(); - delta_i_y_ = diff_raan * sin(noe_reference.GetInclination()); + delta_e_x_ = qns_oe_target.GetEccentricityX() - qns_oe_reference.GetEccentricityX(); + delta_e_y_ = qns_oe_target.GetEccentricityY() - qns_oe_reference.GetEccentricityY(); + delta_i_x_ = qns_oe_target.GetInclination() - qns_oe_reference.GetInclination(); + delta_i_y_ = diff_raan * sin(qns_oe_reference.GetInclination()); } RelativeOrbitalElements::~RelativeOrbitalElements() {} diff --git a/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.hpp b/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.hpp index 21b33d57..1447f076 100644 --- a/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.hpp @@ -3,11 +3,11 @@ #include -#include "../Orbit/NonsingularOrbitalElements.hpp" +#include "../Orbit/QuasiNonsingularOrbitalElements.hpp" class RelativeOrbitalElements { public: - RelativeOrbitalElements(const NonsingularOrbitalElements noe_chief, const NonsingularOrbitalElements noe_deputy); + RelativeOrbitalElements(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target); ~RelativeOrbitalElements(); libra::Vector<3> CalcRelativePositionRtn_m(const double arg_lat_rad); //!< Calculate relative position from ROE From 75f074690e62edad82c9481261f56b76755d3dd5 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sun, 2 Oct 2022 16:37:19 +0200 Subject: [PATCH 05/54] Fix comments --- .../Library/Orbit/QuasiNonsingularOrbitalElements.hpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp index 8cd24520..a2b753bc 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp @@ -3,6 +3,10 @@ #include +/** + * @class QuasiNonsingularOrbitalElements + * @brief Orbital elements avoid singularity when the eccentricity is near zero. + */ class QuasiNonsingularOrbitalElements { public: QuasiNonsingularOrbitalElements(const OrbitalElements oe); @@ -18,11 +22,11 @@ class QuasiNonsingularOrbitalElements { private: double semi_major_axis_m_; - double eccentricity_x_; - double eccentricity_y_; + double eccentricity_x_; // e * cos(arg_peri) + double eccentricity_y_; // e * sin(arg_peri) double inclination_rad_; double raan_rad_; //!< Right Ascension of the Ascending Node [rad] - double mean_arg_latitude_epoch_rad_; //!< Mean argument of Latitude at epoch [rad] + double mean_arg_latitude_epoch_rad_; //!< Mean argument of Latitude at epoch (arg_peri + mean anomaly) [rad] }; #endif From 66969aff1297acc765d1ff8cce8a621e4d922c3e Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sun, 2 Oct 2022 16:48:00 +0200 Subject: [PATCH 06/54] Rename relative orbital elements --- s2e-ff/CMakeLists.txt | 2 +- ...Elements.cpp => QuasiNonsingularRelativeOrbitalElements.cpp} | 2 +- ...Elements.hpp => QuasiNonsingularRelativeOrbitalElements.hpp} | 0 3 files changed, 2 insertions(+), 2 deletions(-) rename s2e-ff/src/Library/RelativeOrbit/{RelativeOrbitalElements.cpp => QuasiNonsingularRelativeOrbitalElements.cpp} (96%) rename s2e-ff/src/Library/RelativeOrbit/{RelativeOrbitalElements.hpp => QuasiNonsingularRelativeOrbitalElements.hpp} (100%) diff --git a/s2e-ff/CMakeLists.txt b/s2e-ff/CMakeLists.txt index 2bea48a1..54e37011 100644 --- a/s2e-ff/CMakeLists.txt +++ b/s2e-ff/CMakeLists.txt @@ -96,7 +96,7 @@ set(SOURCE_FILES src/Library/math/RotationFirstDualQuaternion.cpp src/Library/math/TranslationFirstDualQuaternion.cpp src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp - src/Library/RelativeOrbit/RelativeOrbitalElements.cpp + src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp src/Simulation/Case/FfCase.cpp src/Simulation/Spacecraft/FfSat.cpp src/Simulation/Spacecraft/FfComponents.cpp diff --git a/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp similarity index 96% rename from s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.cpp rename to s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp index 1ec7f290..63f911b0 100644 --- a/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp @@ -1,4 +1,4 @@ -#include "RelativeOrbitalElements.hpp" +#include "QuasiNonsingularRelativeOrbitalElements.hpp" #include diff --git a/s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp similarity index 100% rename from s2e-ff/src/Library/RelativeOrbit/RelativeOrbitalElements.hpp rename to s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp From 4b91a380f544073824cc243b6018b2800da45c23 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sun, 2 Oct 2022 17:07:52 +0200 Subject: [PATCH 07/54] Add OED --- s2e-ff/CMakeLists.txt | 1 + ...siNonsingularOrbitalElementDifferences.cpp | 14 +++++++++ ...siNonsingularOrbitalElementDifferences.hpp | 29 +++++++++++++++++++ ...uasiNonsingularRelativeOrbitalElements.cpp | 6 ++-- ...uasiNonsingularRelativeOrbitalElements.hpp | 15 ++++++---- 5 files changed, 57 insertions(+), 8 deletions(-) create mode 100644 s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp create mode 100644 s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp diff --git a/s2e-ff/CMakeLists.txt b/s2e-ff/CMakeLists.txt index 54e37011..310ff6bd 100644 --- a/s2e-ff/CMakeLists.txt +++ b/s2e-ff/CMakeLists.txt @@ -97,6 +97,7 @@ set(SOURCE_FILES 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 diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp new file mode 100644 index 00000000..1cb2766a --- /dev/null +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp @@ -0,0 +1,14 @@ +#include "QuasiNonsingularOrbitalElementDifferences.hpp" + +#include + +QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDifferences(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target) { + d_semi_major_axis_m_ = qns_oe_target.GetSemiMajor() - qns_oe_reference.GetSemiMajor(); + d_eccentricity_x_ = qns_oe_target.GetEccentricityX() - qns_oe_reference.GetEccentricityX(); + d_eccentricity_y_ = qns_oe_target.GetEccentricityY() - qns_oe_reference.GetEccentricityY(); + d_inclination_rad_ = qns_oe_target.GetInclination() - qns_oe_reference.GetInclination(); + d_raan_rad_ = qns_oe_target.GetRaan() - qns_oe_reference.GetRaan(); + d_mean_arg_latitude_epoch_rad_ = qns_oe_target.GetMeanArgLatEpoch() - qns_oe_reference.GetMeanArgLatEpoch(); +} + +QuasiNonsingularOrbitalElementDifferences::~QuasiNonsingularOrbitalElementDifferences() {} diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp new file mode 100644 index 00000000..8ed0ad7f --- /dev/null +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp @@ -0,0 +1,29 @@ +#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: + QuasiNonsingularOrbitalElementDifferences(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target); + ~QuasiNonsingularOrbitalElementDifferences(); + // libra::Vector<3> CalcRelativePositionRtn_m(const double arg_lat_rad); //!< Calculate relative position from ROE + + private: + // Relative Orbital Elements + double d_semi_major_axis_m_; + double d_eccentricity_x_; + double d_eccentricity_y_; + double d_inclination_rad_; + double d_raan_rad_; + double d_mean_arg_latitude_epoch_rad_; +}; + +#endif diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp index 63f911b0..e9f0bf64 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp @@ -2,7 +2,7 @@ #include -RelativeOrbitalElements::RelativeOrbitalElements(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target) { +QuasiNonsingularRelativeOrbitalElements::QuasiNonsingularRelativeOrbitalElements(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target) { double diff_raan = qns_oe_target.GetRaan() - qns_oe_reference.GetRaan(); a_ref_m_ = qns_oe_reference.GetSemiMajor(); @@ -15,9 +15,9 @@ RelativeOrbitalElements::RelativeOrbitalElements(const QuasiNonsingularOrbitalEl delta_i_y_ = diff_raan * sin(qns_oe_reference.GetInclination()); } -RelativeOrbitalElements::~RelativeOrbitalElements() {} +QuasiNonsingularRelativeOrbitalElements::~QuasiNonsingularRelativeOrbitalElements() {} -libra::Vector<3> RelativeOrbitalElements::CalcRelativePositionRtn_m(const double arg_lat_rad) { +libra::Vector<3> QuasiNonsingularRelativeOrbitalElements::CalcRelativePositionRtn_m(const double arg_lat_rad) { libra::Vector<3> relative_position_rtn_m; double cos_u = cos(arg_lat_rad); double sin_u = sin(arg_lat_rad); diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp index 1447f076..a994d9e6 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp @@ -1,14 +1,19 @@ -#ifndef RELATIVE_ORBITAL_ELEMENTS_H_ -#define RELATIVE_ORBITAL_ELEMENTS_H_ +#ifndef QUASI_NONSINGULAR_RELATIVE_ORBITAL_ELEMENTS_H_ +#define QUASI_NONSINGULAR_RELATIVE_ORBITAL_ELEMENTS_H_ #include #include "../Orbit/QuasiNonsingularOrbitalElements.hpp" -class RelativeOrbitalElements { +/** + * @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: - RelativeOrbitalElements(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target); - ~RelativeOrbitalElements(); + QuasiNonsingularRelativeOrbitalElements(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target); + ~QuasiNonsingularRelativeOrbitalElements(); libra::Vector<3> CalcRelativePositionRtn_m(const double arg_lat_rad); //!< Calculate relative position from ROE private: From cda563c448a214e9f780779fe5bf01d52103f7aa Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sun, 2 Oct 2022 17:12:28 +0200 Subject: [PATCH 08/54] Add unit to get function --- .../Orbit/QuasiNonsingularOrbitalElements.hpp | 8 ++++---- .../QuasiNonsingularOrbitalElementDifferences.cpp | 8 ++++---- .../QuasiNonsingularRelativeOrbitalElements.cpp | 12 ++++++------ 3 files changed, 14 insertions(+), 14 deletions(-) diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp index a2b753bc..e48c4735 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp @@ -13,12 +13,12 @@ class QuasiNonsingularOrbitalElements { ~QuasiNonsingularOrbitalElements(); // Getter - inline double GetSemiMajor() const { return semi_major_axis_m_; } + inline double GetSemiMajor_m() const { return semi_major_axis_m_; } inline double GetEccentricityX() const { return eccentricity_x_; } inline double GetEccentricityY() const { return eccentricity_y_; } - inline double GetInclination() const { return inclination_rad_; } - inline double GetRaan() const { return raan_rad_; } - inline double GetMeanArgLatEpoch() const { return mean_arg_latitude_epoch_rad_; } + inline double GetInclination_rad() const { return inclination_rad_; } + inline double GetRaan_rad() const { return raan_rad_; } + inline double GetMeanArgLatEpoch_rad() const { return mean_arg_latitude_epoch_rad_; } private: double semi_major_axis_m_; diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp index 1cb2766a..2a146dfc 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp @@ -3,12 +3,12 @@ #include QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDifferences(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target) { - d_semi_major_axis_m_ = qns_oe_target.GetSemiMajor() - qns_oe_reference.GetSemiMajor(); + d_semi_major_axis_m_ = qns_oe_target.GetSemiMajor_m() - qns_oe_reference.GetSemiMajor_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_rad_ = qns_oe_target.GetInclination() - qns_oe_reference.GetInclination(); - d_raan_rad_ = qns_oe_target.GetRaan() - qns_oe_reference.GetRaan(); - d_mean_arg_latitude_epoch_rad_ = qns_oe_target.GetMeanArgLatEpoch() - qns_oe_reference.GetMeanArgLatEpoch(); + d_inclination_rad_ = qns_oe_target.GetInclination_rad() - qns_oe_reference.GetInclination_rad(); + d_raan_rad_ = qns_oe_target.GetRaan_rad() - qns_oe_reference.GetRaan_rad(); + d_mean_arg_latitude_epoch_rad_ = qns_oe_target.GetMeanArgLatEpoch_rad() - qns_oe_reference.GetMeanArgLatEpoch_rad(); } QuasiNonsingularOrbitalElementDifferences::~QuasiNonsingularOrbitalElementDifferences() {} diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp index e9f0bf64..4b620ccc 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp @@ -3,16 +3,16 @@ #include QuasiNonsingularRelativeOrbitalElements::QuasiNonsingularRelativeOrbitalElements(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target) { - double diff_raan = qns_oe_target.GetRaan() - qns_oe_reference.GetRaan(); + double diff_raan = qns_oe_target.GetRaan_rad() - qns_oe_reference.GetRaan_rad(); - a_ref_m_ = qns_oe_reference.GetSemiMajor(); - delta_a_ = (qns_oe_target.GetSemiMajor() - a_ref_m_) / a_ref_m_; - delta_lambda_ = (qns_oe_target.GetMeanArgLatEpoch() - qns_oe_reference.GetMeanArgLatEpoch()) + diff_raan * cos(qns_oe_reference.GetInclination()); + a_ref_m_ = qns_oe_reference.GetSemiMajor_m(); + delta_a_ = (qns_oe_target.GetSemiMajor_m() - a_ref_m_) / a_ref_m_; + delta_lambda_ = (qns_oe_target.GetMeanArgLatEpoch_rad() - qns_oe_reference.GetMeanArgLatEpoch_rad()) + diff_raan * cos(qns_oe_reference.GetInclination_rad()); delta_e_x_ = qns_oe_target.GetEccentricityX() - qns_oe_reference.GetEccentricityX(); delta_e_y_ = qns_oe_target.GetEccentricityY() - qns_oe_reference.GetEccentricityY(); - delta_i_x_ = qns_oe_target.GetInclination() - qns_oe_reference.GetInclination(); - delta_i_y_ = diff_raan * sin(qns_oe_reference.GetInclination()); + delta_i_x_ = qns_oe_target.GetInclination_rad() - qns_oe_reference.GetInclination_rad(); + delta_i_y_ = diff_raan * sin(qns_oe_reference.GetInclination_rad()); } QuasiNonsingularRelativeOrbitalElements::~QuasiNonsingularRelativeOrbitalElements() {} From 21987ef158a53f4d9873030103370ef5dcb4c141 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sun, 2 Oct 2022 17:26:29 +0200 Subject: [PATCH 09/54] Add getter for OOED --- .../QuasiNonsingularOrbitalElementDifferences.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp index 8ed0ad7f..9a768522 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp @@ -16,6 +16,14 @@ class QuasiNonsingularOrbitalElementDifferences { ~QuasiNonsingularOrbitalElementDifferences(); // libra::Vector<3> CalcRelativePositionRtn_m(const double arg_lat_rad); //!< Calculate relative position from ROE + // Getter + inline double GetDiffSemiMajor_m() const { return d_semi_major_axis_m_; } + inline double GetDiffEccentricityX() const { return d_eccentricity_x_; } + inline double GetDiffEccentricityY() const { return d_eccentricity_y_; } + inline double GetDiffInclination_rad() const { return d_inclination_rad_; } + inline double GetDiffRaan_rad() const { return d_raan_rad_; } + inline double GetDiffMeanArgLatEpoch_rad() const { return d_mean_arg_latitude_epoch_rad_; } + private: // Relative Orbital Elements double d_semi_major_axis_m_; From 3bee1903028acc0e6a6b9ff06ee487838a14b245 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sun, 2 Oct 2022 17:35:54 +0200 Subject: [PATCH 10/54] Add getter for ROE --- ...uasiNonsingularRelativeOrbitalElements.cpp | 24 +++++++++---------- ...uasiNonsingularRelativeOrbitalElements.hpp | 23 ++++++++++++------ 2 files changed, 28 insertions(+), 19 deletions(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp index 4b620ccc..c7a44885 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp @@ -5,14 +5,14 @@ QuasiNonsingularRelativeOrbitalElements::QuasiNonsingularRelativeOrbitalElements(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target) { double diff_raan = qns_oe_target.GetRaan_rad() - qns_oe_reference.GetRaan_rad(); - a_ref_m_ = qns_oe_reference.GetSemiMajor_m(); - delta_a_ = (qns_oe_target.GetSemiMajor_m() - a_ref_m_) / a_ref_m_; - delta_lambda_ = (qns_oe_target.GetMeanArgLatEpoch_rad() - qns_oe_reference.GetMeanArgLatEpoch_rad()) + diff_raan * cos(qns_oe_reference.GetInclination_rad()); - - delta_e_x_ = qns_oe_target.GetEccentricityX() - qns_oe_reference.GetEccentricityX(); - delta_e_y_ = qns_oe_target.GetEccentricityY() - qns_oe_reference.GetEccentricityY(); - delta_i_x_ = qns_oe_target.GetInclination_rad() - qns_oe_reference.GetInclination_rad(); - delta_i_y_ = diff_raan * sin(qns_oe_reference.GetInclination_rad()); + semi_major_axis_ref_m_ = qns_oe_reference.GetSemiMajor_m(); + delta_semi_major_axis_ = (qns_oe_target.GetSemiMajor_m() - semi_major_axis_ref_m_) / semi_major_axis_ref_m_; + delta_mean_longitude_ = (qns_oe_target.GetMeanArgLatEpoch_rad() - qns_oe_reference.GetMeanArgLatEpoch_rad()) + diff_raan * cos(qns_oe_reference.GetInclination_rad()); + + delta_eccentricity_x_ = qns_oe_target.GetEccentricityX() - qns_oe_reference.GetEccentricityX(); + delta_eccentricity_y_ = qns_oe_target.GetEccentricityY() - qns_oe_reference.GetEccentricityY(); + delta_inclination_x_ = qns_oe_target.GetInclination_rad() - qns_oe_reference.GetInclination_rad(); + delta_inclination_y_ = diff_raan * sin(qns_oe_reference.GetInclination_rad()); } QuasiNonsingularRelativeOrbitalElements::~QuasiNonsingularRelativeOrbitalElements() {} @@ -22,10 +22,10 @@ libra::Vector<3> QuasiNonsingularRelativeOrbitalElements::CalcRelativePositionRt double cos_u = cos(arg_lat_rad); double sin_u = sin(arg_lat_rad); - relative_position_rtn_m[0] = delta_a_ - (delta_e_x_ * cos_u + delta_e_y_ * sin_u); - relative_position_rtn_m[1] = -1.5 * delta_a_ * arg_lat_rad + delta_lambda_ + 2.0 * (delta_e_x_ * sin_u - delta_e_y_ * cos_u); - relative_position_rtn_m[2] = delta_i_x_ * sin_u - delta_i_y_ * cos_u; + relative_position_rtn_m[0] = delta_semi_major_axis_ - (delta_eccentricity_x_ * cos_u + delta_eccentricity_y_ * sin_u); + relative_position_rtn_m[1] = -1.5 * delta_semi_major_axis_ * arg_lat_rad + delta_mean_longitude_ + 2.0 * (delta_eccentricity_x_ * sin_u - delta_eccentricity_y_ * cos_u); + relative_position_rtn_m[2] = delta_inclination_x_ * sin_u - delta_inclination_y_ * cos_u; - relative_position_rtn_m *= a_ref_m_; + relative_position_rtn_m *= semi_major_axis_ref_m_; return relative_position_rtn_m; } diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp index a994d9e6..a8ffac49 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp @@ -16,17 +16,26 @@ class QuasiNonsingularRelativeOrbitalElements { ~QuasiNonsingularRelativeOrbitalElements(); libra::Vector<3> CalcRelativePositionRtn_m(const double arg_lat_rad); //!< Calculate relative position from ROE + // Getter + inline double GetSemiMajorRef_m() const { return semi_major_axis_ref_m_; } + inline double GetDeltaSemiMajor() const { return delta_semi_major_axis_; } + inline double GetDeltaMeanLongitude() const { return delta_mean_longitude_; } + inline double GetDeltaEccentricityX() const { return delta_eccentricity_x_; } + inline double GetDeltaEccentricityY() const { return delta_eccentricity_y_; } + inline double GetDeltaInclinationX() const { return delta_inclination_x_; } + inline double GetDeltaInclinationY() const { return delta_inclination_y_; } + private: // Reference orbit information - double a_ref_m_; //!< Semi major axis of reference orbit [m] + double semi_major_axis_ref_m_; //!< Semi major axis of reference orbit [m] // Relative Orbital Elements - double delta_a_; //!< Relative semi major axis [-] - double delta_lambda_; //!< Relative mean longitude [-] - double delta_e_x_; //!< Relative eccentricity vector X component [-] - double delta_e_y_; //!< Relative eccentricity vector Y component [-] - double delta_i_x_; //!< Relative inclination vector X component [-] - double delta_i_y_; //!< Relative inclination vector Y component [-] + double delta_semi_major_axis_; //!< Relative semi major axis [-] + double delta_mean_longitude_; //!< Relative mean longitude [-] + double delta_eccentricity_x_; //!< Relative eccentricity vector X component [-] + double delta_eccentricity_y_; //!< Relative eccentricity vector Y component [-] + double delta_inclination_x_; //!< Relative inclination vector X component [-] + double delta_inclination_y_; //!< Relative inclination vector Y component [-] }; #endif From 6603b6a4e955cb496b7c37da97c84c4719923daf Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Sun, 2 Oct 2022 22:05:40 +0200 Subject: [PATCH 11/54] rename variables in REO --- ...siNonsingularOrbitalElementDifferences.cpp | 5 ++++ ...siNonsingularOrbitalElementDifferences.hpp | 4 ++- ...uasiNonsingularRelativeOrbitalElements.cpp | 21 +++++++------- ...uasiNonsingularRelativeOrbitalElements.hpp | 28 ++++++++++--------- 4 files changed, 34 insertions(+), 24 deletions(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp index 2a146dfc..2809f5a9 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp @@ -12,3 +12,8 @@ QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDiffere } QuasiNonsingularOrbitalElementDifferences::~QuasiNonsingularOrbitalElementDifferences() {} + +libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePositionCircularApprox_rtn_m(const double arg_lat_rad) +{ + +} diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp index 9a768522..9f1e205e 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp @@ -14,7 +14,9 @@ class QuasiNonsingularOrbitalElementDifferences { public: QuasiNonsingularOrbitalElementDifferences(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target); ~QuasiNonsingularOrbitalElementDifferences(); - // libra::Vector<3> CalcRelativePositionRtn_m(const double arg_lat_rad); //!< Calculate relative position from ROE + + // Calculation + libra::Vector<3> CalcRelativePositionCircularApprox_rtn_m(const double arg_lat_rad); //!< Calculate relative position from OED when near circular chief orbit // Getter inline double GetDiffSemiMajor_m() const { return d_semi_major_axis_m_; } diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp index c7a44885..e76e85c0 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp @@ -6,25 +6,26 @@ QuasiNonsingularRelativeOrbitalElements::QuasiNonsingularRelativeOrbitalElements double diff_raan = qns_oe_target.GetRaan_rad() - qns_oe_reference.GetRaan_rad(); semi_major_axis_ref_m_ = qns_oe_reference.GetSemiMajor_m(); - delta_semi_major_axis_ = (qns_oe_target.GetSemiMajor_m() - semi_major_axis_ref_m_) / semi_major_axis_ref_m_; - delta_mean_longitude_ = (qns_oe_target.GetMeanArgLatEpoch_rad() - qns_oe_reference.GetMeanArgLatEpoch_rad()) + diff_raan * cos(qns_oe_reference.GetInclination_rad()); - delta_eccentricity_x_ = qns_oe_target.GetEccentricityX() - qns_oe_reference.GetEccentricityX(); - delta_eccentricity_y_ = qns_oe_target.GetEccentricityY() - qns_oe_reference.GetEccentricityY(); - delta_inclination_x_ = qns_oe_target.GetInclination_rad() - qns_oe_reference.GetInclination_rad(); - delta_inclination_y_ = diff_raan * sin(qns_oe_reference.GetInclination_rad()); + d_semi_major_axis_ = (qns_oe_target.GetSemiMajor_m() - semi_major_axis_ref_m_) / semi_major_axis_ref_m_; + d_mean_longitude_ = (qns_oe_target.GetMeanArgLatEpoch_rad() - qns_oe_reference.GetMeanArgLatEpoch_rad()) + diff_raan * cos(qns_oe_reference.GetInclination_rad()); + + 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(); + d_inclination_y_ = diff_raan * sin(qns_oe_reference.GetInclination_rad()); } QuasiNonsingularRelativeOrbitalElements::~QuasiNonsingularRelativeOrbitalElements() {} -libra::Vector<3> QuasiNonsingularRelativeOrbitalElements::CalcRelativePositionRtn_m(const double arg_lat_rad) { +libra::Vector<3> QuasiNonsingularRelativeOrbitalElements::CalcRelativePositionCircularApprox_rtn_m(const double arg_lat_rad) { libra::Vector<3> relative_position_rtn_m; double cos_u = cos(arg_lat_rad); double sin_u = sin(arg_lat_rad); - relative_position_rtn_m[0] = delta_semi_major_axis_ - (delta_eccentricity_x_ * cos_u + delta_eccentricity_y_ * sin_u); - relative_position_rtn_m[1] = -1.5 * delta_semi_major_axis_ * arg_lat_rad + delta_mean_longitude_ + 2.0 * (delta_eccentricity_x_ * sin_u - delta_eccentricity_y_ * cos_u); - relative_position_rtn_m[2] = delta_inclination_x_ * sin_u - delta_inclination_y_ * cos_u; + 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_ * 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; diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp index a8ffac49..79f35e9e 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp @@ -14,28 +14,30 @@ class QuasiNonsingularRelativeOrbitalElements { public: QuasiNonsingularRelativeOrbitalElements(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target); ~QuasiNonsingularRelativeOrbitalElements(); - libra::Vector<3> CalcRelativePositionRtn_m(const double arg_lat_rad); //!< Calculate relative position from ROE + + // Calculation + libra::Vector<3> CalcRelativePositionCircularApprox_rtn_m(const double arg_lat_rad); //!< Calculate relative position from ROE when near circular chief orbit // Getter inline double GetSemiMajorRef_m() const { return semi_major_axis_ref_m_; } - inline double GetDeltaSemiMajor() const { return delta_semi_major_axis_; } - inline double GetDeltaMeanLongitude() const { return delta_mean_longitude_; } - inline double GetDeltaEccentricityX() const { return delta_eccentricity_x_; } - inline double GetDeltaEccentricityY() const { return delta_eccentricity_y_; } - inline double GetDeltaInclinationX() const { return delta_inclination_x_; } - inline double GetDeltaInclinationY() const { return delta_inclination_y_; } + inline double GetDeltaSemiMajor() const { return d_semi_major_axis_; } + inline double GetDeltaMeanLongitude() const { return d_mean_longitude_; } + inline double GetDeltaEccentricityX() const { return d_eccentricity_x_; } + inline double GetDeltaEccentricityY() const { return d_eccentricity_y_; } + inline double GetDeltaInclinationX() const { return d_inclination_x_; } + 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 delta_semi_major_axis_; //!< Relative semi major axis [-] - double delta_mean_longitude_; //!< Relative mean longitude [-] - double delta_eccentricity_x_; //!< Relative eccentricity vector X component [-] - double delta_eccentricity_y_; //!< Relative eccentricity vector Y component [-] - double delta_inclination_x_; //!< Relative inclination vector X component [-] - double delta_inclination_y_; //!< Relative inclination vector Y component [-] + 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 [-] }; #endif From 7c980934591eb8d1897765f07ea15b3cb5dc0fb1 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Thu, 17 Nov 2022 10:55:13 +0100 Subject: [PATCH 12/54] c --- ...siNonsingularOrbitalElementDifferences.cpp | 8 +++++- ...siNonsingularOrbitalElementDifferences.hpp | 7 +++-- ...uasiNonsingularRelativeOrbitalElements.hpp | 1 - .../TestTranslationFirstDualQuaternion.cpp | 26 +++++++++++++++++++ 4 files changed, 38 insertions(+), 4 deletions(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp index 2809f5a9..814f3794 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp @@ -3,6 +3,8 @@ #include QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDifferences(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target) { + semi_major_axis_ref_m_ = qns_oe_reference.GetSemiMajor_m(); + d_semi_major_axis_m_ = qns_oe_target.GetSemiMajor_m() - qns_oe_reference.GetSemiMajor_m(); d_eccentricity_x_ = qns_oe_target.GetEccentricityX() - qns_oe_reference.GetEccentricityX(); d_eccentricity_y_ = qns_oe_target.GetEccentricityY() - qns_oe_reference.GetEccentricityY(); @@ -13,7 +15,11 @@ QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDiffere QuasiNonsingularOrbitalElementDifferences::~QuasiNonsingularOrbitalElementDifferences() {} -libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePositionCircularApprox_rtn_m(const double arg_lat_rad) +libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePositionCircularApprox_rtn_m(const double true_anomaly_rad) { + libra::Vector<3> relative_position_rtn_m; + double cos_f = cos(true_anomaly_rad); + double sin_f = sin(true_anomaly_rad); + } diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp index 9f1e205e..5f87f27a 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp @@ -16,7 +16,7 @@ class QuasiNonsingularOrbitalElementDifferences { ~QuasiNonsingularOrbitalElementDifferences(); // Calculation - libra::Vector<3> CalcRelativePositionCircularApprox_rtn_m(const double arg_lat_rad); //!< Calculate relative position from OED when near circular chief orbit + libra::Vector<3> CalcRelativePositionCircularApprox_rtn_m(const double true_anomaly_rad); //!< Calculate relative position from OED when near circular chief orbit // Getter inline double GetDiffSemiMajor_m() const { return d_semi_major_axis_m_; } @@ -27,7 +27,10 @@ class QuasiNonsingularOrbitalElementDifferences { inline double GetDiffMeanArgLatEpoch_rad() const { return d_mean_arg_latitude_epoch_rad_; } private: - // Relative Orbital Elements + // Reference orbit information + double semi_major_axis_ref_m_; //!< Semi major axis of reference orbit [m] + + // Orbital Element Differences double d_semi_major_axis_m_; double d_eccentricity_x_; double d_eccentricity_y_; diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp index 79f35e9e..45c9ce62 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp @@ -19,7 +19,6 @@ class QuasiNonsingularRelativeOrbitalElements { libra::Vector<3> CalcRelativePositionCircularApprox_rtn_m(const double arg_lat_rad); //!< Calculate relative position from ROE when near circular chief orbit // Getter - inline double GetSemiMajorRef_m() const { return semi_major_axis_ref_m_; } inline double GetDeltaSemiMajor() const { return d_semi_major_axis_; } inline double GetDeltaMeanLongitude() const { return d_mean_longitude_; } inline double GetDeltaEccentricityX() const { return d_eccentricity_x_; } diff --git a/s2e-ff/test/TestTranslationFirstDualQuaternion.cpp b/s2e-ff/test/TestTranslationFirstDualQuaternion.cpp index d01ee06b..eb92128a 100644 --- a/s2e-ff/test/TestTranslationFirstDualQuaternion.cpp +++ b/s2e-ff/test/TestTranslationFirstDualQuaternion.cpp @@ -1,6 +1,7 @@ #include #include "../src/Library/math/TranslationFirstDualQuaternion.hpp" +#include TEST(TranslationFirstDualQuaternion, ConstructorFromRotationTranslation) { libra::Quaternion q_rot(1.0, 0.0, 0.0, 1.0); // 90deg rotation around X axis @@ -92,8 +93,18 @@ TEST(TranslationFirstDualQuaternion, Integral) { double dt = 0.01; // Integration + //std::cout << "time[sec],"; + //std::cout << "dq_r_x, dq_r_y, dq_r_z, dq_r_w,"; + //std::cout << "dq_d_x, dq_d_y, dq_d_z, dq_d_w,"; + //std::cout << "v_x, v_y, v_z,"; + //std::cout << std::endl; for (int i = 0; i < 9000; i++) { dq = dq.Integrate(omega, velocity, dt); + //std::cout << i * dt << ","; + //std::cout << dq.GetRealPart()[0] << "," << dq.GetRealPart()[1] << "," << dq.GetRealPart()[2] << "," << dq.GetRealPart()[3] << ","; + //std::cout << dq.GetDualPart()[0] << "," << dq.GetDualPart()[1] << "," << dq.GetDualPart()[2] << "," << dq.GetDualPart()[3] << ","; + //std::cout << dq.GetTranslationVector()[0] << "," << dq.GetTranslationVector()[1] << "," << dq.GetTranslationVector()[2] << ","; + //std::cout << std::endl; } // Check rotation @@ -236,6 +247,21 @@ TEST(TranslationFirstDualQuaternion, SclerpXAxisRotXYAxisMove) { v2_translation[2] = 0.0; libra::TranslationFirstDualQuaternion dq2(v2_translation, q2_rot); + // debug + std::cout << "time[sec],"; + std::cout << "dq_r_x, dq_r_y, dq_r_z, dq_r_w,"; + std::cout << "dq_d_x, dq_d_y, dq_d_z, dq_d_w,"; + std::cout << "v_x, v_y, v_z,"; + std::cout << std::endl; + for (double duty = 0.005; duty <= 1; duty+=0.005) + { + libra::TranslationFirstDualQuaternion dq_test = libra::Sclerp(dq1, dq2, duty); + std::cout << duty << ","; + std::cout << dq_test.GetRealPart()[0] << "," << dq_test.GetRealPart()[1] << "," << dq_test.GetRealPart()[2] << "," << dq_test.GetRealPart()[3] << ","; + std::cout << dq_test.GetDualPart()[0] << "," << dq_test.GetDualPart()[1] << "," << dq_test.GetDualPart()[2] << "," << dq_test.GetDualPart()[3] << ","; + std::cout << dq_test.GetTranslationVector()[0] << "," << dq_test.GetTranslationVector()[1] << "," << dq_test.GetTranslationVector()[2] << ","; + std::cout << std::endl; + } // X 90deg rotation and X-Y axis translation libra::TranslationFirstDualQuaternion dq_out = libra::Sclerp(dq1, dq2, 0.5); From cc97c9bf9c0b5fd5a54d3bcc398c2b9b24d9e7e8 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 29 Nov 2022 13:48:29 +0100 Subject: [PATCH 13/54] Add comments --- .../Orbit/QuasiNonsingularOrbitalElements.cpp | 5 ++ .../Orbit/QuasiNonsingularOrbitalElements.hpp | 46 +++++++++++-- ...siNonsingularOrbitalElementDifferences.cpp | 13 ++-- ...siNonsingularOrbitalElementDifferences.hpp | 64 ++++++++++++++++--- ...uasiNonsingularRelativeOrbitalElements.cpp | 14 +++- ...uasiNonsingularRelativeOrbitalElements.hpp | 23 ++++--- 6 files changed, 136 insertions(+), 29 deletions(-) diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp index 42543051..ccea7e29 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp @@ -1,3 +1,8 @@ +/** + * @file QuasiNonsingularOrbitalElements.cpp + * @brief Orbital elements avoid singularity when the eccentricity is near zero. + */ + #include "QuasiNonsingularOrbitalElements.hpp" QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const OrbitalElements oe) { diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp index e48c4735..702837cc 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp @@ -1,3 +1,8 @@ +/** + * @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_ @@ -9,22 +14,55 @@ */ class QuasiNonsingularOrbitalElements { public: + /** + * @fn QuasiNonsingularOrbitalElements + * @brief Constructor initialized with orbital elements + * @param oe: Orbital Elements + */ QuasiNonsingularOrbitalElements(const OrbitalElements oe); + /** + * @fn ~QuasiNonsingularOrbitalElements + * @brief Deconstructor + */ ~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 GetMeanArgLatEpoch_rad + * @brief Return Mean argument of Latitude at epoch [rad] + */ inline double GetMeanArgLatEpoch_rad() const { return mean_arg_latitude_epoch_rad_; } private: - double semi_major_axis_m_; - double eccentricity_x_; // e * cos(arg_peri) - double eccentricity_y_; // e * sin(arg_peri) - double inclination_rad_; + 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 mean_arg_latitude_epoch_rad_; //!< Mean argument of Latitude at epoch (arg_peri + mean anomaly) [rad] }; diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp index 814f3794..f9fb802a 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp @@ -1,8 +1,14 @@ +/** + * @file QuasiNonsingularOrbitalElementDifferences.cpp + * @brief Orbital elements differences to avoid singularity when the eccentricity is near zero. + */ + #include "QuasiNonsingularOrbitalElementDifferences.hpp" #include -QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDifferences(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target) { +QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDifferences(const QuasiNonsingularOrbitalElements qns_oe_reference, + const QuasiNonsingularOrbitalElements qns_oe_target) { semi_major_axis_ref_m_ = qns_oe_reference.GetSemiMajor_m(); d_semi_major_axis_m_ = qns_oe_target.GetSemiMajor_m() - qns_oe_reference.GetSemiMajor_m(); @@ -15,11 +21,8 @@ QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDiffere QuasiNonsingularOrbitalElementDifferences::~QuasiNonsingularOrbitalElementDifferences() {} -libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePositionCircularApprox_rtn_m(const double true_anomaly_rad) -{ +libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePositionCircularApprox_rtn_m(const double true_anomaly_rad) { libra::Vector<3> relative_position_rtn_m; double cos_f = cos(true_anomaly_rad); double sin_f = sin(true_anomaly_rad); - - } diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp index 5f87f27a..785bfde8 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp @@ -1,3 +1,8 @@ +/** + * @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_ @@ -12,18 +17,59 @@ */ class QuasiNonsingularOrbitalElementDifferences { public: - QuasiNonsingularOrbitalElementDifferences(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target); + /** + * @fn QuasiNonsingularOrbitalElementDifferences + * @brief Constructor initialized with tow quasi-nonsingular orbital elements + * @param qns_oe_reference: Quasi-nonsingular orbital elements of the reference spacecraft + * @param 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 deconstructor + */ ~QuasiNonsingularOrbitalElementDifferences(); - + // Calculation - libra::Vector<3> CalcRelativePositionCircularApprox_rtn_m(const double true_anomaly_rad); //!< Calculate relative position from OED when near circular chief orbit + /** + * @fn CalcRelativePositionCircularApprox_rtn_m + * @brief Calculate the relative position of target spacecraft with circular approximation + * @params true_anomaly_rad: True anomaly [rad] + * @return Relative position vector in RTN frame of reference spacecraft + */ + libra::Vector<3> CalcRelativePositionCircularApprox_rtn_m(const double true_anomaly_rad); // Getter + /** + * @fn GetDiffSemiMajor_m + * @brief Return difference of semi-major axis [m] + */ inline double GetDiffSemiMajor_m() const { return d_semi_major_axis_m_; } + /** + * @fn GetDiffEccentricityX + * @brief Return difference of eccentricity vector X component + */ inline double GetDiffEccentricityX() const { return d_eccentricity_x_; } + /** + * @fn GetDiffEccentricityY + * @brief Return difference of eccentricity vector Y component + */ inline double GetDiffEccentricityY() const { return d_eccentricity_y_; } + /** + * @fn GetDiffInclination_rad + * @brief Return difference of inclination [rad] + */ inline double GetDiffInclination_rad() const { return d_inclination_rad_; } + /** + * @fn GetDiffRaan_rad + * @brief Return difference of RAAN [rad] + */ inline double GetDiffRaan_rad() const { return d_raan_rad_; } + /** + * @fn GetDiffMeanArgLatEpoch_rad + * @brief Return difference of argument of latitude [rad] + */ inline double GetDiffMeanArgLatEpoch_rad() const { return d_mean_arg_latitude_epoch_rad_; } private: @@ -31,12 +77,12 @@ class QuasiNonsingularOrbitalElementDifferences { double semi_major_axis_ref_m_; //!< Semi major axis of reference orbit [m] // Orbital Element Differences - double d_semi_major_axis_m_; - double d_eccentricity_x_; - double d_eccentricity_y_; - double d_inclination_rad_; - double d_raan_rad_; - double d_mean_arg_latitude_epoch_rad_; + double d_semi_major_axis_m_; //!< Difference of semi major axis of reference orbit [m] + double d_eccentricity_x_; //!< Difference of eccentricity vector X component + double d_eccentricity_y_; //!< Difference of eccentricity vector Y component + double d_inclination_rad_; //!< Difference of inclination [rad] + double d_raan_rad_; //!< Difference of RAAN [rad] + double d_mean_arg_latitude_epoch_rad_; //!< Difference of argument of latitude [rad] }; #endif diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp index e76e85c0..41985f56 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp @@ -1,14 +1,21 @@ +/** + * @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) { +QuasiNonsingularRelativeOrbitalElements::QuasiNonsingularRelativeOrbitalElements(const QuasiNonsingularOrbitalElements qns_oe_reference, + const QuasiNonsingularOrbitalElements qns_oe_target) { double diff_raan = qns_oe_target.GetRaan_rad() - qns_oe_reference.GetRaan_rad(); 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_mean_longitude_ = (qns_oe_target.GetMeanArgLatEpoch_rad() - qns_oe_reference.GetMeanArgLatEpoch_rad()) + diff_raan * cos(qns_oe_reference.GetInclination_rad()); + d_mean_longitude_ = + (qns_oe_target.GetMeanArgLatEpoch_rad() - qns_oe_reference.GetMeanArgLatEpoch_rad()) + diff_raan * cos(qns_oe_reference.GetInclination_rad()); d_eccentricity_x_ = qns_oe_target.GetEccentricityX() - qns_oe_reference.GetEccentricityX(); d_eccentricity_y_ = qns_oe_target.GetEccentricityY() - qns_oe_reference.GetEccentricityY(); @@ -24,7 +31,8 @@ libra::Vector<3> QuasiNonsingularRelativeOrbitalElements::CalcRelativePositionCi double sin_u = sin(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_ * arg_lat_rad + d_mean_longitude_ + 2.0 * (d_eccentricity_x_ * sin_u - d_eccentricity_y_ * cos_u); + relative_position_rtn_m[1] = + -1.5 * d_semi_major_axis_ * 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_; diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp index 45c9ce62..81175225 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp @@ -1,3 +1,8 @@ +/** + * @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_ @@ -12,11 +17,13 @@ */ class QuasiNonsingularRelativeOrbitalElements { public: - QuasiNonsingularRelativeOrbitalElements(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target); + QuasiNonsingularRelativeOrbitalElements(const QuasiNonsingularOrbitalElements qns_oe_reference, + const QuasiNonsingularOrbitalElements qns_oe_target); ~QuasiNonsingularRelativeOrbitalElements(); // Calculation - libra::Vector<3> CalcRelativePositionCircularApprox_rtn_m(const double arg_lat_rad); //!< Calculate relative position from ROE when near circular chief orbit + libra::Vector<3> CalcRelativePositionCircularApprox_rtn_m( + const double arg_lat_rad); //!< Calculate relative position from ROE when near circular chief orbit // Getter inline double GetDeltaSemiMajor() const { return d_semi_major_axis_; } @@ -31,12 +38,12 @@ class QuasiNonsingularRelativeOrbitalElements { 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 [-] + 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 [-] }; #endif From 4ba17772563b5edfb4660de818395dfbd7137449 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 29 Nov 2022 15:43:26 +0100 Subject: [PATCH 14/54] Add subtract function --- .../Orbit/QuasiNonsingularOrbitalElements.cpp | 23 +++++++++++++++++++ .../Orbit/QuasiNonsingularOrbitalElements.hpp | 12 ++++++++++ 2 files changed, 35 insertions(+) diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp index ccea7e29..451647e2 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp @@ -5,6 +5,16 @@ #include "QuasiNonsingularOrbitalElements.hpp" +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_epoch_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), + mean_arg_latitude_epoch_rad_(mean_arg_latitude_epoch_rad) {} + QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const OrbitalElements oe) { double mean_anomaly_rad = 0.0; // since the epoch is the perigee pass time @@ -15,3 +25,16 @@ QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const OrbitalEl inclination_rad_ = oe.GetInclination(); raan_rad_ = oe.GetRaan(); } + +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 mean_arg_latitude_epoch_rad = lhs.GetMeanArgLatEpoch_rad() - lhs.GetMeanArgLatEpoch_rad(); + + QuasiNonsingularOrbitalElements out(semi_major_axis_m, eccentricity_x, eccentricity_y, inclination_rad, raan_rad, mean_arg_latitude_epoch_rad); + + return out; +} diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp index 702837cc..aa198e88 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp @@ -14,6 +14,12 @@ */ class QuasiNonsingularOrbitalElements { public: + /** + * @fn QuasiNonsingularOrbitalElements + * @brief Constructor initialized with values + */ + 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_epoch_rad); /** * @fn QuasiNonsingularOrbitalElements * @brief Constructor initialized with orbital elements @@ -67,4 +73,10 @@ class QuasiNonsingularOrbitalElements { double mean_arg_latitude_epoch_rad_; //!< Mean argument of Latitude at epoch (arg_peri + mean anomaly) [rad] }; +/** + * @fn Operator - + * @brief Calculate subtract of two orbital elements + */ +QuasiNonsingularOrbitalElements operator-(const QuasiNonsingularOrbitalElements lhs, const QuasiNonsingularOrbitalElements rhs); + #endif From 91f50adeecd5e3976f55666a2361024944c4f767 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 29 Nov 2022 15:53:49 +0100 Subject: [PATCH 15/54] Add default constructor --- .../Library/Orbit/QuasiNonsingularOrbitalElements.cpp | 11 +++++++++++ .../Library/Orbit/QuasiNonsingularOrbitalElements.hpp | 7 ++++++- 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp index 451647e2..73439e51 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp @@ -5,6 +5,15 @@ #include "QuasiNonsingularOrbitalElements.hpp" +QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements() { + semi_major_axis_m_ = 0.0; + mean_arg_latitude_epoch_rad_ = 0.0; + eccentricity_x_ = 0.0; + eccentricity_y_ = 0.0; + inclination_rad_ = 0.0; + raan_rad_ = 0.0; +} + 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_epoch_rad) @@ -26,6 +35,8 @@ QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const OrbitalEl raan_rad_ = oe.GetRaan(); } +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(); diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp index aa198e88..6388719c 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp @@ -14,6 +14,11 @@ */ class QuasiNonsingularOrbitalElements { public: + /** + * @fn QuasiNonsingularOrbitalElements + * @brief Default Constructor + */ + QuasiNonsingularOrbitalElements(); /** * @fn QuasiNonsingularOrbitalElements * @brief Constructor initialized with values @@ -75,7 +80,7 @@ class QuasiNonsingularOrbitalElements { /** * @fn Operator - - * @brief Calculate subtract of two orbital elements + * @brief Calculate subtract of two quasi-nonsingular orbital elements */ QuasiNonsingularOrbitalElements operator-(const QuasiNonsingularOrbitalElements lhs, const QuasiNonsingularOrbitalElements rhs); From 52c2b4802dbc133fea0b0babd969c6712c7203c4 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 29 Nov 2022 15:54:04 +0100 Subject: [PATCH 16/54] Modify to use subtract of nonsingular OE --- ...siNonsingularOrbitalElementDifferences.cpp | 9 ++-- ...siNonsingularOrbitalElementDifferences.hpp | 43 +++---------------- 2 files changed, 8 insertions(+), 44 deletions(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp index f9fb802a..dd2c3c6d 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp @@ -11,12 +11,7 @@ QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDiffere const QuasiNonsingularOrbitalElements qns_oe_target) { semi_major_axis_ref_m_ = qns_oe_reference.GetSemiMajor_m(); - d_semi_major_axis_m_ = qns_oe_target.GetSemiMajor_m() - qns_oe_reference.GetSemiMajor_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_rad_ = qns_oe_target.GetInclination_rad() - qns_oe_reference.GetInclination_rad(); - d_raan_rad_ = qns_oe_target.GetRaan_rad() - qns_oe_reference.GetRaan_rad(); - d_mean_arg_latitude_epoch_rad_ = qns_oe_target.GetMeanArgLatEpoch_rad() - qns_oe_reference.GetMeanArgLatEpoch_rad(); + diff_qns_oe_ = qns_oe_target - qns_oe_reference; } QuasiNonsingularOrbitalElementDifferences::~QuasiNonsingularOrbitalElementDifferences() {} @@ -25,4 +20,6 @@ libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePosition libra::Vector<3> relative_position_rtn_m; double cos_f = cos(true_anomaly_rad); double sin_f = sin(true_anomaly_rad); + + return relative_position_rtn_m; } diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp index 785bfde8..3f274a7f 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp @@ -42,47 +42,14 @@ class QuasiNonsingularOrbitalElementDifferences { // Getter /** - * @fn GetDiffSemiMajor_m - * @brief Return difference of semi-major axis [m] + * @fn GetDiffQuasiNonSingularOrbitalElements + * @brief Return difference of quasi-nonsingular orbital elements */ - inline double GetDiffSemiMajor_m() const { return d_semi_major_axis_m_; } - /** - * @fn GetDiffEccentricityX - * @brief Return difference of eccentricity vector X component - */ - inline double GetDiffEccentricityX() const { return d_eccentricity_x_; } - /** - * @fn GetDiffEccentricityY - * @brief Return difference of eccentricity vector Y component - */ - inline double GetDiffEccentricityY() const { return d_eccentricity_y_; } - /** - * @fn GetDiffInclination_rad - * @brief Return difference of inclination [rad] - */ - inline double GetDiffInclination_rad() const { return d_inclination_rad_; } - /** - * @fn GetDiffRaan_rad - * @brief Return difference of RAAN [rad] - */ - inline double GetDiffRaan_rad() const { return d_raan_rad_; } - /** - * @fn GetDiffMeanArgLatEpoch_rad - * @brief Return difference of argument of latitude [rad] - */ - inline double GetDiffMeanArgLatEpoch_rad() const { return d_mean_arg_latitude_epoch_rad_; } + inline QuasiNonsingularOrbitalElements GetDiffQuasiNonSingularOrbitalElements() const { return diff_qns_oe_; } private: - // Reference orbit information - double semi_major_axis_ref_m_; //!< Semi major axis of reference orbit [m] - - // Orbital Element Differences - double d_semi_major_axis_m_; //!< Difference of semi major axis of reference orbit [m] - double d_eccentricity_x_; //!< Difference of eccentricity vector X component - double d_eccentricity_y_; //!< Difference of eccentricity vector Y component - double d_inclination_rad_; //!< Difference of inclination [rad] - double d_raan_rad_; //!< Difference of RAAN [rad] - double d_mean_arg_latitude_epoch_rad_; //!< Difference of argument of latitude [rad] + double semi_major_axis_ref_m_; //!< Semi major axis of reference orbit [m] + QuasiNonsingularOrbitalElements diff_qns_oe_; //!< Difference of quasi-nonsingular orbital elements }; #endif From fec3c8ea25b82a09a44366dbc5b014c96fc1891e Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 20 Dec 2022 10:29:03 +0100 Subject: [PATCH 17/54] Add calculation --- ...siNonsingularOrbitalElementDifferences.cpp | 35 ++++++++++++++++--- ...siNonsingularOrbitalElementDifferences.hpp | 4 +-- 2 files changed, 32 insertions(+), 7 deletions(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp index dd2c3c6d..7c834133 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp @@ -8,18 +8,43 @@ #include QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDifferences(const QuasiNonsingularOrbitalElements qns_oe_reference, - const QuasiNonsingularOrbitalElements qns_oe_target) { - semi_major_axis_ref_m_ = qns_oe_reference.GetSemiMajor_m(); - + const QuasiNonsingularOrbitalElements qns_oe_target) + : qns_oe_reference_(qns_oe_reference) { diff_qns_oe_ = qns_oe_target - qns_oe_reference; } QuasiNonsingularOrbitalElementDifferences::~QuasiNonsingularOrbitalElementDifferences() {} libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePositionCircularApprox_rtn_m(const double true_anomaly_rad) { + // 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_.GetMeanArgLatEpoch_rad(); // + some thing? + const double cos_theta = cos(theta); + const double sin_theta = sin(theta); + const double p = a * (1.0 - (ex * ex + ey * ey)); //!< Semilatus rectum + + // 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_.GetMeanArgLatEpoch_rad(); + const double d_i = diff_qns_oe_.GetInclination_rad(); + const double d_raan = diff_qns_oe_.GetRaan_rad(); + + // calculation + const double r = p / (1.0 + ex * cos_theta + ey * sin_theta); + const double v_r = (ex * sin_theta - ey * cos_theta); + 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; - double cos_f = cos(true_anomaly_rad); - double sin_f = sin(true_anomaly_rad); + 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; } diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp index 3f274a7f..ec2daf81 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp @@ -48,8 +48,8 @@ class QuasiNonsingularOrbitalElementDifferences { inline QuasiNonsingularOrbitalElements GetDiffQuasiNonSingularOrbitalElements() const { return diff_qns_oe_; } private: - double semi_major_axis_ref_m_; //!< Semi major axis of reference orbit [m] - QuasiNonsingularOrbitalElements diff_qns_oe_; //!< Difference of quasi-nonsingular orbital elements + QuasiNonsingularOrbitalElements qns_oe_reference_; //!< Quasi-nonsingular orbital elements of reference spacecraft + QuasiNonsingularOrbitalElements diff_qns_oe_; //!< Difference of quasi-nonsingular orbital elements }; #endif From 364783012c32741986aed4e0ab5ef3cea403e10d Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 20 Dec 2022 10:40:26 +0100 Subject: [PATCH 18/54] revert unrelated codes --- .../TestTranslationFirstDualQuaternion.cpp | 26 ------------------- 1 file changed, 26 deletions(-) diff --git a/s2e-ff/test/TestTranslationFirstDualQuaternion.cpp b/s2e-ff/test/TestTranslationFirstDualQuaternion.cpp index eb92128a..d01ee06b 100644 --- a/s2e-ff/test/TestTranslationFirstDualQuaternion.cpp +++ b/s2e-ff/test/TestTranslationFirstDualQuaternion.cpp @@ -1,7 +1,6 @@ #include #include "../src/Library/math/TranslationFirstDualQuaternion.hpp" -#include TEST(TranslationFirstDualQuaternion, ConstructorFromRotationTranslation) { libra::Quaternion q_rot(1.0, 0.0, 0.0, 1.0); // 90deg rotation around X axis @@ -93,18 +92,8 @@ TEST(TranslationFirstDualQuaternion, Integral) { double dt = 0.01; // Integration - //std::cout << "time[sec],"; - //std::cout << "dq_r_x, dq_r_y, dq_r_z, dq_r_w,"; - //std::cout << "dq_d_x, dq_d_y, dq_d_z, dq_d_w,"; - //std::cout << "v_x, v_y, v_z,"; - //std::cout << std::endl; for (int i = 0; i < 9000; i++) { dq = dq.Integrate(omega, velocity, dt); - //std::cout << i * dt << ","; - //std::cout << dq.GetRealPart()[0] << "," << dq.GetRealPart()[1] << "," << dq.GetRealPart()[2] << "," << dq.GetRealPart()[3] << ","; - //std::cout << dq.GetDualPart()[0] << "," << dq.GetDualPart()[1] << "," << dq.GetDualPart()[2] << "," << dq.GetDualPart()[3] << ","; - //std::cout << dq.GetTranslationVector()[0] << "," << dq.GetTranslationVector()[1] << "," << dq.GetTranslationVector()[2] << ","; - //std::cout << std::endl; } // Check rotation @@ -247,21 +236,6 @@ TEST(TranslationFirstDualQuaternion, SclerpXAxisRotXYAxisMove) { v2_translation[2] = 0.0; libra::TranslationFirstDualQuaternion dq2(v2_translation, q2_rot); - // debug - std::cout << "time[sec],"; - std::cout << "dq_r_x, dq_r_y, dq_r_z, dq_r_w,"; - std::cout << "dq_d_x, dq_d_y, dq_d_z, dq_d_w,"; - std::cout << "v_x, v_y, v_z,"; - std::cout << std::endl; - for (double duty = 0.005; duty <= 1; duty+=0.005) - { - libra::TranslationFirstDualQuaternion dq_test = libra::Sclerp(dq1, dq2, duty); - std::cout << duty << ","; - std::cout << dq_test.GetRealPart()[0] << "," << dq_test.GetRealPart()[1] << "," << dq_test.GetRealPart()[2] << "," << dq_test.GetRealPart()[3] << ","; - std::cout << dq_test.GetDualPart()[0] << "," << dq_test.GetDualPart()[1] << "," << dq_test.GetDualPart()[2] << "," << dq_test.GetDualPart()[3] << ","; - std::cout << dq_test.GetTranslationVector()[0] << "," << dq_test.GetTranslationVector()[1] << "," << dq_test.GetTranslationVector()[2] << ","; - std::cout << std::endl; - } // X 90deg rotation and X-Y axis translation libra::TranslationFirstDualQuaternion dq_out = libra::Sclerp(dq1, dq2, 0.5); From 391f1d189eb61b9209861498e18f782ab89fff44 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 20 Dec 2022 10:45:00 +0100 Subject: [PATCH 19/54] fix comment small --- .../QuasiNonsingularOrbitalElementDifferences.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp index ec2daf81..54c9405f 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp @@ -20,14 +20,14 @@ class QuasiNonsingularOrbitalElementDifferences { /** * @fn QuasiNonsingularOrbitalElementDifferences * @brief Constructor initialized with tow quasi-nonsingular orbital elements - * @param qns_oe_reference: Quasi-nonsingular orbital elements of the reference spacecraft - * @param qns_oe_target: Quasi-nonsingular orbital elements of the target spacecraft + * @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 deconstructor + * @brief Destructor */ ~QuasiNonsingularOrbitalElementDifferences(); @@ -35,7 +35,7 @@ class QuasiNonsingularOrbitalElementDifferences { /** * @fn CalcRelativePositionCircularApprox_rtn_m * @brief Calculate the relative position of target spacecraft with circular approximation - * @params true_anomaly_rad: True anomaly [rad] + * @param [in] true_anomaly_rad: True anomaly [rad] * @return Relative position vector in RTN frame of reference spacecraft */ libra::Vector<3> CalcRelativePositionCircularApprox_rtn_m(const double true_anomaly_rad); From 8f51512c1de2c7bdb1d5150d7f97a4d2215db1e3 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 20 Dec 2022 10:52:44 +0100 Subject: [PATCH 20/54] Fix comments --- .../Orbit/QuasiNonsingularOrbitalElements.hpp | 2 +- ...uasiNonsingularRelativeOrbitalElements.hpp | 43 ++++++++++++++++++- 2 files changed, 42 insertions(+), 3 deletions(-) diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp index 6388719c..6702f2c7 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp @@ -33,7 +33,7 @@ class QuasiNonsingularOrbitalElements { QuasiNonsingularOrbitalElements(const OrbitalElements oe); /** * @fn ~QuasiNonsingularOrbitalElements - * @brief Deconstructor + * @brief Destructor */ ~QuasiNonsingularOrbitalElements(); diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp index 81175225..8cdf1fe0 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp @@ -17,20 +17,59 @@ */ 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 Destructor + */ ~QuasiNonsingularRelativeOrbitalElements(); // Calculation - libra::Vector<3> CalcRelativePositionCircularApprox_rtn_m( - const double arg_lat_rad); //!< Calculate relative position from ROE when near circular chief orbit + /** + * @fn CalcRelativePositionCircularApprox_rtn_m + * @brief Calculate the relative position of target spacecraft with circular approximation + * @param [in] arg_lat_rad: Argument of latitude [rad] + * @return Relative position vector in RTN frame of reference spacecraft + */ + libra::Vector<3> CalcRelativePositionCircularApprox_rtn_m(const double arg_lat_rad); // 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: From 8b9527a60f042f32953440efd4bf0b5a11d47ac0 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 20 Dec 2022 13:10:44 +0100 Subject: [PATCH 21/54] Add velocity calculation --- ...siNonsingularOrbitalElementDifferences.cpp | 40 ++++++++++++++++++- ...siNonsingularOrbitalElementDifferences.hpp | 12 ++++-- 2 files changed, 47 insertions(+), 5 deletions(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp index 7c834133..cf6d6e35 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp @@ -15,7 +15,7 @@ QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDiffere QuasiNonsingularOrbitalElementDifferences::~QuasiNonsingularOrbitalElementDifferences() {} -libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePositionCircularApprox_rtn_m(const double true_anomaly_rad) { +libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePositionCircularApprox_rtn_m() { // Reference orbit variables const double a = qns_oe_reference_.GetSemiMajor_m(); const double ex = qns_oe_reference_.GetEccentricityX(); @@ -36,7 +36,7 @@ libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePosition // calculation const double r = p / (1.0 + ex * cos_theta + ey * sin_theta); - const double v_r = (ex * sin_theta - ey * cos_theta); + 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); @@ -48,3 +48,39 @@ libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePosition return relative_position_rtn_m; } + +libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativeVelocityCircularApprox_rtn_m_s(const double mu) { + // 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_.GetMeanArgLatEpoch_rad(); // + some thing? + const double cos_theta = cos(theta); + const double sin_theta = sin(theta); + const double p = a * (1.0 - (ex * ex + ey * ey)); //!< Semilatus rectum + const double h = sqrt(mu * p); //!< Orbit angular momentum + + // 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_.GetMeanArgLatEpoch_rad(); + const double d_i = diff_qns_oe_.GetInclination_rad(); + const double d_raan = diff_qns_oe_.GetRaan_rad(); + + // calculation + const double r = p / (1.0 + ex * cos_theta + ey * sin_theta); + 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 index 54c9405f..a36704db 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp @@ -35,10 +35,16 @@ class QuasiNonsingularOrbitalElementDifferences { /** * @fn CalcRelativePositionCircularApprox_rtn_m * @brief Calculate the relative position of target spacecraft with circular approximation - * @param [in] true_anomaly_rad: True anomaly [rad] - * @return Relative position vector in RTN frame of reference spacecraft + * @return Relative position vector in RTN frame of reference spacecraft [m] */ - libra::Vector<3> CalcRelativePositionCircularApprox_rtn_m(const double true_anomaly_rad); + libra::Vector<3> CalcRelativePositionCircularApprox_rtn_m(); + /** + * @fn CalcRelativeVelocityCircularApprox_rtn_m_s + * @brief Calculate the relative velocity of target spacecraft with circular 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> CalcRelativeVelocityCircularApprox_rtn_m_s(const double mu_m3_s2); // Getter /** From 035ba3601a00e142f274e2b9ac089003601b2f4a Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 20 Dec 2022 14:30:22 +0100 Subject: [PATCH 22/54] Add relative OE calculation from relative position and velocity --- ...siNonsingularOrbitalElementDifferences.cpp | 85 ++++++++++++++++++- ...siNonsingularOrbitalElementDifferences.hpp | 10 +++ 2 files changed, 91 insertions(+), 4 deletions(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp index cf6d6e35..b94e71c8 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp @@ -5,6 +5,7 @@ #include "QuasiNonsingularOrbitalElementDifferences.hpp" +#include #include QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDifferences(const QuasiNonsingularOrbitalElements qns_oe_reference, @@ -13,6 +14,82 @@ QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDiffere 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_.GetMeanArgLatEpoch_rad(); // + some thing? + 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 = a * (1.0 - (ex * ex + ey * ey)); //!< Semilatus rectum + const double h = sqrt(mu_m3_s2 * p); //!< Orbit angular momentum + + // Calculation + const double r = p / (1.0 + ex * cos_theta + ey * sin_theta); + 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 current_r = r; // FIXME: Check correctness + + const double alpha = a / current_r; + const double nu = v_r / v_t; + const double rho = current_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); + + 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; + + conversion_to_oed[1][1] = 1.0 / current_r; + conversion_to_oed[1][2] = cot_i / current_r * (cos_theta + nu * sin_theta); + conversion_to_oed[1][5] = -1.0 * sin_theta * cot_i / v_t; + + conversion_to_oed[2][2] = (sin_theta - nu * cos_theta) / current_r; + conversion_to_oed[2][5] = -1.0 * cos_theta / v_t; + + conversion_to_oed[3][0] = (3.0 * cos_theta + 2.0 * nu * sin_theta) / (rho * current_r); + conversion_to_oed[3][1] = -1.0 * (nu * nu * sin_theta / rho + ex * sin_2theta - ey * cos_2theta) / current_r; + conversion_to_oed[3][2] = -1.0 * ey * cot_i * (cos_theta + nu * sin_theta) / current_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; + + conversion_to_oed[4][0] = (3.0 * cos_theta - 2.0 * nu * sin_theta) / (rho * current_r); + conversion_to_oed[4][1] = (nu * nu * cos_theta / rho + ey * sin_2theta + ex * cos_2theta) / current_r; + conversion_to_oed[4][2] = ex * cot_i * (cos_theta + nu * sin_theta) / current_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; + + conversion_to_oed[5][2] = -1.0 * (cos_theta + nu * sin_theta) / (current_r * sin_i); + conversion_to_oed[5][2] = 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::CalcRelativePositionCircularApprox_rtn_m() { @@ -34,7 +111,7 @@ libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePosition const double d_i = diff_qns_oe_.GetInclination_rad(); const double d_raan = diff_qns_oe_.GetRaan_rad(); - // calculation + // Calculation const double r = p / (1.0 + ex * cos_theta + ey * sin_theta); 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); @@ -49,7 +126,7 @@ libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePosition return relative_position_rtn_m; } -libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativeVelocityCircularApprox_rtn_m_s(const double mu) { +libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativeVelocityCircularApprox_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(); @@ -59,7 +136,7 @@ libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativeVelocity const double cos_theta = cos(theta); const double sin_theta = sin(theta); const double p = a * (1.0 - (ex * ex + ey * ey)); //!< Semilatus rectum - const double h = sqrt(mu * p); //!< Orbit angular momentum + const double h = sqrt(mu_m3_s2 * p); //!< Orbit angular momentum // Relative orbit variables const double d_a = diff_qns_oe_.GetSemiMajor_m(); @@ -69,7 +146,7 @@ libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativeVelocity const double d_i = diff_qns_oe_.GetInclination_rad(); const double d_raan = diff_qns_oe_.GetRaan_rad(); - // calculation + // Calculation const double r = p / (1.0 + ex * cos_theta + ey * sin_theta); 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); diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp index a36704db..f6dfd1b1 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp @@ -25,6 +25,16 @@ class QuasiNonsingularOrbitalElementDifferences { */ 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 From 606866f95c07eac0bbfc8984734e8861c2771c18 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 20 Dec 2022 14:32:20 +0100 Subject: [PATCH 23/54] Add comments --- .../QuasiNonsingularOrbitalElementDifferences.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp index b94e71c8..119e2544 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp @@ -47,33 +47,33 @@ QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDiffere 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 / current_r; conversion_to_oed[1][2] = cot_i / current_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) / current_r; conversion_to_oed[2][5] = -1.0 * cos_theta / v_t; - + // For eccentricity vector X conversion_to_oed[3][0] = (3.0 * cos_theta + 2.0 * nu * sin_theta) / (rho * current_r); conversion_to_oed[3][1] = -1.0 * (nu * nu * sin_theta / rho + ex * sin_2theta - ey * cos_2theta) / current_r; conversion_to_oed[3][2] = -1.0 * ey * cot_i * (cos_theta + nu * sin_theta) / current_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 * cos_theta - 2.0 * nu * sin_theta) / (rho * current_r); conversion_to_oed[4][1] = (nu * nu * cos_theta / rho + ey * sin_2theta + ex * cos_2theta) / current_r; conversion_to_oed[4][2] = ex * cot_i * (cos_theta + nu * sin_theta) / current_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) / (current_r * sin_i); conversion_to_oed[5][2] = sin_theta / (v_t * sin_i); From 1d629323bb27d8d3b7d2a867bced90c3689733e3 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 20 Dec 2022 15:04:49 +0100 Subject: [PATCH 24/54] Fix current_r --- ...siNonsingularOrbitalElementDifferences.cpp | 25 +++++++++---------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp index 119e2544..f19f6dc8 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp @@ -38,11 +38,10 @@ QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDiffere const double r = p / (1.0 + ex * cos_theta + ey * sin_theta); 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 current_r = r; // FIXME: Check correctness - const double alpha = a / current_r; + const double alpha = a / r; const double nu = v_r / v_t; - const double rho = current_r / p; + const double rho = r / p; const double kappa_1 = alpha * (1.0 / rho - 1.0); const double kappa_2 = alpha * nu * nu / rho; @@ -53,28 +52,28 @@ QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDiffere 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 / current_r; - conversion_to_oed[1][2] = cot_i / current_r * (cos_theta + nu * sin_theta); + 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) / current_r; + conversion_to_oed[2][2] = (sin_theta - nu * cos_theta) / r; conversion_to_oed[2][5] = -1.0 * cos_theta / v_t; // For eccentricity vector X - conversion_to_oed[3][0] = (3.0 * cos_theta + 2.0 * nu * sin_theta) / (rho * current_r); - conversion_to_oed[3][1] = -1.0 * (nu * nu * sin_theta / rho + ex * sin_2theta - ey * cos_2theta) / current_r; - conversion_to_oed[3][2] = -1.0 * ey * cot_i * (cos_theta + nu * sin_theta) / current_r; + 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 * cos_theta - 2.0 * nu * sin_theta) / (rho * current_r); - conversion_to_oed[4][1] = (nu * nu * cos_theta / rho + ey * sin_2theta + ex * cos_2theta) / current_r; - conversion_to_oed[4][2] = ex * cot_i * (cos_theta + nu * sin_theta) / current_r; + conversion_to_oed[4][0] = (3.0 * cos_theta - 2.0 * nu * sin_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) / (current_r * sin_i); + conversion_to_oed[5][2] = -1.0 * (cos_theta + nu * sin_theta) / (r * sin_i); conversion_to_oed[5][2] = sin_theta / (v_t * sin_i); // Output From 3569db13f473587182073632c243457d631c6d09 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 20 Dec 2022 21:17:44 +0100 Subject: [PATCH 25/54] Add orbit constants calculation --- .../Library/Orbit/QuasiNonsingularOrbitalElements.cpp | 11 ++++++++++- .../Library/Orbit/QuasiNonsingularOrbitalElements.hpp | 10 ++++++++++ 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp index 73439e51..b5ccc45f 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp @@ -12,6 +12,7 @@ QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements() { eccentricity_y_ = 0.0; inclination_rad_ = 0.0; raan_rad_ = 0.0; + CalcOrbitConstants(); } QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const double semi_major_axis_m, const double eccentricity_x, @@ -22,7 +23,9 @@ QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const double se eccentricity_y_(eccentricity_y), inclination_rad_(inclination_rad), raan_rad_(raan_rad), - mean_arg_latitude_epoch_rad_(mean_arg_latitude_epoch_rad) {} + mean_arg_latitude_epoch_rad_(mean_arg_latitude_epoch_rad) { + CalcOrbitConstants(); +} QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const OrbitalElements oe) { double mean_anomaly_rad = 0.0; // since the epoch is the perigee pass time @@ -33,6 +36,8 @@ QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const OrbitalEl eccentricity_y_ = oe.GetEccentricity() * sin(oe.GetArgPerigee()); inclination_rad_ = oe.GetInclination(); raan_rad_ = oe.GetRaan(); + + CalcOrbitConstants(); } QuasiNonsingularOrbitalElements ::~QuasiNonsingularOrbitalElements() {} @@ -49,3 +54,7 @@ QuasiNonsingularOrbitalElements operator-(const QuasiNonsingularOrbitalElements return out; } + +void QuasiNonsingularOrbitalElements::CalcOrbitConstants() { + semi_latus_rectum_ = semi_major_axis_m_ * (1.0 - (eccentricity_x_ * eccentricity_x_ + eccentricity_y_ * eccentricity_y_)); +} diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp index 6702f2c7..fcd8129b 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp @@ -76,6 +76,16 @@ class QuasiNonsingularOrbitalElements { double inclination_rad_; //!< Inclination [rad] double raan_rad_; //!< Right Ascension of the Ascending Node [rad] double mean_arg_latitude_epoch_rad_; //!< Mean argument of Latitude at epoch (arg_peri + mean anomaly) [rad] + + // Orbit states + double semi_latus_rectum_; //!< Semi-latus rectum [m] + double radius_; //!< Current radius [m] + + /** + * @fn CalcOrbitConstants + * @brief Calculate constant values for orbit + */ + void CalcOrbitConstants(); }; /** From 3027ed694d4ce4556738581081711b6a706ead1e Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 20 Dec 2022 21:26:52 +0100 Subject: [PATCH 26/54] Fix mean anomaly to true anomaly --- .../Orbit/QuasiNonsingularOrbitalElements.cpp | 10 +++++----- .../Orbit/QuasiNonsingularOrbitalElements.hpp | 18 +++++++++--------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp index b5ccc45f..63c46780 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp @@ -7,7 +7,7 @@ QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements() { semi_major_axis_m_ = 0.0; - mean_arg_latitude_epoch_rad_ = 0.0; + true_latitude_angle_epoch_rad_ = 0.0; eccentricity_x_ = 0.0; eccentricity_y_ = 0.0; inclination_rad_ = 0.0; @@ -23,15 +23,15 @@ QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const double se eccentricity_y_(eccentricity_y), inclination_rad_(inclination_rad), raan_rad_(raan_rad), - mean_arg_latitude_epoch_rad_(mean_arg_latitude_epoch_rad) { + true_latitude_angle_epoch_rad_(mean_arg_latitude_epoch_rad) { CalcOrbitConstants(); } QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const OrbitalElements oe) { - double mean_anomaly_rad = 0.0; // since the epoch is the perigee pass time + double true_anomaly_rad = 0.0; // since the epoch is the perigee pass time semi_major_axis_m_ = oe.GetSemiMajor(); - mean_arg_latitude_epoch_rad_ = oe.GetArgPerigee() + mean_anomaly_rad; + true_latitude_angle_epoch_rad_ = oe.GetArgPerigee() + true_anomaly_rad; eccentricity_x_ = oe.GetEccentricity() * cos(oe.GetArgPerigee()); eccentricity_y_ = oe.GetEccentricity() * sin(oe.GetArgPerigee()); inclination_rad_ = oe.GetInclination(); @@ -48,7 +48,7 @@ QuasiNonsingularOrbitalElements operator-(const QuasiNonsingularOrbitalElements 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 mean_arg_latitude_epoch_rad = lhs.GetMeanArgLatEpoch_rad() - lhs.GetMeanArgLatEpoch_rad(); + double mean_arg_latitude_epoch_rad = lhs.GetTrueLatAngEpoch_rad() - lhs.GetTrueLatAngEpoch_rad(); QuasiNonsingularOrbitalElements out(semi_major_axis_m, eccentricity_x, eccentricity_y, inclination_rad, raan_rad, mean_arg_latitude_epoch_rad); diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp index fcd8129b..a3ba2420 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp @@ -24,7 +24,7 @@ class QuasiNonsingularOrbitalElements { * @brief Constructor initialized with values */ 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_epoch_rad); + const double inclination_rad, const double raan_rad, const double true_latitude_angle_epoch_rad); /** * @fn QuasiNonsingularOrbitalElements * @brief Constructor initialized with orbital elements @@ -65,17 +65,17 @@ class QuasiNonsingularOrbitalElements { inline double GetRaan_rad() const { return raan_rad_; } /** * @fn GetMeanArgLatEpoch_rad - * @brief Return Mean argument of Latitude at epoch [rad] + * @brief Return True latitude angle at epoch [rad] */ - inline double GetMeanArgLatEpoch_rad() const { return mean_arg_latitude_epoch_rad_; } + inline double GetTrueLatAngEpoch_rad() const { return true_latitude_angle_epoch_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 mean_arg_latitude_epoch_rad_; //!< Mean argument of Latitude at epoch (arg_peri + mean anomaly) [rad] + 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_epoch_rad_; //!< True latitude angle at epoch (arg_peri + true anomaly) [rad] // Orbit states double semi_latus_rectum_; //!< Semi-latus rectum [m] From bc7a6af279b5b31ac679bcb55e4fc065db44b27b Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 20 Dec 2022 21:27:24 +0100 Subject: [PATCH 27/54] fix bug --- s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp index 63c46780..b2ef15bc 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp @@ -48,7 +48,7 @@ QuasiNonsingularOrbitalElements operator-(const QuasiNonsingularOrbitalElements 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 mean_arg_latitude_epoch_rad = lhs.GetTrueLatAngEpoch_rad() - lhs.GetTrueLatAngEpoch_rad(); + double mean_arg_latitude_epoch_rad = lhs.GetTrueLatAngEpoch_rad() - rhs.GetTrueLatAngEpoch_rad(); QuasiNonsingularOrbitalElements out(semi_major_axis_m, eccentricity_x, eccentricity_y, inclination_rad, raan_rad, mean_arg_latitude_epoch_rad); From 41f8c5763ac13d0f8364fcb4851b2f65dde5b5b7 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 20 Dec 2022 22:10:52 +0100 Subject: [PATCH 28/54] Modify QNOE to use time variable theta --- .../Orbit/QuasiNonsingularOrbitalElements.cpp | 21 ++++------------ .../Orbit/QuasiNonsingularOrbitalElements.hpp | 24 +++++++------------ ...siNonsingularOrbitalElementDifferences.cpp | 10 ++++---- ...uasiNonsingularRelativeOrbitalElements.cpp | 2 +- 4 files changed, 19 insertions(+), 38 deletions(-) diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp index b2ef15bc..87234754 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp @@ -7,7 +7,7 @@ QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements() { semi_major_axis_m_ = 0.0; - true_latitude_angle_epoch_rad_ = 0.0; + true_latitude_angle_rad_ = 0.0; eccentricity_x_ = 0.0; eccentricity_y_ = 0.0; inclination_rad_ = 0.0; @@ -17,26 +17,13 @@ QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements() { 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_epoch_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_epoch_rad_(mean_arg_latitude_epoch_rad) { - CalcOrbitConstants(); -} - -QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const OrbitalElements oe) { - double true_anomaly_rad = 0.0; // since the epoch is the perigee pass time - - semi_major_axis_m_ = oe.GetSemiMajor(); - true_latitude_angle_epoch_rad_ = oe.GetArgPerigee() + true_anomaly_rad; - eccentricity_x_ = oe.GetEccentricity() * cos(oe.GetArgPerigee()); - eccentricity_y_ = oe.GetEccentricity() * sin(oe.GetArgPerigee()); - inclination_rad_ = oe.GetInclination(); - raan_rad_ = oe.GetRaan(); - + true_latitude_angle_rad_(mean_arg_latitude_rad) { CalcOrbitConstants(); } @@ -48,7 +35,7 @@ QuasiNonsingularOrbitalElements operator-(const QuasiNonsingularOrbitalElements 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 mean_arg_latitude_epoch_rad = lhs.GetTrueLatAngEpoch_rad() - rhs.GetTrueLatAngEpoch_rad(); + double mean_arg_latitude_epoch_rad = lhs.GetTrueLatAng_rad() - rhs.GetTrueLatAng_rad(); QuasiNonsingularOrbitalElements out(semi_major_axis_m, eccentricity_x, eccentricity_y, inclination_rad, raan_rad, mean_arg_latitude_epoch_rad); diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp index a3ba2420..8c7f61d5 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp @@ -24,13 +24,7 @@ class QuasiNonsingularOrbitalElements { * @brief Constructor initialized with values */ 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_epoch_rad); - /** - * @fn QuasiNonsingularOrbitalElements - * @brief Constructor initialized with orbital elements - * @param oe: Orbital Elements - */ - QuasiNonsingularOrbitalElements(const OrbitalElements oe); + const double inclination_rad, const double raan_rad, const double true_latitude_angle_rad); /** * @fn ~QuasiNonsingularOrbitalElements * @brief Destructor @@ -65,17 +59,17 @@ class QuasiNonsingularOrbitalElements { inline double GetRaan_rad() const { return raan_rad_; } /** * @fn GetMeanArgLatEpoch_rad - * @brief Return True latitude angle at epoch [rad] + * @brief Return True latitude angle [rad] */ - inline double GetTrueLatAngEpoch_rad() const { return true_latitude_angle_epoch_rad_; } + inline double GetTrueLatAng_rad() const { return 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_epoch_rad_; //!< True latitude angle at epoch (arg_peri + true anomaly) [rad] + 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 (arg_peri + true anomaly) [rad] // Orbit states double semi_latus_rectum_; //!< Semi-latus rectum [m] diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp index f19f6dc8..ff8da6c1 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp @@ -26,7 +26,7 @@ QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDiffere 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_.GetMeanArgLatEpoch_rad(); // + some thing? + 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); @@ -97,7 +97,7 @@ libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePosition 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_.GetMeanArgLatEpoch_rad(); // + some thing? + const double theta = qns_oe_reference_.GetTrueLatAng_rad(); const double cos_theta = cos(theta); const double sin_theta = sin(theta); const double p = a * (1.0 - (ex * ex + ey * ey)); //!< Semilatus rectum @@ -106,7 +106,7 @@ libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePosition 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_.GetMeanArgLatEpoch_rad(); + 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(); @@ -131,7 +131,7 @@ libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativeVelocity 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_.GetMeanArgLatEpoch_rad(); // + some thing? + const double theta = qns_oe_reference_.GetTrueLatAng_rad(); const double cos_theta = cos(theta); const double sin_theta = sin(theta); const double p = a * (1.0 - (ex * ex + ey * ey)); //!< Semilatus rectum @@ -141,7 +141,7 @@ libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativeVelocity 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_.GetMeanArgLatEpoch_rad(); + 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(); diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp index 41985f56..cb27c8be 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp @@ -15,7 +15,7 @@ QuasiNonsingularRelativeOrbitalElements::QuasiNonsingularRelativeOrbitalElements d_semi_major_axis_ = (qns_oe_target.GetSemiMajor_m() - semi_major_axis_ref_m_) / semi_major_axis_ref_m_; d_mean_longitude_ = - (qns_oe_target.GetMeanArgLatEpoch_rad() - qns_oe_reference.GetMeanArgLatEpoch_rad()) + diff_raan * cos(qns_oe_reference.GetInclination_rad()); + (qns_oe_target.GetTrueLatAng_rad() - qns_oe_reference.GetTrueLatAng_rad()) + diff_raan * cos(qns_oe_reference.GetInclination_rad()); d_eccentricity_x_ = qns_oe_target.GetEccentricityX() - qns_oe_reference.GetEccentricityX(); d_eccentricity_y_ = qns_oe_target.GetEccentricityY() - qns_oe_reference.GetEccentricityY(); From 040260a5ee0e977622ff8d1607a7aa6a15c44b6f Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 20 Dec 2022 22:20:21 +0100 Subject: [PATCH 29/54] Modify orbit parameter calculation --- .../Orbit/QuasiNonsingularOrbitalElements.cpp | 13 ++++++----- .../Orbit/QuasiNonsingularOrbitalElements.hpp | 22 ++++++++++++++----- 2 files changed, 23 insertions(+), 12 deletions(-) diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp index 87234754..545b6537 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp @@ -12,7 +12,7 @@ QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements() { eccentricity_y_ = 0.0; inclination_rad_ = 0.0; raan_rad_ = 0.0; - CalcOrbitConstants(); + CalcOrbitParameters(); } QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const double semi_major_axis_m, const double eccentricity_x, @@ -24,7 +24,7 @@ QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const double se inclination_rad_(inclination_rad), raan_rad_(raan_rad), true_latitude_angle_rad_(mean_arg_latitude_rad) { - CalcOrbitConstants(); + CalcOrbitParameters(); } QuasiNonsingularOrbitalElements ::~QuasiNonsingularOrbitalElements() {} @@ -35,13 +35,14 @@ QuasiNonsingularOrbitalElements operator-(const QuasiNonsingularOrbitalElements 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 mean_arg_latitude_epoch_rad = lhs.GetTrueLatAng_rad() - rhs.GetTrueLatAng_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, mean_arg_latitude_epoch_rad); + QuasiNonsingularOrbitalElements out(semi_major_axis_m, eccentricity_x, eccentricity_y, inclination_rad, raan_rad, true_latitude_angle_rad); return out; } -void QuasiNonsingularOrbitalElements::CalcOrbitConstants() { - semi_latus_rectum_ = semi_major_axis_m_ * (1.0 - (eccentricity_x_ * eccentricity_x_ + eccentricity_y_ * eccentricity_y_)); +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 index 8c7f61d5..1075fac6 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp @@ -58,10 +58,20 @@ class QuasiNonsingularOrbitalElements { */ inline double GetRaan_rad() const { return raan_rad_; } /** - * @fn GetMeanArgLatEpoch_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_; } private: double semi_major_axis_m_; //!< Semi major axis [m] @@ -72,14 +82,14 @@ class QuasiNonsingularOrbitalElements { double true_latitude_angle_rad_; //!< True latitude angle (arg_peri + true anomaly) [rad] // Orbit states - double semi_latus_rectum_; //!< Semi-latus rectum [m] - double radius_; //!< Current radius [m] + double semi_latus_rectum_m_; //!< Semi-latus rectum [m] + double radius_m_; //!< Current radius [m] /** - * @fn CalcOrbitConstants - * @brief Calculate constant values for orbit + * @fn CalcOrbitParameters + * @brief Calculate parameters for orbit */ - void CalcOrbitConstants(); + void CalcOrbitParameters(); }; /** From f2810dd46e3d573226e775f407f4a426a6f2ec7f Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 20 Dec 2022 22:24:27 +0100 Subject: [PATCH 30/54] Fix to use calculated parameter of reference orbit --- ...QuasiNonsingularOrbitalElementDifferences.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp index ff8da6c1..2e011548 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp @@ -31,11 +31,11 @@ QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDiffere 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 = a * (1.0 - (ex * ex + ey * ey)); //!< Semilatus rectum - const double h = sqrt(mu_m3_s2 * p); //!< Orbit angular momentum + 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 r = p / (1.0 + ex * cos_theta + ey * sin_theta); 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); @@ -100,7 +100,8 @@ libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePosition const double theta = qns_oe_reference_.GetTrueLatAng_rad(); const double cos_theta = cos(theta); const double sin_theta = sin(theta); - const double p = a * (1.0 - (ex * ex + ey * ey)); //!< Semilatus rectum + 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(); @@ -111,7 +112,6 @@ libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePosition const double d_raan = diff_qns_oe_.GetRaan_rad(); // Calculation - const double r = p / (1.0 + ex * cos_theta + ey * sin_theta); 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); @@ -134,8 +134,9 @@ libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativeVelocity const double theta = qns_oe_reference_.GetTrueLatAng_rad(); const double cos_theta = cos(theta); const double sin_theta = sin(theta); - const double p = a * (1.0 - (ex * ex + ey * ey)); //!< Semilatus rectum - const double h = sqrt(mu_m3_s2 * p); //!< Orbit angular momentum + 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(); @@ -146,7 +147,6 @@ libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativeVelocity const double d_raan = diff_qns_oe_.GetRaan_rad(); // Calculation - const double r = p / (1.0 + ex * cos_theta + ey * sin_theta); 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); From fb8baaca56bce58eefec8aac5ec0808a1d3ea409 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 20 Dec 2022 22:32:22 +0100 Subject: [PATCH 31/54] fix bug --- .../RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp index 2e011548..361cc672 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp @@ -74,7 +74,7 @@ QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDiffere 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][2] = sin_theta / (v_t * sin_i); + conversion_to_oed[5][5] = sin_theta / (v_t * sin_i); // Output libra::Vector<6> position_and_velocity; From 245213e48411dbc3cf2908aedc1c1587bce53766 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 20 Dec 2022 23:09:18 +0100 Subject: [PATCH 32/54] Add constructor for pos/vel initialization --- .../Orbit/QuasiNonsingularOrbitalElements.cpp | 48 +++++++++++++++++++ .../Orbit/QuasiNonsingularOrbitalElements.hpp | 10 +++- 2 files changed, 57 insertions(+), 1 deletion(-) diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp index 545b6537..9b435b02 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp @@ -5,6 +5,8 @@ #include "QuasiNonsingularOrbitalElements.hpp" +#include + QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements() { semi_major_axis_m_ = 0.0; true_latitude_angle_rad_ = 0.0; @@ -27,6 +29,52 @@ QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const double se 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]); + + // 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])); + } + + // 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); + + CalcOrbitParameters(); +} + QuasiNonsingularOrbitalElements ::~QuasiNonsingularOrbitalElements() {} QuasiNonsingularOrbitalElements operator-(const QuasiNonsingularOrbitalElements lhs, const QuasiNonsingularOrbitalElements rhs) { diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp index 1075fac6..0ce75da5 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp @@ -6,7 +6,7 @@ #ifndef QUASI_NONSINGULAR_ORBITAL_ELEMENTS_H_ #define QUASI_NONSINGULAR_ORBITAL_ELEMENTS_H_ -#include +#include /** * @class QuasiNonsingularOrbitalElements @@ -25,6 +25,14 @@ class 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 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 From d89626b8bf042d4d5020958ed4b6ee410838743d Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 20 Dec 2022 23:17:02 +0100 Subject: [PATCH 33/54] Fix comment --- .../src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp index 0ce75da5..918995c3 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp @@ -22,6 +22,12 @@ class 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); @@ -87,7 +93,7 @@ class QuasiNonsingularOrbitalElements { 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 (arg_peri + true anomaly) [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] From 10d2385bbf5db877c3d2461ced92b71a0fbce9d4 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 21 Dec 2022 10:04:51 +0100 Subject: [PATCH 34/54] Add test for QN OE --- s2e-ff/CMakeLists.txt | 8 ++++++-- .../test/TestQuasiNonsingularOrbitalElements.cpp | 16 ++++++++++++++++ 2 files changed, 22 insertions(+), 2 deletions(-) create mode 100644 s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp diff --git a/s2e-ff/CMakeLists.txt b/s2e-ff/CMakeLists.txt index 310ff6bd..ca0c9dd9 100644 --- a/s2e-ff/CMakeLists.txt +++ b/s2e-ff/CMakeLists.txt @@ -9,8 +9,8 @@ project(S2E_FF cmake_minimum_required(VERSION 3.13) # build config -option(BUILD_64BIT "Build 64bit" OFF) -option(GOOGLE_TEST "Execute GoogleTest" OFF) +option(BUILD_64BIT "Build 64bit" ON) +option(GOOGLE_TEST "Execute GoogleTest" ON) if (NOT BUILD_64BIT) option(GOOGLE_TEST OFF) # GoogleTest supports 64bit only endif() @@ -202,12 +202,16 @@ 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 ) add_executable(${TEST_PROJECT_NAME} ${TEST_FILES}) target_link_libraries(${TEST_PROJECT_NAME} gtest gtest_main) diff --git a/s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp b/s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp new file mode 100644 index 00000000..9f86fb15 --- /dev/null +++ b/s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp @@ -0,0 +1,16 @@ +#include + +#include "../src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp" + +TEST(QuasiNonsingularOrbitalElement, DefaultConstructor) { + QuasiNonsingularOrbitalElements qn_oe; + + 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()); + EXPECT_DOUBLE_EQ(0.0, qn_oe.GetSemiLatusRectum_m()); + EXPECT_DOUBLE_EQ(0.0, qn_oe.GetRadius_m()); +} From d09c5ae86d4ec18074778bc1d8c8b701c98e4ff0 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 21 Dec 2022 11:25:10 +0100 Subject: [PATCH 35/54] Add test for QN OE constructor --- .../Orbit/QuasiNonsingularOrbitalElements.cpp | 4 ++ .../TestQuasiNonsingularOrbitalElements.cpp | 69 +++++++++++++++++++ 2 files changed, 73 insertions(+) diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp index 9b435b02..ac178e11 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp @@ -5,6 +5,7 @@ #include "QuasiNonsingularOrbitalElements.hpp" +#include #include QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements() { @@ -45,6 +46,7 @@ QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const double mu 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]); @@ -54,6 +56,7 @@ QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const double mu } 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_); @@ -71,6 +74,7 @@ QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements(const double mu // 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(); } diff --git a/s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp b/s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp index 9f86fb15..e340bac2 100644 --- a/s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp +++ b/s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp @@ -5,12 +5,81 @@ TEST(QuasiNonsingularOrbitalElement, 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(QuasiNonsingularOrbitalElement, 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(QuasiNonsingularOrbitalElement, 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(QuasiNonsingularOrbitalElement, 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); +} From 6b8503753e1d943ba6690362254b5ef63b8e8161 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 21 Dec 2022 13:12:37 +0100 Subject: [PATCH 36/54] Add test QN ED subtract --- .../TestQuasiNonsingularOrbitalElements.cpp | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp b/s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp index e340bac2..25fe010e 100644 --- a/s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp +++ b/s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp @@ -83,3 +83,33 @@ TEST(QuasiNonsingularOrbitalElement, ConstructorWithPositionVelocity) { EXPECT_NEAR(6.8993218223e6, qn_oe.GetSemiLatusRectum_m(), 1e-1); EXPECT_NEAR(r_norm_m, qn_oe.GetRadius_m(), 1e-1); } + +TEST(QuasiNonsingularOrbitalElement, 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); +} From 3eb4421da6726ab9b2badcad6a1110b2ea2484da Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 21 Dec 2022 13:26:33 +0100 Subject: [PATCH 37/54] Add test for QZ OED --- s2e-ff/CMakeLists.txt | 2 ++ ...siNonsingularOrbitalElementDifferences.cpp | 34 +++++++++++++++++++ 2 files changed, 36 insertions(+) create mode 100644 s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp diff --git a/s2e-ff/CMakeLists.txt b/s2e-ff/CMakeLists.txt index ca0c9dd9..48fccdfd 100644 --- a/s2e-ff/CMakeLists.txt +++ b/s2e-ff/CMakeLists.txt @@ -212,6 +212,8 @@ if(GOOGLE_TEST) # Orbital elements src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp test/TestQuasiNonsingularOrbitalElements.cpp + src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp + test/TestQuasiNonsingularOrbitalElementDifferences.cpp ) add_executable(${TEST_PROJECT_NAME} ${TEST_FILES}) target_link_libraries(${TEST_PROJECT_NAME} gtest gtest_main) diff --git a/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp new file mode 100644 index 00000000..2cc6edf3 --- /dev/null +++ b/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp @@ -0,0 +1,34 @@ +#include + +#include "../src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp" + +TEST(QuasiNonsingularOrbitalElementDifference, Constructor) { + // lhs + 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); + // rhs + 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); +} From 252b1a9df8daab95f89fd9542f212b356479e598 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 21 Dec 2022 13:32:29 +0100 Subject: [PATCH 38/54] Fix test name --- .../TestQuasiNonsingularOrbitalElementDifferences.cpp | 2 +- s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp index 2cc6edf3..707c9903 100644 --- a/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp +++ b/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp @@ -2,7 +2,7 @@ #include "../src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp" -TEST(QuasiNonsingularOrbitalElementDifference, Constructor) { +TEST(QuasiNonsingularOrbitalElementDifferences, ConstructorWithOe) { // lhs const double reference_semi_major_axis_m = 6896e3; const double reference_eccentricity_x = 0.0; // Test singular point diff --git a/s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp b/s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp index 25fe010e..d1732b3e 100644 --- a/s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp +++ b/s2e-ff/test/TestQuasiNonsingularOrbitalElements.cpp @@ -2,7 +2,7 @@ #include "../src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp" -TEST(QuasiNonsingularOrbitalElement, DefaultConstructor) { +TEST(QuasiNonsingularOrbitalElements, DefaultConstructor) { QuasiNonsingularOrbitalElements qn_oe; // OEs @@ -17,7 +17,7 @@ TEST(QuasiNonsingularOrbitalElement, DefaultConstructor) { EXPECT_DOUBLE_EQ(0.0, qn_oe.GetRadius_m()); } -TEST(QuasiNonsingularOrbitalElement, ConstructorWithSingularOE) { +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 @@ -38,7 +38,7 @@ TEST(QuasiNonsingularOrbitalElement, ConstructorWithSingularOE) { EXPECT_DOUBLE_EQ(semi_major_axis_m, qn_oe.GetRadius_m()); } -TEST(QuasiNonsingularOrbitalElement, ConstructorWithOE) { +TEST(QuasiNonsingularOrbitalElements, ConstructorWithOe) { const double semi_major_axis_m = 6896e3; const double eccentricity_x = 0.05; const double eccentricity_y = 0.03; @@ -59,7 +59,7 @@ TEST(QuasiNonsingularOrbitalElement, ConstructorWithOE) { EXPECT_NEAR(6494189.8, qn_oe.GetRadius_m(), 1e-1); } -TEST(QuasiNonsingularOrbitalElement, ConstructorWithPositionVelocity) { +TEST(QuasiNonsingularOrbitalElements, ConstructorWithPositionVelocity) { const double mu_m3_s2 = 3.986008e14; libra::Vector<3> position_i_m; position_i_m[0] = -5659121.225; @@ -84,7 +84,7 @@ TEST(QuasiNonsingularOrbitalElement, ConstructorWithPositionVelocity) { EXPECT_NEAR(r_norm_m, qn_oe.GetRadius_m(), 1e-1); } -TEST(QuasiNonsingularOrbitalElement, Subtract) { +TEST(QuasiNonsingularOrbitalElements, Subtract) { // lhs const double lhs_semi_major_axis_m = 6896e3; const double lhs_eccentricity_x = 0.0; // Test singular point From 8e2b24786751834fdc12a908399c3e2891649def Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 21 Dec 2022 14:33:55 +0100 Subject: [PATCH 39/54] Add test for NS OED position and velocity calculation --- ...siNonsingularOrbitalElementDifferences.cpp | 6 ++-- ...siNonsingularOrbitalElementDifferences.cpp | 34 +++++++++++++++++++ 2 files changed, 37 insertions(+), 3 deletions(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp index 361cc672..96ee6e91 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp @@ -57,7 +57,7 @@ QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDiffere 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] = -1.0 * cos_theta / v_t; + 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; @@ -66,7 +66,7 @@ QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDiffere 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 * cos_theta - 2.0 * nu * sin_theta) / (rho * r); + 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); @@ -154,7 +154,7 @@ libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativeVelocity 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 + + 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; diff --git a/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp index 707c9903..93c937d6 100644 --- a/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp +++ b/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp @@ -32,3 +32,37 @@ TEST(QuasiNonsingularOrbitalElementDifferences, ConstructorWithOe) { 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.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 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.CalcRelativePositionCircularApprox_rtn_m(); + libra::Vector<3> calc_velocity_rtn_m_s = qn_oe.CalcRelativeVelocityCircularApprox_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); +} From 7a26c881a8ee334b9db98059dbf1e99c8653df43 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 21 Dec 2022 14:36:14 +0100 Subject: [PATCH 40/54] Fix test value --- .../TestQuasiNonsingularOrbitalElementDifferences.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp index 93c937d6..eb381344 100644 --- a/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp +++ b/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp @@ -3,7 +3,7 @@ #include "../src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp" TEST(QuasiNonsingularOrbitalElementDifferences, ConstructorWithOe) { - // lhs + // 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 @@ -12,7 +12,7 @@ TEST(QuasiNonsingularOrbitalElementDifferences, ConstructorWithOe) { 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); - // rhs + // target const double target_semi_major_axis_m = 6896e3; const double target_eccentricity_x = 0.05; const double target_eccentricity_y = 0.03; @@ -37,8 +37,8 @@ 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.0; // Test singular point - const double reference_eccentricity_y = 0.0; // Test singular point + 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; From 3afdf42e8ccd39472151535f107049191283ea72 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Wed, 21 Dec 2022 14:48:07 +0100 Subject: [PATCH 41/54] Add NS OE setter --- .../src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp | 1 - .../src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp | 8 ++++++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp index ac178e11..2d6b02f6 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.cpp @@ -5,7 +5,6 @@ #include "QuasiNonsingularOrbitalElements.hpp" -#include #include QuasiNonsingularOrbitalElements::QuasiNonsingularOrbitalElements() { diff --git a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp index 918995c3..30d70aa7 100644 --- a/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp +++ b/s2e-ff/src/Library/Orbit/QuasiNonsingularOrbitalElements.hpp @@ -7,6 +7,7 @@ #define QUASI_NONSINGULAR_ORBITAL_ELEMENTS_H_ #include +#include /** * @class QuasiNonsingularOrbitalElements @@ -87,6 +88,13 @@ class QuasiNonsingularOrbitalElements { */ 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) From 9575d95dcde9537cec86692f4a68ff9a29c98b05 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 26 Dec 2022 20:35:21 +0100 Subject: [PATCH 42/54] Fix function name --- .../QuasiNonsingularOrbitalElementDifferences.cpp | 4 ++-- .../QuasiNonsingularOrbitalElementDifferences.hpp | 12 ++++++------ ...TestQuasiNonsingularOrbitalElementDifferences.cpp | 4 ++-- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp index 96ee6e91..27bedce0 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.cpp @@ -91,7 +91,7 @@ QuasiNonsingularOrbitalElementDifferences::QuasiNonsingularOrbitalElementDiffere QuasiNonsingularOrbitalElementDifferences::~QuasiNonsingularOrbitalElementDifferences() {} -libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePositionCircularApprox_rtn_m() { +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(); @@ -125,7 +125,7 @@ libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativePosition return relative_position_rtn_m; } -libra::Vector<3> QuasiNonsingularOrbitalElementDifferences::CalcRelativeVelocityCircularApprox_rtn_m_s(const double mu_m3_s2) { +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(); diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp index f6dfd1b1..21e202ca 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularOrbitalElementDifferences.hpp @@ -43,18 +43,18 @@ class QuasiNonsingularOrbitalElementDifferences { // Calculation /** - * @fn CalcRelativePositionCircularApprox_rtn_m - * @brief Calculate the relative position of target spacecraft with circular approximation + * @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> CalcRelativePositionCircularApprox_rtn_m(); + libra::Vector<3> CalcRelativePosition_rtn_m(); /** - * @fn CalcRelativeVelocityCircularApprox_rtn_m_s - * @brief Calculate the relative velocity of target spacecraft with circular approximation + * @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> CalcRelativeVelocityCircularApprox_rtn_m_s(const double mu_m3_s2); + libra::Vector<3> CalcRelativeVelocity_rtn_m_s(const double mu_m3_s2); // Getter /** diff --git a/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp b/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp index eb381344..d0feb5de 100644 --- a/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp +++ b/s2e-ff/test/TestQuasiNonsingularOrbitalElementDifferences.cpp @@ -55,8 +55,8 @@ TEST(QuasiNonsingularOrbitalElementDifferences, ConstructorWithPositionVelocity) 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.CalcRelativePositionCircularApprox_rtn_m(); - libra::Vector<3> calc_velocity_rtn_m_s = qn_oe.CalcRelativeVelocityCircularApprox_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); From 21e608d31dfd95ea0031072dc1e4beca3284f5a3 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Mon, 26 Dec 2022 22:23:57 +0100 Subject: [PATCH 43/54] Add calculation of mean argument of latitude --- ...uasiNonsingularRelativeOrbitalElements.cpp | 46 +++++++++++++++++-- ...uasiNonsingularRelativeOrbitalElements.hpp | 10 ++++ 2 files changed, 51 insertions(+), 5 deletions(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp index cb27c8be..d80bdb71 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp @@ -9,18 +9,20 @@ QuasiNonsingularRelativeOrbitalElements::QuasiNonsingularRelativeOrbitalElements(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target) { - double diff_raan = qns_oe_target.GetRaan_rad() - qns_oe_reference.GetRaan_rad(); - 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_mean_longitude_ = - (qns_oe_target.GetTrueLatAng_rad() - qns_oe_reference.GetTrueLatAng_rad()) + diff_raan * cos(qns_oe_reference.GetInclination_rad()); - 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() {} @@ -38,3 +40,37 @@ libra::Vector<3> QuasiNonsingularRelativeOrbitalElements::CalcRelativePositionCi relative_position_rtn_m *= semi_major_axis_ref_m_; return relative_position_rtn_m; } + +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 sin_f = (q1 * sin_theta - q2 * cos_theta) / e; + + const double denominator = (1.0 + e_cos_f) * (1.0 + e_cos_f); + + // Difference Info + 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 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_eccentricity = 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_eccentricity); + 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 index 8cdf1fe0..607db4c3 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp @@ -83,6 +83,16 @@ class QuasiNonsingularRelativeOrbitalElements { 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 Calaculate 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 argment of latitude [rad] + */ + double CalcDiffMeanArgLat_rad(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target); }; #endif From 1189a1d25fef851a0e30a300875979f2726052ee Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 27 Dec 2022 07:59:40 +0100 Subject: [PATCH 44/54] Fix variables name --- .../RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp index d80bdb71..06b7ca5c 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp @@ -69,8 +69,8 @@ double QuasiNonsingularRelativeOrbitalElements::CalcDiffMeanArgLat_rad(const Qua const double q2_target = qns_oe_reference.GetEccentricityY(); const double e_target = sqrt(q1_target * q1_target + q2_target * q2_target); - const double d_eccentricity = e_target - e; + 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_eccentricity); + 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; } From ee49393004b580836fc1d9a79306edfc73cffddf Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 27 Dec 2022 08:03:18 +0100 Subject: [PATCH 45/54] Fix typo --- .../RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp index 607db4c3..794a938c 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp @@ -86,7 +86,7 @@ class QuasiNonsingularRelativeOrbitalElements { /** * @fn CalcDiffMeanArgLat_rad - * @brief Calaculate difference of mean argument of latitude [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 From ec49813f1145b4c92da0448e21cfd8e7c2cca2f2 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 27 Dec 2022 08:17:30 +0100 Subject: [PATCH 46/54] Fix typo --- .../RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp index 794a938c..f0c06387 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp @@ -90,7 +90,7 @@ class QuasiNonsingularRelativeOrbitalElements { * @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 argment of latitude [rad] + * @return Difference of mean argument of latitude [rad] */ double CalcDiffMeanArgLat_rad(const QuasiNonsingularOrbitalElements qns_oe_reference, const QuasiNonsingularOrbitalElements qns_oe_target); }; From a00f2c71d57b70a26ad49078977ba17df980ef2f Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 27 Dec 2022 08:22:36 +0100 Subject: [PATCH 47/54] Fix argument name --- .../QuasiNonsingularRelativeOrbitalElements.cpp | 8 ++++---- .../QuasiNonsingularRelativeOrbitalElements.hpp | 13 ++++++++++--- 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp index 06b7ca5c..def623c8 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp @@ -27,14 +27,14 @@ QuasiNonsingularRelativeOrbitalElements::QuasiNonsingularRelativeOrbitalElements QuasiNonsingularRelativeOrbitalElements::~QuasiNonsingularRelativeOrbitalElements() {} -libra::Vector<3> QuasiNonsingularRelativeOrbitalElements::CalcRelativePositionCircularApprox_rtn_m(const double arg_lat_rad) { +libra::Vector<3> QuasiNonsingularRelativeOrbitalElements::CalcRelativePositionCircularApprox_rtn_m(const double mean_arg_lat_rad) { libra::Vector<3> relative_position_rtn_m; - double cos_u = cos(arg_lat_rad); - double sin_u = sin(arg_lat_rad); + double cos_u = cos(mean_arg_lat_rad); + 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_ * arg_lat_rad + d_mean_longitude_ + 2.0 * (d_eccentricity_x_ * sin_u - d_eccentricity_y_ * cos_u); + -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_; diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp index f0c06387..09fa59d8 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp @@ -35,10 +35,17 @@ class QuasiNonsingularRelativeOrbitalElements { /** * @fn CalcRelativePositionCircularApprox_rtn_m * @brief Calculate the relative position of target spacecraft with circular approximation - * @param [in] arg_lat_rad: Argument of latitude [rad] - * @return Relative position vector in RTN frame of reference spacecraft + * @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 arg_lat_rad); + 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] + * @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); // Getter /** From 194284d814fa5788eede35fad4842722eda7abc4 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 27 Dec 2022 08:34:34 +0100 Subject: [PATCH 48/54] Add relative velocity calculation --- .../QuasiNonsingularRelativeOrbitalElements.cpp | 17 +++++++++++++++++ .../QuasiNonsingularRelativeOrbitalElements.hpp | 3 ++- 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp index def623c8..f5fedba8 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp @@ -41,6 +41,23 @@ libra::Vector<3> QuasiNonsingularRelativeOrbitalElements::CalcRelativePositionCi 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 diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp index 09fa59d8..d190ac8f 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp @@ -43,9 +43,10 @@ class QuasiNonsingularRelativeOrbitalElements { * @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); + libra::Vector<3> CalcRelativeVelocityCircularApprox_rtn_m_s(const double mean_arg_lat_rad, const double mu_m3_s2); // Getter /** From b8e87910219e5f34e4e36cdccc06e1cff6180259 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 27 Dec 2022 08:34:58 +0100 Subject: [PATCH 49/54] Add const --- .../RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp index f5fedba8..26d39c15 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp @@ -29,8 +29,8 @@ QuasiNonsingularRelativeOrbitalElements::~QuasiNonsingularRelativeOrbitalElement libra::Vector<3> QuasiNonsingularRelativeOrbitalElements::CalcRelativePositionCircularApprox_rtn_m(const double mean_arg_lat_rad) { libra::Vector<3> relative_position_rtn_m; - double cos_u = cos(mean_arg_lat_rad); - double sin_u = sin(mean_arg_lat_rad); + 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] = From 06170173a9173865ba4afcf4ec869b3be1c69c7c Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 27 Dec 2022 10:06:29 +0100 Subject: [PATCH 50/54] Add constructor with relative position and velocity --- ...uasiNonsingularRelativeOrbitalElements.cpp | 20 +++++++++++++++++++ ...uasiNonsingularRelativeOrbitalElements.hpp | 10 ++++++++++ 2 files changed, 30 insertions(+) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp index 26d39c15..2c471f31 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp @@ -25,6 +25,26 @@ QuasiNonsingularRelativeOrbitalElements::QuasiNonsingularRelativeOrbitalElements 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_ = (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) { diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp index d190ac8f..627c1117 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.hpp @@ -25,6 +25,16 @@ class QuasiNonsingularRelativeOrbitalElements { */ 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 From d94ef906e52b91d852a7cc0e24c5f5e8ead114db Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 27 Dec 2022 10:20:53 +0100 Subject: [PATCH 51/54] Add test for RelativeOrbitalElements --- s2e-ff/CMakeLists.txt | 3 + ...uasiNonsingularRelativeOrbitalElements.cpp | 2 +- ...uasiNonsingularRelativeOrbitalElements.cpp | 60 +++++++++++++++++++ 3 files changed, 64 insertions(+), 1 deletion(-) create mode 100644 s2e-ff/test/TestQuasiNonsingularRelativeOrbitalElements.cpp diff --git a/s2e-ff/CMakeLists.txt b/s2e-ff/CMakeLists.txt index 48fccdfd..8c702a01 100644 --- a/s2e-ff/CMakeLists.txt +++ b/s2e-ff/CMakeLists.txt @@ -212,8 +212,11 @@ if(GOOGLE_TEST) # 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/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp index 2c471f31..6e396809 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp @@ -39,7 +39,7 @@ QuasiNonsingularRelativeOrbitalElements::QuasiNonsingularRelativeOrbitalElements 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_ = (relative_position_rtn_m[2]) / a; + d_inclination_y_ = (-1.0 * relative_position_rtn_m[2]) / a; // Reference info semi_major_axis_ref_m_ = a; diff --git a/s2e-ff/test/TestQuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/test/TestQuasiNonsingularRelativeOrbitalElements.cpp new file mode 100644 index 00000000..67eb5d84 --- /dev/null +++ b/s2e-ff/test/TestQuasiNonsingularRelativeOrbitalElements.cpp @@ -0,0 +1,60 @@ +#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); +} + +TEST(QuasiNonsingularRelativeOrbitalElements, 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; + + QuasiNonsingularRelativeOrbitalElements qn_oe(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_oe.CalcRelativePositionCircularApprox_rtn_m(u); + libra::Vector<3> calc_velocity_rtn_m_s = qn_oe.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); +} From 8233bb53c367ec2afbdad376c2c6fa25fe04f4fe Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 27 Dec 2022 10:23:43 +0100 Subject: [PATCH 52/54] Remove unnecessary variables --- ...TestQuasiNonsingularRelativeOrbitalElements.cpp | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/s2e-ff/test/TestQuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/test/TestQuasiNonsingularRelativeOrbitalElements.cpp index 67eb5d84..c9d53a9e 100644 --- a/s2e-ff/test/TestQuasiNonsingularRelativeOrbitalElements.cpp +++ b/s2e-ff/test/TestQuasiNonsingularRelativeOrbitalElements.cpp @@ -27,13 +27,7 @@ TEST(QuasiNonsingularRelativeOrbitalElements, 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; @@ -44,11 +38,11 @@ TEST(QuasiNonsingularRelativeOrbitalElements, ConstructorWithPositionVelocity) { velocity_rtn_m_s[1] = 0.0; velocity_rtn_m_s[2] = 0.005298; - QuasiNonsingularRelativeOrbitalElements qn_oe(reference_semi_major_axis_m, position_rtn_m, velocity_rtn_m_s, mu_m3_s2); + 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_oe.CalcRelativePositionCircularApprox_rtn_m(u); - libra::Vector<3> calc_velocity_rtn_m_s = qn_oe.CalcRelativeVelocityCircularApprox_rtn_m_s(u, mu_m3_s2); + 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); From 3d291f7a1421fc03c740605bd9175e8885ec9656 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 27 Dec 2022 10:50:31 +0100 Subject: [PATCH 53/54] Fix true anomaly calculation --- .../QuasiNonsingularRelativeOrbitalElements.cpp | 7 +++---- .../TestQuasiNonsingularRelativeOrbitalElements.cpp | 12 ++++++++++++ 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp index 6e396809..256bbdd4 100644 --- a/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp +++ b/s2e-ff/src/Library/RelativeOrbit/QuasiNonsingularRelativeOrbitalElements.cpp @@ -91,13 +91,13 @@ double QuasiNonsingularRelativeOrbitalElements::CalcDiffMeanArgLat_rad(const Qua 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 sin_f = (q1 * sin_theta - q2 * cos_theta) / e; - const double denominator = (1.0 + e_cos_f) * (1.0 + e_cos_f); - // Difference Info 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; @@ -105,7 +105,6 @@ double QuasiNonsingularRelativeOrbitalElements::CalcDiffMeanArgLat_rad(const Qua 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); diff --git a/s2e-ff/test/TestQuasiNonsingularRelativeOrbitalElements.cpp b/s2e-ff/test/TestQuasiNonsingularRelativeOrbitalElements.cpp index c9d53a9e..af597407 100644 --- a/s2e-ff/test/TestQuasiNonsingularRelativeOrbitalElements.cpp +++ b/s2e-ff/test/TestQuasiNonsingularRelativeOrbitalElements.cpp @@ -21,6 +21,18 @@ TEST(QuasiNonsingularRelativeOrbitalElements, ConstructorWithOe) { 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) { From 7ca2737b9bd21cb7e719af0de15328b699b526fd Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 27 Dec 2022 11:01:16 +0100 Subject: [PATCH 54/54] Revert default test setting --- s2e-ff/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/s2e-ff/CMakeLists.txt b/s2e-ff/CMakeLists.txt index 8c702a01..105bb5a7 100644 --- a/s2e-ff/CMakeLists.txt +++ b/s2e-ff/CMakeLists.txt @@ -9,8 +9,8 @@ project(S2E_FF cmake_minimum_required(VERSION 3.13) # build config -option(BUILD_64BIT "Build 64bit" ON) -option(GOOGLE_TEST "Execute GoogleTest" ON) +option(BUILD_64BIT "Build 64bit" OFF) +option(GOOGLE_TEST "Execute GoogleTest" OFF) if (NOT BUILD_64BIT) option(GOOGLE_TEST OFF) # GoogleTest supports 64bit only endif()