From 28a72459a5a33d239685047a2ac6b3f1b0d335f4 Mon Sep 17 00:00:00 2001 From: TomokiMochizuki Date: Tue, 24 Sep 2024 23:32:59 +0900 Subject: [PATCH 1/2] fix transition matrix for Yamanaka-Ankersen's STM --- src/dynamics/orbit/relative_orbit.cpp | 2 +- .../orbit/relative_orbit_yamanaka_ankersen.cpp | 11 +++++++++-- .../orbit/relative_orbit_yamanaka_ankersen.hpp | 3 ++- 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index 94ed20d5f..050dfa69d 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -103,7 +103,7 @@ void RelativeOrbit::InitializeStmMatrix(orbit::StmModel stm_model_type, const Or relative_orbit_carter_.CalculateInitialInverseMatrix(gravity_constant_m3_s2, f_ref_rad, &reference_oe); break; case orbit::StmModel::kYamakawaAnkersen: - relative_orbit_yamanaka_ankersen_.CalculateInitialInverseMatrix(f_ref_rad, &reference_oe); + relative_orbit_yamanaka_ankersen_.CalculateInitialInverseMatrix(gravity_constant_m3_s2, f_ref_rad, &reference_oe); break; default: diff --git a/src/math_physics/orbit/relative_orbit_yamanaka_ankersen.cpp b/src/math_physics/orbit/relative_orbit_yamanaka_ankersen.cpp index 30cdd3ec9..284a62717 100644 --- a/src/math_physics/orbit/relative_orbit_yamanaka_ankersen.cpp +++ b/src/math_physics/orbit/relative_orbit_yamanaka_ankersen.cpp @@ -6,6 +6,7 @@ #include +#include "./relative_orbit_models.hpp" #include "./sgp4/sgp4unit.h" // for getgravconst() namespace orbit { @@ -14,9 +15,12 @@ RelativeOrbitYamanakaAnkersen::RelativeOrbitYamanakaAnkersen() {} RelativeOrbitYamanakaAnkersen::~RelativeOrbitYamanakaAnkersen() {} -void RelativeOrbitYamanakaAnkersen::CalculateInitialInverseMatrix(double f_ref_rad, OrbitalElements* reference_oe) { +void RelativeOrbitYamanakaAnkersen::CalculateInitialInverseMatrix(double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe) { // The following variables are defined in Spacecraft Formation Flying (Section 5.6.4) const double e = reference_oe->GetEccentricity(); + const double a = reference_oe->GetSemiMajorAxis_m(); + const double h = pow(a * (1.0 - pow(e, 2)) * gravity_constant_m3_s2, 0.5); // angular momentum + const double eta = pow(1 - pow(e, 2.0), 0.5); const double k = 1 + e * cos(f_ref_rad); const double c = k * cos(f_ref_rad); @@ -69,6 +73,9 @@ void RelativeOrbitYamanakaAnkersen::CalculateInitialInverseMatrix(double f_ref_r initial_inverse_matrix_[i][j] = 1 / pow(eta, 2.0) * initial_inverse_matrix_[i][j]; } } + + initial_inverse_matrix_ = + initial_inverse_matrix_ * orbit::CalcStateTransformationMatrixLvlhToTschaunerHampel(gravity_constant_m3_s2, e, h, f_ref_rad); } math::Matrix<6, 6> RelativeOrbitYamanakaAnkersen::CalculateSTM(double gravity_constant_m3_s2, double elapsed_time_s, double f_ref_rad, @@ -128,7 +135,7 @@ math::Matrix<6, 6> RelativeOrbitYamanakaAnkersen::CalculateSTM(double gravity_co update_matrix[5][4] = 0.0; update_matrix[5][5] = cos(f_ref_rad); - return update_matrix * initial_inverse_matrix_; + return orbit::CalcStateTransformationMatrixTschaunerHampelToLvlh(gravity_constant_m3_s2, e, h, f_ref_rad) * update_matrix * initial_inverse_matrix_; } } // namespace orbit diff --git a/src/math_physics/orbit/relative_orbit_yamanaka_ankersen.hpp b/src/math_physics/orbit/relative_orbit_yamanaka_ankersen.hpp index 518449827..0e49e177f 100644 --- a/src/math_physics/orbit/relative_orbit_yamanaka_ankersen.hpp +++ b/src/math_physics/orbit/relative_orbit_yamanaka_ankersen.hpp @@ -31,10 +31,11 @@ class RelativeOrbitYamanakaAnkersen { /** * @fn CalculateInitialInverseMatrix * @brief Calculate position and velocity with Kepler orbit propagation + * @param [in] gravity_constant_m3_s2: Gravity constant of the center body [m3/s2] * @param [in] f_ref_rad: True anomaly of the reference satellite [rad] * @param [in] reference_oe: Orbital elements of reference satellite */ - void CalculateInitialInverseMatrix(double f_ref_rad, OrbitalElements* reference_oe); + void CalculateInitialInverseMatrix(double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe); /** * @fn CalculateSTM From bfbef2a5f7b769672a4c8a563d529c9177acce4d Mon Sep 17 00:00:00 2001 From: TomokiMochizuki Date: Tue, 24 Sep 2024 23:36:39 +0900 Subject: [PATCH 2/2] ffix *.ini file --- settings/sample_satellite/satellite_sub.ini | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/settings/sample_satellite/satellite_sub.ini b/settings/sample_satellite/satellite_sub.ini index 32d399efd..7703cbe47 100644 --- a/settings/sample_satellite/satellite_sub.ini +++ b/settings/sample_satellite/satellite_sub.ini @@ -104,7 +104,7 @@ relative_orbit_update_method = 1 relative_dynamics_model_type = 0 // STM Relative Dynamics model type (only valid for STM update) // 0: HCW, 1: Melton, 2: SS, 3: Sabatini, 4: Carter, 5: Yamanaka-Ankersen -stm_model_type = 4 +stm_model_type = 5 // Initial satellite position relative to the reference satellite in LVLH frame[m] // * The coordinate system is defined at [PLANET_SELECTION] in SampleSimBase.ini initial_relative_position_lvlh_m(0) = 0.0