From 8da00c9bec55ff53b8361294dbc2de66d926aebc Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 15:11:25 +0900 Subject: [PATCH 01/34] add empty stm --- .../orbit/relative_orbit_models.cpp | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index 77d3ce5c3..b1efb0d07 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -132,6 +132,43 @@ math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constan math::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe) { math::Matrix<6, 6> stm; // ここでstmを計算してください + double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); + stm[0][0] = 0.0; + stm[0][1] = 0.0; + stm[0][2] = 0.0; + stm[0][3] = 0.0; + stm[0][4] = 0.0; + stm[0][5] = 0.0; + stm[1][0] = 0.0; + stm[1][1] = 0.0; + stm[1][2] = 0.0; + stm[1][3] = 0.0; + stm[1][4] = 0.0; + stm[1][5] = 0.0; + stm[2][0] = 0.0; + stm[2][1] = 0.0; + stm[2][2] = 0.0; + stm[2][3] = 0.0; + stm[2][4] = 0.0; + stm[2][5] = 0.0; + stm[3][0] = 0.0; + stm[3][1] = 0.0; + stm[3][2] = 0.0; + stm[3][3] = 0.0; + stm[3][4] = 0.0; + stm[3][5] = 0.0; + stm[4][0] = 0.0; + stm[4][1] = 0.0; + stm[4][2] = 0.0; + stm[4][3] = 0.0; + stm[4][4] = 0.0; + stm[4][5] = 0.0; + stm[5][0] = 0.0; + stm[5][1] = 0.0; + stm[5][2] = 0.0; + stm[5][3] = 0.0; + stm[5][4] = 0.0; + stm[5][5] = 0.0; return stm; } From 6b6dc4905725be222bf7c02896e821b014abe6e8 Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 15:40:48 +0900 Subject: [PATCH 02/34] add params calculation --- .../orbit/relative_orbit_models.cpp | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index b1efb0d07..a6afb16ab 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -133,6 +133,31 @@ math::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_ math::Matrix<6, 6> stm; // ここでstmを計算してください double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); + double e = reference_oe->GetEccentricity(); + double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); + double k = 1 + e * cos(f_ref_rad); + double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); + double K2 = pow(1 - e * e, -2.5) * (0.5 * E_rad - 0.25 * sin(2 * E_rad) - e * pow(sin(E_rad), 3) / 3.); + double phi1 = sin(f_ref_rad) * (e * cos(f_ref_rad) + 1); + double sigma1 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); + double term1_phi2 = 2 * e * sin(f_ref_rad) * (e * cos(f_ref_rad) + 1); + double term2_phi2 = (e * pow(sin(2 * sigma1), 3) / 3 + sin(4 * sigma1) / 4 - sigma1) / pow(1 - pow(e, 2), 5.0 / 2.0); + double term3_phi2 = sin(f_ref_rad) / pow(e * cos(f_ref_rad) + 1, 3); + double term4_phi2 = cos(f_ref_rad) / (e * cos(f_ref_rad) + 1); + double phi2 = term1_phi2 * (term2_phi2 + term3_phi2) - term4_phi2; + double sigma2 = e * cos(f_ref_rad) + 1; + double term1_phi3 = pow(cos(f_ref_rad), 2); + double term2_phi3 = 6 * e * sin(f_ref_rad) * sigma2 * (e * pow(sin(2 * sigma1), 3) / 3 + sin(4 * sigma1) / 4 - sigma1) / pow(1 - pow(e, 2), 5.0 / 2.0); + double phi3 = -term1_phi3 - (term1_phi3 / sigma2) - (2 * pow(sin(f_ref_rad), 2) / pow(sigma2, 2)) - term2_phi3; + double phi1_prime = cos(f_ref_rad) * (e * cos(f_ref_rad) + 1) - e * pow(sin(f_ref_rad), 2); + double sigma4 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); + double term1_phi2_prime = sin(f_ref_rad) / sigma2; + double term2_phi2_prime = 2 * e * sin(f_ref_rad) * sigma2 * (cos(f_ref_rad) / pow(sigma2, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); + double phi2_prime = term1_phi2_prime + term2_phi2_prime; + double phi3_prime = sigma2 - 4 * e * pow(sin(f_ref_rad), 3) / pow(sigma1, 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(sigma1, 2) - 6 * e * cos(f_ref_rad) * sigma1 / pow(1 - pow(e, 2), 5.0 / 2.0); + double S_phi1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); + double S_phi2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); + double S_phi3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); stm[0][0] = 0.0; stm[0][1] = 0.0; stm[0][2] = 0.0; From 466c5f315506fcfd323994193337b19775ccf2e1 Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 16:06:46 +0900 Subject: [PATCH 03/34] refac formulation --- .../orbit/relative_orbit_models.cpp | 24 +++++++------------ 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index a6afb16ab..f335b7f23 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -135,26 +135,18 @@ math::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_ double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); double e = reference_oe->GetEccentricity(); double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); - double k = 1 + e * cos(f_ref_rad); + double k = e * cos(f_ref_rad) + 1; double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); double K2 = pow(1 - e * e, -2.5) * (0.5 * E_rad - 0.25 * sin(2 * E_rad) - e * pow(sin(E_rad), 3) / 3.); - double phi1 = sin(f_ref_rad) * (e * cos(f_ref_rad) + 1); - double sigma1 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); - double term1_phi2 = 2 * e * sin(f_ref_rad) * (e * cos(f_ref_rad) + 1); - double term2_phi2 = (e * pow(sin(2 * sigma1), 3) / 3 + sin(4 * sigma1) / 4 - sigma1) / pow(1 - pow(e, 2), 5.0 / 2.0); - double term3_phi2 = sin(f_ref_rad) / pow(e * cos(f_ref_rad) + 1, 3); - double term4_phi2 = cos(f_ref_rad) / (e * cos(f_ref_rad) + 1); - double phi2 = term1_phi2 * (term2_phi2 + term3_phi2) - term4_phi2; - double sigma2 = e * cos(f_ref_rad) + 1; - double term1_phi3 = pow(cos(f_ref_rad), 2); - double term2_phi3 = 6 * e * sin(f_ref_rad) * sigma2 * (e * pow(sin(2 * sigma1), 3) / 3 + sin(4 * sigma1) / 4 - sigma1) / pow(1 - pow(e, 2), 5.0 / 2.0); - double phi3 = -term1_phi3 - (term1_phi3 / sigma2) - (2 * pow(sin(f_ref_rad), 2) / pow(sigma2, 2)) - term2_phi3; - double phi1_prime = cos(f_ref_rad) * (e * cos(f_ref_rad) + 1) - e * pow(sin(f_ref_rad), 2); + double phi1 = sin(f_ref_rad) * k; + double phi2 = 2 * e * phi1 * (sin(f_ref_rad) / pow(k, 3) - K2) - cos(f_ref_rad) / k; + double phi3 = (6 * e * phi1 * K2 - (2 * pow(sin(f_ref_rad), 2) / pow(k, 2)) - (pow(cos(f_ref_rad), 2) / k) - pow(cos(f_ref_rad), 2)); + double phi1_prime = cos(f_ref_rad) * k - e * pow(sin(f_ref_rad), 2); double sigma4 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); - double term1_phi2_prime = sin(f_ref_rad) / sigma2; - double term2_phi2_prime = 2 * e * sin(f_ref_rad) * sigma2 * (cos(f_ref_rad) / pow(sigma2, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); + double term1_phi2_prime = sin(f_ref_rad) / k; + double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); double phi2_prime = term1_phi2_prime + term2_phi2_prime; - double phi3_prime = sigma2 - 4 * e * pow(sin(f_ref_rad), 3) / pow(sigma1, 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(sigma1, 2) - 6 * e * cos(f_ref_rad) * sigma1 / pow(1 - pow(e, 2), 5.0 / 2.0); + double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); double S_phi1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); double S_phi2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); double S_phi3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); From a8944cb6b89d7ce2e8dd657b4d5a32138d73171f Mon Sep 17 00:00:00 2001 From: YuseiAlexS Date: Mon, 23 Sep 2024 16:21:44 +0900 Subject: [PATCH 04/34] stm + stm_inv --- ExtLibraries/CMakeLists.txt | 2 +- .../orbit/relative_orbit_models.cpp | 80 ++++++++++++++----- 2 files changed, 60 insertions(+), 22 deletions(-) diff --git a/ExtLibraries/CMakeLists.txt b/ExtLibraries/CMakeLists.txt index 1723ecb87..f95bf41b0 100644 --- a/ExtLibraries/CMakeLists.txt +++ b/ExtLibraries/CMakeLists.txt @@ -26,6 +26,6 @@ string(REPLACE "\\" "/" SETTINGS_DIR ${SETTINGS_DIR}) message("ExtLibraries install dir: ${EXT_LIB_DIR}") -add_subdirectory(nrlmsise00) +#add_subdirectory(nrlmsise00) add_subdirectory(cspice) add_subdirectory(lunar_gravity_field) diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index f335b7f23..7aa8c9b14 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -129,7 +129,7 @@ math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constan return stm; } -math::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe) { +math::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe){ math::Matrix<6, 6> stm; // ここでstmを計算してください double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); @@ -147,30 +147,30 @@ math::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_ double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); double phi2_prime = term1_phi2_prime + term2_phi2_prime; double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); - double S_phi1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); - double S_phi2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); - double S_phi3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); - stm[0][0] = 0.0; - stm[0][1] = 0.0; - stm[0][2] = 0.0; + double S1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); + double S2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); + double S3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); + stm[0][0] = phi1; + stm[0][1] = phi2; + stm[0][2] = phi3; stm[0][3] = 0.0; stm[0][4] = 0.0; stm[0][5] = 0.0; - stm[1][0] = 0.0; - stm[1][1] = 0.0; - stm[1][2] = 0.0; + stm[1][0] = phi1_prime; + stm[1][1] = phi2_prime; + stm[1][2] = phi3_prime; stm[1][3] = 0.0; stm[1][4] = 0.0; stm[1][5] = 0.0; - stm[2][0] = 0.0; - stm[2][1] = 0.0; - stm[2][2] = 0.0; - stm[2][3] = 0.0; + stm[2][0] = -2*S1; + stm[2][1] = -2*S2; + stm[2][2] = -S3; + stm[2][3] = -1; stm[2][4] = 0.0; stm[2][5] = 0.0; - stm[3][0] = 0.0; - stm[3][1] = 0.0; - stm[3][2] = 0.0; + stm[3][0] = -2*phi1; + stm[3][1] = -2*phi2; + stm[3][2] = -(2*phi3 + 1); stm[3][3] = 0.0; stm[3][4] = 0.0; stm[3][5] = 0.0; @@ -178,14 +178,52 @@ math::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_ stm[4][1] = 0.0; stm[4][2] = 0.0; stm[4][3] = 0.0; - stm[4][4] = 0.0; - stm[4][5] = 0.0; + stm[4][4] = cos(f_ref_rad); + stm[4][5] = sin(f_ref_rad); stm[5][0] = 0.0; stm[5][1] = 0.0; stm[5][2] = 0.0; stm[5][3] = 0.0; - stm[5][4] = 0.0; - stm[5][5] = 0.0; + stm[5][4] = -sin(f_ref_rad); + stm[5][5] = cos(f_ref_rad); + + stm_inv[0][0] = 4*S2 + phi2_prime; + stm_inv[0][1] = -phi2; + stm_inv[0][2] = 0.0; + stm_inv[0][3] = 2*S2; + stm_inv[0][4] = 0.0; + stm_inv[0][5] = 0.0; + stm_inv[1][0] = -(4*S1 + phi1_prime); + stm_inv[1][1] = phi1; + stm_inv[1][2] = 0.0; + stm_inv[1][3] = -2*S1; + stm_inv[1][4] = 0.0; + stm_inv[1][5] = 0.0; + stm_inv[2][0] = -2; + stm_inv[2][1] = 0.0; + stm_inv[2][2] = 0.0; + stm_inv[2][3] = -1; + stm_inv[2][4] = 0.0; + stm_inv[2][5] = 0.0; + stm_inv[3][0] = 2*S3 + phi3_prime; + stm_inv[3][1] = -phi3; + stm_inv[3][2] = -1; + stm_inv[3][3] = S3; + stm_inv[3][4] = 0.0; + stm_inv[3][5] = 0.0; + stm_inv[4][0] = 0.0; + stm_inv[4][1] = 0.0; + stm_inv[4][2] = 0.0; + stm_inv[4][3] = 0.0; + stm_inv[4][4] = cos(f_ref_rad); + stm_inv[4][5] = -sin(f_ref_rad); + stm_inv[5][0] = 0.0; + stm_inv[5][1] = 0.0; + stm_inv[5][2] = 0.0; + stm_inv[5][3] = 0.0; + stm_inv[5][4] = sin(f_ref_rad); + stm_inv[5][5] = cos(f_ref_rad); + return stm; } From cfae17f83c888219ca1b55a65de6eec383495052 Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 15:10:25 +0900 Subject: [PATCH 05/34] add empty stm --- src/library/orbit/relative_orbit_models.cpp | 170 ++++++++++++++++++++ 1 file changed, 170 insertions(+) create mode 100644 src/library/orbit/relative_orbit_models.cpp diff --git a/src/library/orbit/relative_orbit_models.cpp b/src/library/orbit/relative_orbit_models.cpp new file mode 100644 index 000000000..d5bfaa8c5 --- /dev/null +++ b/src/library/orbit/relative_orbit_models.cpp @@ -0,0 +1,170 @@ +/** + * @file relative_orbit_models.cpp + * @brief Functions for relative orbit + */ +#include "relative_orbit_models.hpp" + +#include +#include "../external/sgp4/sgp4unit.h" // for getgravconst() + +libra::Matrix<6, 6> CalcHillSystemMatrix(double orbit_radius_m, double gravity_constant_m3_s2) { + libra::Matrix<6, 6> system_matrix; + + double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); + system_matrix[0][0] = 0.0; + system_matrix[0][1] = 0.0; + system_matrix[0][2] = 0.0; + system_matrix[0][3] = 1.0; + system_matrix[0][4] = 0.0; + system_matrix[0][5] = 0.0; + system_matrix[1][0] = 0.0; + system_matrix[1][1] = 0.0; + system_matrix[1][2] = 0.0; + system_matrix[1][3] = 0.0; + system_matrix[1][4] = 1.0; + system_matrix[1][5] = 0.0; + system_matrix[2][0] = 0.0; + system_matrix[2][1] = 0.0; + system_matrix[2][2] = 0.0; + system_matrix[2][3] = 0.0; + system_matrix[2][4] = 0.0; + system_matrix[2][5] = 1.0; + system_matrix[3][0] = 3.0 * n * n; + system_matrix[3][1] = 0.0; + system_matrix[3][2] = 0.0; + system_matrix[3][3] = 0.0; + system_matrix[3][4] = 2.0 * n; + system_matrix[3][5] = 0.0; + system_matrix[4][0] = 0.0; + system_matrix[4][1] = 0.0; + system_matrix[4][2] = 0.0; + system_matrix[4][3] = -2.0 * n; + system_matrix[4][4] = 0.0; + system_matrix[4][5] = 0.0; + system_matrix[5][0] = 0.0; + system_matrix[5][1] = 0.0; + system_matrix[5][2] = -n * n; + system_matrix[5][3] = 0.0; + system_matrix[5][4] = 0.0; + system_matrix[5][5] = 0.0; + + return system_matrix; +} + +libra::Matrix<6, 6> CalcHcwStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s) { + libra::Matrix<6, 6> stm; + + double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); + double t = elapsed_time_s; + stm[0][0] = 4.0 - 3.0 * cos(n * t); + stm[0][1] = 0.0; + stm[0][2] = 0.0; + stm[0][3] = sin(n * t) / n; + stm[0][4] = 2.0 / n - 2.0 * cos(n * t) / n; + stm[0][5] = 0.0; + stm[1][0] = -6.0 * n * t + 6 * sin(n * t); + stm[1][1] = 1.0; + stm[1][2] = 0.0; + stm[1][3] = -2.0 / n + 2.0 * cos(n * t) / n; + stm[1][4] = 4.0 * sin(n * t) / n - 3.0 * t; + stm[1][5] = 0.0; + stm[2][0] = 0.0; + stm[2][1] = 0.0; + stm[2][2] = cos(n * t); + stm[2][3] = 0.0; + stm[2][4] = 0.0; + stm[2][5] = sin(n * t) / n; + stm[3][0] = 3.0 * n * sin(n * t); + stm[3][1] = 0.0; + stm[3][2] = 0.0; + stm[3][3] = cos(n * t); + stm[3][4] = 2 * sin(n * t); + stm[3][5] = 0.0; + stm[4][0] = -6.0 * n + 6.0 * n * cos(n * t); + stm[4][1] = 0.0; + stm[4][2] = 0.0; + stm[4][3] = -2.0 * sin(n * t); + stm[4][4] = -3.0 + 4.0 * cos(n * t); + stm[4][5] = 0.0; + stm[5][0] = 0.0; + stm[5][1] = 0.0; + stm[5][2] = -n * sin(n * t); + stm[5][3] = 0.0; + stm[5][4] = 0.0; + stm[5][5] = cos(n * t); + return stm; +} + +// 定数やreferenceの軌道要素は以下のように取得できます +// gravconsttype whichconst = wgs72; +// double mu_km3_s2, radius_earth_km, tumin, xke, j2, j3, j4, j3oj2; +// getgravconst(whichconst, tumin, mu_km3_s2, radius_earth_km, xke, j2, j3, j4, j3oj2); +// double semi_major_axis_m = reference_oe->GetSemiMajorAxis_m(); +// double eccentricity = reference_oe->GetEccentricity(); + +libra::Matrix<6, 6> CalcMeltonStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, OrbitalElements* reference_oe) { + libra::Matrix<6, 6> stm; + // ここでstmを計算してください + return stm; +} + +libra::Matrix<6, 6> CalcSsStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, OrbitalElements* reference_oe) { + libra::Matrix<6, 6> stm; + // ここでstmを計算してください + return stm; +} + +libra::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, OrbitalElements* reference_oe) { + libra::Matrix<6, 6> stm; + // ここでstmを計算してください + return stm; +} + +libra::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe){ + libra::Matrix<6, 6> stm; + // ここでstmを計算してください + double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); + stm[0][0] = 0.0; + stm[0][1] = 0.0; + stm[0][2] = 0.0; + stm[0][3] = 0.0; + stm[0][4] = 0.0; + stm[0][5] = 0.0; + stm[1][0] = 0.0; + stm[1][1] = 0.0; + stm[1][2] = 0.0; + stm[1][3] = 0.0; + stm[1][4] = 0.0; + stm[1][5] = 0.0; + stm[2][0] = 0.0; + stm[2][1] = 0.0; + stm[2][2] = 0.0; + stm[2][3] = 0.0; + stm[2][4] = 0.0; + stm[2][5] = 0.0; + stm[3][0] = 0.0; + stm[3][1] = 0.0; + stm[3][2] = 0.0; + stm[3][3] = 0.0; + stm[3][4] = 0.0; + stm[3][5] = 0.0; + stm[4][0] = 0.0; + stm[4][1] = 0.0; + stm[4][2] = 0.0; + stm[4][3] = 0.0; + stm[4][4] = 0.0; + stm[4][5] = 0.0; + stm[5][0] = 0.0; + stm[5][1] = 0.0; + stm[5][2] = 0.0; + stm[5][3] = 0.0; + stm[5][4] = 0.0; + stm[5][5] = 0.0; + return stm; +} + +libra::Matrix<6, 6> CalcYamakawaAnkersenStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe){ + libra::Matrix<6, 6> stm; + // ここでstmを計算してください + return stm; +} From 73721dd1d7d365455dd1ac8a1ee34c4bebc1d8b2 Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 15:40:48 +0900 Subject: [PATCH 06/34] add params calculation --- src/library/orbit/relative_orbit_models.cpp | 25 +++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/library/orbit/relative_orbit_models.cpp b/src/library/orbit/relative_orbit_models.cpp index d5bfaa8c5..d54b6426c 100644 --- a/src/library/orbit/relative_orbit_models.cpp +++ b/src/library/orbit/relative_orbit_models.cpp @@ -124,6 +124,31 @@ libra::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant libra::Matrix<6, 6> stm; // ここでstmを計算してください double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); + double e = reference_oe->GetEccentricity(); + double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); + double k = 1 + e * cos(f_ref_rad); + double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); + double K2 = pow(1 - e * e, -2.5) * (0.5 * E_rad - 0.25 * sin(2 * E_rad) - e * pow(sin(E_rad), 3) / 3.); + double phi1 = sin(f_ref_rad) * (e * cos(f_ref_rad) + 1); + double sigma1 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); + double term1_phi2 = 2 * e * sin(f_ref_rad) * (e * cos(f_ref_rad) + 1); + double term2_phi2 = (e * pow(sin(2 * sigma1), 3) / 3 + sin(4 * sigma1) / 4 - sigma1) / pow(1 - pow(e, 2), 5.0 / 2.0); + double term3_phi2 = sin(f_ref_rad) / pow(e * cos(f_ref_rad) + 1, 3); + double term4_phi2 = cos(f_ref_rad) / (e * cos(f_ref_rad) + 1); + double phi2 = term1_phi2 * (term2_phi2 + term3_phi2) - term4_phi2; + double sigma2 = e * cos(f_ref_rad) + 1; + double term1_phi3 = pow(cos(f_ref_rad), 2); + double term2_phi3 = 6 * e * sin(f_ref_rad) * sigma2 * (e * pow(sin(2 * sigma1), 3) / 3 + sin(4 * sigma1) / 4 - sigma1) / pow(1 - pow(e, 2), 5.0 / 2.0); + double phi3 = -term1_phi3 - (term1_phi3 / sigma2) - (2 * pow(sin(f_ref_rad), 2) / pow(sigma2, 2)) - term2_phi3; + double phi1_prime = cos(f_ref_rad) * (e * cos(f_ref_rad) + 1) - e * pow(sin(f_ref_rad), 2); + double sigma4 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); + double term1_phi2_prime = sin(f_ref_rad) / sigma2; + double term2_phi2_prime = 2 * e * sin(f_ref_rad) * sigma2 * (cos(f_ref_rad) / pow(sigma2, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); + double phi2_prime = term1_phi2_prime + term2_phi2_prime; + double phi3_prime = sigma2 - 4 * e * pow(sin(f_ref_rad), 3) / pow(sigma1, 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(sigma1, 2) - 6 * e * cos(f_ref_rad) * sigma1 / pow(1 - pow(e, 2), 5.0 / 2.0); + double S_phi1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); + double S_phi2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); + double S_phi3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); stm[0][0] = 0.0; stm[0][1] = 0.0; stm[0][2] = 0.0; From e15c7cd4b9d19e522e78260273910f2ffde4ed56 Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 16:06:46 +0900 Subject: [PATCH 07/34] refac formulation --- src/library/orbit/relative_orbit_models.cpp | 24 +++++++-------------- 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/src/library/orbit/relative_orbit_models.cpp b/src/library/orbit/relative_orbit_models.cpp index d54b6426c..4ccfe050b 100644 --- a/src/library/orbit/relative_orbit_models.cpp +++ b/src/library/orbit/relative_orbit_models.cpp @@ -126,26 +126,18 @@ libra::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); double e = reference_oe->GetEccentricity(); double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); - double k = 1 + e * cos(f_ref_rad); + double k = e * cos(f_ref_rad) + 1; double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); double K2 = pow(1 - e * e, -2.5) * (0.5 * E_rad - 0.25 * sin(2 * E_rad) - e * pow(sin(E_rad), 3) / 3.); - double phi1 = sin(f_ref_rad) * (e * cos(f_ref_rad) + 1); - double sigma1 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); - double term1_phi2 = 2 * e * sin(f_ref_rad) * (e * cos(f_ref_rad) + 1); - double term2_phi2 = (e * pow(sin(2 * sigma1), 3) / 3 + sin(4 * sigma1) / 4 - sigma1) / pow(1 - pow(e, 2), 5.0 / 2.0); - double term3_phi2 = sin(f_ref_rad) / pow(e * cos(f_ref_rad) + 1, 3); - double term4_phi2 = cos(f_ref_rad) / (e * cos(f_ref_rad) + 1); - double phi2 = term1_phi2 * (term2_phi2 + term3_phi2) - term4_phi2; - double sigma2 = e * cos(f_ref_rad) + 1; - double term1_phi3 = pow(cos(f_ref_rad), 2); - double term2_phi3 = 6 * e * sin(f_ref_rad) * sigma2 * (e * pow(sin(2 * sigma1), 3) / 3 + sin(4 * sigma1) / 4 - sigma1) / pow(1 - pow(e, 2), 5.0 / 2.0); - double phi3 = -term1_phi3 - (term1_phi3 / sigma2) - (2 * pow(sin(f_ref_rad), 2) / pow(sigma2, 2)) - term2_phi3; - double phi1_prime = cos(f_ref_rad) * (e * cos(f_ref_rad) + 1) - e * pow(sin(f_ref_rad), 2); + double phi1 = sin(f_ref_rad) * k; + double phi2 = 2 * e * phi1 * (sin(f_ref_rad) / pow(k, 3) - K2) - cos(f_ref_rad) / k; + double phi3 = (6 * e * phi1 * K2 - (2 * pow(sin(f_ref_rad), 2) / pow(k, 2)) - (pow(cos(f_ref_rad), 2) / k) - pow(cos(f_ref_rad), 2)); + double phi1_prime = cos(f_ref_rad) * k - e * pow(sin(f_ref_rad), 2); double sigma4 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); - double term1_phi2_prime = sin(f_ref_rad) / sigma2; - double term2_phi2_prime = 2 * e * sin(f_ref_rad) * sigma2 * (cos(f_ref_rad) / pow(sigma2, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); + double term1_phi2_prime = sin(f_ref_rad) / k; + double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); double phi2_prime = term1_phi2_prime + term2_phi2_prime; - double phi3_prime = sigma2 - 4 * e * pow(sin(f_ref_rad), 3) / pow(sigma1, 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(sigma1, 2) - 6 * e * cos(f_ref_rad) * sigma1 / pow(1 - pow(e, 2), 5.0 / 2.0); + double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); double S_phi1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); double S_phi2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); double S_phi3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); From 4109b70c94197399c7a311bb2fbda7912d589cec Mon Sep 17 00:00:00 2001 From: YuseiAlexS Date: Mon, 23 Sep 2024 16:21:44 +0900 Subject: [PATCH 08/34] stm + stm_inv --- src/library/orbit/relative_orbit_models.cpp | 81 +++++++++++++++------ 1 file changed, 60 insertions(+), 21 deletions(-) diff --git a/src/library/orbit/relative_orbit_models.cpp b/src/library/orbit/relative_orbit_models.cpp index 4ccfe050b..4c270e6a9 100644 --- a/src/library/orbit/relative_orbit_models.cpp +++ b/src/library/orbit/relative_orbit_models.cpp @@ -120,8 +120,9 @@ libra::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_consta return stm; } -libra::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe){ +libra::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, double f_ref_0_rad, OrbitalElements* reference_oe){ libra::Matrix<6, 6> stm; + libra::Matrix<6, 6> stm_inv; // ここでstmを計算してください double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); double e = reference_oe->GetEccentricity(); @@ -138,30 +139,30 @@ libra::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); double phi2_prime = term1_phi2_prime + term2_phi2_prime; double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); - double S_phi1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); - double S_phi2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); - double S_phi3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); - stm[0][0] = 0.0; - stm[0][1] = 0.0; - stm[0][2] = 0.0; + double S1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); + double S2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); + double S3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); + stm[0][0] = phi1; + stm[0][1] = phi2; + stm[0][2] = phi3; stm[0][3] = 0.0; stm[0][4] = 0.0; stm[0][5] = 0.0; - stm[1][0] = 0.0; - stm[1][1] = 0.0; - stm[1][2] = 0.0; + stm[1][0] = phi1_prime; + stm[1][1] = phi2_prime; + stm[1][2] = phi3_prime; stm[1][3] = 0.0; stm[1][4] = 0.0; stm[1][5] = 0.0; - stm[2][0] = 0.0; - stm[2][1] = 0.0; - stm[2][2] = 0.0; - stm[2][3] = 0.0; + stm[2][0] = -2*S1; + stm[2][1] = -2*S2; + stm[2][2] = -S3; + stm[2][3] = -1; stm[2][4] = 0.0; stm[2][5] = 0.0; - stm[3][0] = 0.0; - stm[3][1] = 0.0; - stm[3][2] = 0.0; + stm[3][0] = -2*phi1; + stm[3][1] = -2*phi2; + stm[3][2] = -(2*phi3 + 1); stm[3][3] = 0.0; stm[3][4] = 0.0; stm[3][5] = 0.0; @@ -169,14 +170,52 @@ libra::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant stm[4][1] = 0.0; stm[4][2] = 0.0; stm[4][3] = 0.0; - stm[4][4] = 0.0; - stm[4][5] = 0.0; + stm[4][4] = cos(f_ref_rad); + stm[4][5] = sin(f_ref_rad); stm[5][0] = 0.0; stm[5][1] = 0.0; stm[5][2] = 0.0; stm[5][3] = 0.0; - stm[5][4] = 0.0; - stm[5][5] = 0.0; + stm[5][4] = -sin(f_ref_rad); + stm[5][5] = cos(f_ref_rad); + + stm_inv[0][0] = 4*S2 + phi2_prime; + stm_inv[0][1] = -phi2; + stm_inv[0][2] = 0.0; + stm_inv[0][3] = 2*S2; + stm_inv[0][4] = 0.0; + stm_inv[0][5] = 0.0; + stm_inv[1][0] = -(4*S1 + phi1_prime); + stm_inv[1][1] = phi1; + stm_inv[1][2] = 0.0; + stm_inv[1][3] = -2*S1; + stm_inv[1][4] = 0.0; + stm_inv[1][5] = 0.0; + stm_inv[2][0] = -2; + stm_inv[2][1] = 0.0; + stm_inv[2][2] = 0.0; + stm_inv[2][3] = -1; + stm_inv[2][4] = 0.0; + stm_inv[2][5] = 0.0; + stm_inv[3][0] = 2*S3 + phi3_prime; + stm_inv[3][1] = -phi3; + stm_inv[3][2] = -1; + stm_inv[3][3] = S3; + stm_inv[3][4] = 0.0; + stm_inv[3][5] = 0.0; + stm_inv[4][0] = 0.0; + stm_inv[4][1] = 0.0; + stm_inv[4][2] = 0.0; + stm_inv[4][3] = 0.0; + stm_inv[4][4] = cos(f_ref_rad); + stm_inv[4][5] = -sin(f_ref_rad); + stm_inv[5][0] = 0.0; + stm_inv[5][1] = 0.0; + stm_inv[5][2] = 0.0; + stm_inv[5][3] = 0.0; + stm_inv[5][4] = sin(f_ref_rad); + stm_inv[5][5] = cos(f_ref_rad); + return stm; } From 99073123d7e66b66bf39009d0471b587c670cddb Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 16:43:51 +0900 Subject: [PATCH 09/34] add class --- .../orbit/relative_orbit_carter.cpp | 62 ++++++++++++++++++ .../orbit/relative_orbit_carter.hpp | 63 +++++++++++++++++++ .../orbit/relative_orbit_models.cpp | 1 + 3 files changed, 126 insertions(+) create mode 100644 src/math_physics/orbit/relative_orbit_carter.cpp create mode 100644 src/math_physics/orbit/relative_orbit_carter.hpp diff --git a/src/math_physics/orbit/relative_orbit_carter.cpp b/src/math_physics/orbit/relative_orbit_carter.cpp new file mode 100644 index 000000000..a61b466a2 --- /dev/null +++ b/src/math_physics/orbit/relative_orbit_carter.cpp @@ -0,0 +1,62 @@ +/** + * @file relative_orbit_models.cpp + * @brief Functions to calculate Yamanaka-ANkersen STM for relative orbit + */ +#include "relative_orbit_carter.hpp" + +#include + +#include "./sgp4/sgp4unit.h" // for getgravconst() + +namespace orbit { + +RelativeOrbitCarter::RelativeOrbitCarter() {} + +RelativeOrbitCarter::~RelativeOrbitCarter() {} + +void RelativeOrbitCarter::CalculateInitialInverseMatrix(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, + OrbitalElements* reference_oe) { + double eccentricity = reference_oe->GetEccentricity(); + double eta = pow(1 + pow(eccentricity, 2.0), 0.5); + double k = 1 + eccentricity * cos(f_ref_rad); + double c = k * cos(f_ref_rad); + double s = k * sin(f_ref_rad); + initial_inverse_matrix_[0][0] = -3.0 * s * (k + pow(eccentricity, 2.0) / pow(k, 2.0)); + initial_inverse_matrix_[0][1] = c - 2.0 * eccentricity; + initial_inverse_matrix_[0][2] = 0.0; + initial_inverse_matrix_[0][3] = -s * (k + 1) / k; + initial_inverse_matrix_[0][4] = 0.0; + initial_inverse_matrix_[0][5] = 0.0; + initial_inverse_matrix_[1][0] = -3.0 * (eccentricity + c / k); + initial_inverse_matrix_[1][1] = -s; + initial_inverse_matrix_[1][2] = 0.0; + initial_inverse_matrix_[1][3] = -(c * (k + 1) / k + eccentricity); + initial_inverse_matrix_[1][4] = 0.0; + initial_inverse_matrix_[1][5] = 0.0; + initial_inverse_matrix_[2][0] = 3.0 * k - pow(eta, 2.0); + initial_inverse_matrix_[2][1] = eccentricity * s; + initial_inverse_matrix_[2][2] = 0.0; + initial_inverse_matrix_[2][3] = pow(k, 2.0); + initial_inverse_matrix_[2][4] = 0.0; + initial_inverse_matrix_[2][5] = 0.0; + initial_inverse_matrix_[3][0] = 3.0 * eccentricity * s * (k + 1) / pow(k, 2.0); + initial_inverse_matrix_[3][1] = -2.0 + eccentricity * c; + initial_inverse_matrix_[3][2] = pow(eta, 2.0); + initial_inverse_matrix_[3][3] = -eccentricity * s * (k + 1) / k; + initial_inverse_matrix_[3][4] = 0.0; + initial_inverse_matrix_[3][5] = 0.0; + initial_inverse_matrix_[4][0] = 0.0; + initial_inverse_matrix_[4][1] = 0.0; + initial_inverse_matrix_[4][2] = 0.0; + initial_inverse_matrix_[4][3] = 0.0; + initial_inverse_matrix_[4][4] = 0.0; + initial_inverse_matrix_[4][5] = 0.0; + + for (int i = 0; i < 6; i++) { + for (int j = 0; j < 6; j++) { + initial_inverse_matrix_[i][j] = 1 / pow(eta, 2.0) * initial_inverse_matrix_[0][0]; + } + } +} + +} // namespace orbit diff --git a/src/math_physics/orbit/relative_orbit_carter.hpp b/src/math_physics/orbit/relative_orbit_carter.hpp new file mode 100644 index 000000000..9bcee57f1 --- /dev/null +++ b/src/math_physics/orbit/relative_orbit_carter.hpp @@ -0,0 +1,63 @@ +/** + * @file relative_orbit_yamanaka_ankersen.hpp + * @brief Functions to calculate Yamanaka-ANkersen STM for relative orbit + */ + +#ifndef S2E_LIBRARY_ORBIT_RELATIVE_ORBIT_YAMANAKA_ANKERSEN_HPP_ +#define S2E_LIBRARY_ORBIT_RELATIVE_ORBIT_YAMANAKA_ANKERSEN_HPP_ + +#include "../math/matrix.hpp" +#include "./orbital_elements.hpp" + +namespace orbit { + +/** + * @class RelativeOrbitCarter + * @brief Class to calculate Yamanaka-Ankersen relative orbital STM + */ +class RelativeOrbitCarter { + public: + /** + * @fn RelativeOrbitCarter + * @brief Default Constructor + */ + RelativeOrbitCarter(); + /** + * @fn ~RelativeOrbitCarter + * @brief Destructor + */ + ~RelativeOrbitCarter(); + + /** + * @fn CalculateInitialInverseMatrix + * @brief Calculate position and velocity with Kepler orbit propagation + * @param [in] orbit_radius_m: Orbit radius [m] + * @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 orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe); + + /** + * @fn CalculateSTM + * @brief Calculate position and velocity with Kepler orbit propagation + * @param [in] orbit_radius_m: Orbit radius [m] + * @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 + */ + math::Matrix<6, 6> CalculateSTM(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe); + + /** + * @fn GetInitialInverseMatrix + * @brief Return initial inverse matrix + */ + inline const math::Matrix<6, 6> GetInitialInverseMatrix() const { return initial_inverse_matrix_; } + + private: + math::Matrix<6, 6> initial_inverse_matrix_{0.0}; //!< Gravity constant of the center body [m3/s2] +}; + +} // namespace orbit + +#endif // S2E_LIBRARY_ORBIT_RELATIVE_ORBIT_MODEL_HPP_ diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index 7aa8c9b14..1eef8c50d 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -131,6 +131,7 @@ math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constan math::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe){ math::Matrix<6, 6> stm; + math::Matrix<6, 6> stm_inv; // ここでstmを計算してください double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); double e = reference_oe->GetEccentricity(); From d3ab507bb425ea4926e00d9b07c6712c5be2d709 Mon Sep 17 00:00:00 2001 From: YuseiAlexS Date: Mon, 23 Sep 2024 16:52:09 +0900 Subject: [PATCH 10/34] merge stm --- .../orbit/relative_orbit_carter.cpp | 155 +++++++++++++----- 1 file changed, 114 insertions(+), 41 deletions(-) diff --git a/src/math_physics/orbit/relative_orbit_carter.cpp b/src/math_physics/orbit/relative_orbit_carter.cpp index a61b466a2..b66fa00b2 100644 --- a/src/math_physics/orbit/relative_orbit_carter.cpp +++ b/src/math_physics/orbit/relative_orbit_carter.cpp @@ -16,47 +16,120 @@ RelativeOrbitCarter::~RelativeOrbitCarter() {} void RelativeOrbitCarter::CalculateInitialInverseMatrix(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe) { - double eccentricity = reference_oe->GetEccentricity(); - double eta = pow(1 + pow(eccentricity, 2.0), 0.5); - double k = 1 + eccentricity * cos(f_ref_rad); - double c = k * cos(f_ref_rad); - double s = k * sin(f_ref_rad); - initial_inverse_matrix_[0][0] = -3.0 * s * (k + pow(eccentricity, 2.0) / pow(k, 2.0)); - initial_inverse_matrix_[0][1] = c - 2.0 * eccentricity; - initial_inverse_matrix_[0][2] = 0.0; - initial_inverse_matrix_[0][3] = -s * (k + 1) / k; - initial_inverse_matrix_[0][4] = 0.0; - initial_inverse_matrix_[0][5] = 0.0; - initial_inverse_matrix_[1][0] = -3.0 * (eccentricity + c / k); - initial_inverse_matrix_[1][1] = -s; - initial_inverse_matrix_[1][2] = 0.0; - initial_inverse_matrix_[1][3] = -(c * (k + 1) / k + eccentricity); - initial_inverse_matrix_[1][4] = 0.0; - initial_inverse_matrix_[1][5] = 0.0; - initial_inverse_matrix_[2][0] = 3.0 * k - pow(eta, 2.0); - initial_inverse_matrix_[2][1] = eccentricity * s; - initial_inverse_matrix_[2][2] = 0.0; - initial_inverse_matrix_[2][3] = pow(k, 2.0); - initial_inverse_matrix_[2][4] = 0.0; - initial_inverse_matrix_[2][5] = 0.0; - initial_inverse_matrix_[3][0] = 3.0 * eccentricity * s * (k + 1) / pow(k, 2.0); - initial_inverse_matrix_[3][1] = -2.0 + eccentricity * c; - initial_inverse_matrix_[3][2] = pow(eta, 2.0); - initial_inverse_matrix_[3][3] = -eccentricity * s * (k + 1) / k; - initial_inverse_matrix_[3][4] = 0.0; - initial_inverse_matrix_[3][5] = 0.0; - initial_inverse_matrix_[4][0] = 0.0; - initial_inverse_matrix_[4][1] = 0.0; - initial_inverse_matrix_[4][2] = 0.0; - initial_inverse_matrix_[4][3] = 0.0; - initial_inverse_matrix_[4][4] = 0.0; - initial_inverse_matrix_[4][5] = 0.0; - - for (int i = 0; i < 6; i++) { - for (int j = 0; j < 6; j++) { - initial_inverse_matrix_[i][j] = 1 / pow(eta, 2.0) * initial_inverse_matrix_[0][0]; - } - } + math::Matrix<6, 6> stm_inv; + double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); + double e = reference_oe->GetEccentricity(); + double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); + double k = e * cos(f_ref_rad) + 1; + double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); + double K2 = pow(1 - e * e, -2.5) * (0.5 * E_rad - 0.25 * sin(2 * E_rad) - e * pow(sin(E_rad), 3) / 3.); + double phi1 = sin(f_ref_rad) * k; + double phi2 = 2 * e * phi1 * (sin(f_ref_rad) / pow(k, 3) - K2) - cos(f_ref_rad) / k; + double phi3 = (6 * e * phi1 * K2 - (2 * pow(sin(f_ref_rad), 2) / pow(k, 2)) - (pow(cos(f_ref_rad), 2) / k) - pow(cos(f_ref_rad), 2)); + double phi1_prime = cos(f_ref_rad) * k - e * pow(sin(f_ref_rad), 2); + double sigma4 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); + double term1_phi2_prime = sin(f_ref_rad) / k; + double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); + double phi2_prime = term1_phi2_prime + term2_phi2_prime; + double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); + double S1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); + double S2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); + double S3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); + stm_inv[0][0] = 4*S2 + phi2_prime; + stm_inv[0][1] = -phi2; + stm_inv[0][2] = 0.0; + stm_inv[0][3] = 2*S2; + stm_inv[0][4] = 0.0; + stm_inv[0][5] = 0.0; + stm_inv[1][0] = -(4*S1 + phi1_prime); + stm_inv[1][1] = phi1; + stm_inv[1][2] = 0.0; + stm_inv[1][3] = -2*S1; + stm_inv[1][4] = 0.0; + stm_inv[1][5] = 0.0; + stm_inv[2][0] = -2; + stm_inv[2][1] = 0.0; + stm_inv[2][2] = 0.0; + stm_inv[2][3] = -1; + stm_inv[2][4] = 0.0; + stm_inv[2][5] = 0.0; + stm_inv[3][0] = 2*S3 + phi3_prime; + stm_inv[3][1] = -phi3; + stm_inv[3][2] = -1; + stm_inv[3][3] = S3; + stm_inv[3][4] = 0.0; + stm_inv[3][5] = 0.0; + stm_inv[4][0] = 0.0; + stm_inv[4][1] = 0.0; + stm_inv[4][2] = 0.0; + stm_inv[4][3] = 0.0; + stm_inv[4][4] = cos(f_ref_rad); + stm_inv[4][5] = -sin(f_ref_rad); + stm_inv[5][0] = 0.0; + stm_inv[5][1] = 0.0; + stm_inv[5][2] = 0.0; + stm_inv[5][3] = 0.0; + stm_inv[5][4] = sin(f_ref_rad); + stm_inv[5][5] = cos(f_ref_rad); +} + +math::Matrix<6, 6> CalculateSTM(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe){ + math::Matrix<6, 6> stm; + // ここでstmを計算してください + double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); + double e = reference_oe->GetEccentricity(); + double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); + double k = e * cos(f_ref_rad) + 1; + double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); + double K2 = pow(1 - e * e, -2.5) * (0.5 * E_rad - 0.25 * sin(2 * E_rad) - e * pow(sin(E_rad), 3) / 3.); + double phi1 = sin(f_ref_rad) * k; + double phi2 = 2 * e * phi1 * (sin(f_ref_rad) / pow(k, 3) - K2) - cos(f_ref_rad) / k; + double phi3 = (6 * e * phi1 * K2 - (2 * pow(sin(f_ref_rad), 2) / pow(k, 2)) - (pow(cos(f_ref_rad), 2) / k) - pow(cos(f_ref_rad), 2)); + double phi1_prime = cos(f_ref_rad) * k - e * pow(sin(f_ref_rad), 2); + double sigma4 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); + double term1_phi2_prime = sin(f_ref_rad) / k; + double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); + double phi2_prime = term1_phi2_prime + term2_phi2_prime; + double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); + double S1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); + double S2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); + double S3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); + stm[0][0] = phi1; + stm[0][1] = phi2; + stm[0][2] = phi3; + stm[0][3] = 0.0; + stm[0][4] = 0.0; + stm[0][5] = 0.0; + stm[1][0] = phi1_prime; + stm[1][1] = phi2_prime; + stm[1][2] = phi3_prime; + stm[1][3] = 0.0; + stm[1][4] = 0.0; + stm[1][5] = 0.0; + stm[2][0] = -2*S1; + stm[2][1] = -2*S2; + stm[2][2] = -S3; + stm[2][3] = -1; + stm[2][4] = 0.0; + stm[2][5] = 0.0; + stm[3][0] = -2*phi1; + stm[3][1] = -2*phi2; + stm[3][2] = -(2*phi3 + 1); + stm[3][3] = 0.0; + stm[3][4] = 0.0; + stm[3][5] = 0.0; + stm[4][0] = 0.0; + stm[4][1] = 0.0; + stm[4][2] = 0.0; + stm[4][3] = 0.0; + stm[4][4] = cos(f_ref_rad); + stm[4][5] = sin(f_ref_rad); + stm[5][0] = 0.0; + stm[5][1] = 0.0; + stm[5][2] = 0.0; + stm[5][3] = 0.0; + stm[5][4] = -sin(f_ref_rad); + stm[5][5] = cos(f_ref_rad); } } // namespace orbit From 5d081c2bb91ff0cfdbc1ac1de51c5b860dcecf89 Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 17:04:50 +0900 Subject: [PATCH 11/34] add class content --- .../orbit/relative_orbit_carter.cpp | 145 +++++++++--------- 1 file changed, 73 insertions(+), 72 deletions(-) diff --git a/src/math_physics/orbit/relative_orbit_carter.cpp b/src/math_physics/orbit/relative_orbit_carter.cpp index b66fa00b2..e3d5874b3 100644 --- a/src/math_physics/orbit/relative_orbit_carter.cpp +++ b/src/math_physics/orbit/relative_orbit_carter.cpp @@ -16,7 +16,7 @@ RelativeOrbitCarter::~RelativeOrbitCarter() {} void RelativeOrbitCarter::CalculateInitialInverseMatrix(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe) { - math::Matrix<6, 6> stm_inv; + math::Matrix<6, 6> initial_inverse_matrix_; double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); double e = reference_oe->GetEccentricity(); double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); @@ -35,42 +35,42 @@ void RelativeOrbitCarter::CalculateInitialInverseMatrix(double orbit_radius_m, d double S1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); double S2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); double S3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); - stm_inv[0][0] = 4*S2 + phi2_prime; - stm_inv[0][1] = -phi2; - stm_inv[0][2] = 0.0; - stm_inv[0][3] = 2*S2; - stm_inv[0][4] = 0.0; - stm_inv[0][5] = 0.0; - stm_inv[1][0] = -(4*S1 + phi1_prime); - stm_inv[1][1] = phi1; - stm_inv[1][2] = 0.0; - stm_inv[1][3] = -2*S1; - stm_inv[1][4] = 0.0; - stm_inv[1][5] = 0.0; - stm_inv[2][0] = -2; - stm_inv[2][1] = 0.0; - stm_inv[2][2] = 0.0; - stm_inv[2][3] = -1; - stm_inv[2][4] = 0.0; - stm_inv[2][5] = 0.0; - stm_inv[3][0] = 2*S3 + phi3_prime; - stm_inv[3][1] = -phi3; - stm_inv[3][2] = -1; - stm_inv[3][3] = S3; - stm_inv[3][4] = 0.0; - stm_inv[3][5] = 0.0; - stm_inv[4][0] = 0.0; - stm_inv[4][1] = 0.0; - stm_inv[4][2] = 0.0; - stm_inv[4][3] = 0.0; - stm_inv[4][4] = cos(f_ref_rad); - stm_inv[4][5] = -sin(f_ref_rad); - stm_inv[5][0] = 0.0; - stm_inv[5][1] = 0.0; - stm_inv[5][2] = 0.0; - stm_inv[5][3] = 0.0; - stm_inv[5][4] = sin(f_ref_rad); - stm_inv[5][5] = cos(f_ref_rad); + initial_inverse_matrix_[0][0] = 4*S2 + phi2_prime; + initial_inverse_matrix_[0][1] = 0.0; + initial_inverse_matrix_[0][2] = 0.0; + initial_inverse_matrix_[0][3] = -phi2; + initial_inverse_matrix_[0][4] = 2*S2; + initial_inverse_matrix_[0][5] = 0.0; + initial_inverse_matrix_[1][0] = 0.0; + initial_inverse_matrix_[1][1] = 0.0; + initial_inverse_matrix_[1][2] = 0.0; + initial_inverse_matrix_[1][3] = -2; + initial_inverse_matrix_[1][4] = -1; + initial_inverse_matrix_[1][5] = 0.0; + initial_inverse_matrix_[2][0] = 0.0; + initial_inverse_matrix_[2][1] = 0.0; + initial_inverse_matrix_[2][2] = cos(f_ref_rad); + initial_inverse_matrix_[2][3] = 0.0; + initial_inverse_matrix_[2][4] = 0.0; + initial_inverse_matrix_[2][5] = -sin(f_ref_rad); + initial_inverse_matrix_[3][0] = -(4*S1 + phi1_prime); + initial_inverse_matrix_[3][1] = 0.0; + initial_inverse_matrix_[3][2] = 0.0; + initial_inverse_matrix_[3][3] = phi1; + initial_inverse_matrix_[3][4] = -2*S1; + initial_inverse_matrix_[3][5] = 0.0; + initial_inverse_matrix_[4][0] = 2*S3 + phi3_prime; + initial_inverse_matrix_[4][1] = 0.0; + initial_inverse_matrix_[4][2] = -1; + initial_inverse_matrix_[4][3] = -phi3; + initial_inverse_matrix_[4][4] = S3; + initial_inverse_matrix_[4][5] = 0.0; + initial_inverse_matrix_[5][0] = 0.0; + initial_inverse_matrix_[5][1] = 0.0; + initial_inverse_matrix_[5][2] = sin(f_ref_rad); + initial_inverse_matrix_[5][3] = 0.0; + initial_inverse_matrix_[5][4] = 0.0; + initial_inverse_matrix_[5][5] = cos(f_ref_rad); } math::Matrix<6, 6> CalculateSTM(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe){ @@ -95,41 +95,42 @@ math::Matrix<6, 6> CalculateSTM(double orbit_radius_m, double gravity_constant_m double S2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); double S3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); stm[0][0] = phi1; - stm[0][1] = phi2; - stm[0][2] = phi3; - stm[0][3] = 0.0; - stm[0][4] = 0.0; - stm[0][5] = 0.0; - stm[1][0] = phi1_prime; - stm[1][1] = phi2_prime; - stm[1][2] = phi3_prime; - stm[1][3] = 0.0; - stm[1][4] = 0.0; - stm[1][5] = 0.0; - stm[2][0] = -2*S1; - stm[2][1] = -2*S2; - stm[2][2] = -S3; - stm[2][3] = -1; - stm[2][4] = 0.0; - stm[2][5] = 0.0; - stm[3][0] = -2*phi1; - stm[3][1] = -2*phi2; - stm[3][2] = -(2*phi3 + 1); - stm[3][3] = 0.0; - stm[3][4] = 0.0; - stm[3][5] = 0.0; - stm[4][0] = 0.0; - stm[4][1] = 0.0; - stm[4][2] = 0.0; - stm[4][3] = 0.0; - stm[4][4] = cos(f_ref_rad); - stm[4][5] = sin(f_ref_rad); - stm[5][0] = 0.0; - stm[5][1] = 0.0; - stm[5][2] = 0.0; - stm[5][3] = 0.0; - stm[5][4] = -sin(f_ref_rad); - stm[5][5] = cos(f_ref_rad); + stm[0][1] = phi3; + stm[0][2] = 0.0; + stm[0][3] = phi2; + stm[0][4] = 0.0; + stm[0][5] = 0.0; + stm[1][0] = phi1_prime; + stm[1][1] = phi3_prime; + stm[1][2] = 0.0; + stm[1][3] = phi2_prime; + stm[1][4] = 0.0; + stm[1][5] = 0.0; + stm[2][0] = -2*S1; + stm[2][1] = -S3; + stm[2][2] = 0.0; + stm[2][3] = -2*S2; + stm[2][4] = -1; + stm[2][5] = 0.0; + stm[3][0] = -2*phi1; + stm[3][1] = -(2*phi3 + 1); + stm[3][2] = 0.0; + stm[3][3] = -2*phi2; + stm[3][4] = 0.0; + stm[3][5] = 0.0; + stm[4][0] = 0.0; + stm[4][1] = 0.0; + stm[4][2] = 0.0; + stm[4][3] = 0.0; + stm[4][4] = cos(f_ref_rad); + stm[4][5] = sin(f_ref_rad); + stm[5][0] = 0.0; + stm[5][1] = 0.0; + stm[5][2] = 0.0; + stm[5][3] = 0.0; + stm[5][4] = -sin(f_ref_rad); + stm[5][5] = cos(f_ref_rad); + return stm * initial_inverse_matrix_; } } // namespace orbit From cb9ab1799796a19ce308fa976deaad0eb216a143 Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 17:37:56 +0900 Subject: [PATCH 12/34] follow format --- .../orbit/relative_orbit_carter.cpp | 9 +- .../orbit/relative_orbit_models.cpp | 96 +------------------ 2 files changed, 6 insertions(+), 99 deletions(-) diff --git a/src/math_physics/orbit/relative_orbit_carter.cpp b/src/math_physics/orbit/relative_orbit_carter.cpp index e3d5874b3..2264e0276 100644 --- a/src/math_physics/orbit/relative_orbit_carter.cpp +++ b/src/math_physics/orbit/relative_orbit_carter.cpp @@ -16,7 +16,6 @@ RelativeOrbitCarter::~RelativeOrbitCarter() {} void RelativeOrbitCarter::CalculateInitialInverseMatrix(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe) { - math::Matrix<6, 6> initial_inverse_matrix_; double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); double e = reference_oe->GetEccentricity(); double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); @@ -31,7 +30,8 @@ void RelativeOrbitCarter::CalculateInitialInverseMatrix(double orbit_radius_m, d double term1_phi2_prime = sin(f_ref_rad) / k; double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); double phi2_prime = term1_phi2_prime + term2_phi2_prime; - double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); + double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) + - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); double S1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); double S2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); double S3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); @@ -73,7 +73,7 @@ void RelativeOrbitCarter::CalculateInitialInverseMatrix(double orbit_radius_m, d initial_inverse_matrix_[5][5] = cos(f_ref_rad); } -math::Matrix<6, 6> CalculateSTM(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe){ +math::Matrix<6, 6> RelativeOrbitCarter::CalculateSTM(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe){ math::Matrix<6, 6> stm; // ここでstmを計算してください double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); @@ -90,7 +90,8 @@ math::Matrix<6, 6> CalculateSTM(double orbit_radius_m, double gravity_constant_m double term1_phi2_prime = sin(f_ref_rad) / k; double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); double phi2_prime = term1_phi2_prime + term2_phi2_prime; - double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); + double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) + - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); double S1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); double S2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); double S3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index 1eef8c50d..e96f7387a 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -129,103 +129,9 @@ math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constan return stm; } -math::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe){ +math::Matrix<6, 6> CalcYamakawaAnkersenStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe) { math::Matrix<6, 6> stm; - math::Matrix<6, 6> stm_inv; // ここでstmを計算してください - double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); - double e = reference_oe->GetEccentricity(); - double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); - double k = e * cos(f_ref_rad) + 1; - double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); - double K2 = pow(1 - e * e, -2.5) * (0.5 * E_rad - 0.25 * sin(2 * E_rad) - e * pow(sin(E_rad), 3) / 3.); - double phi1 = sin(f_ref_rad) * k; - double phi2 = 2 * e * phi1 * (sin(f_ref_rad) / pow(k, 3) - K2) - cos(f_ref_rad) / k; - double phi3 = (6 * e * phi1 * K2 - (2 * pow(sin(f_ref_rad), 2) / pow(k, 2)) - (pow(cos(f_ref_rad), 2) / k) - pow(cos(f_ref_rad), 2)); - double phi1_prime = cos(f_ref_rad) * k - e * pow(sin(f_ref_rad), 2); - double sigma4 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); - double term1_phi2_prime = sin(f_ref_rad) / k; - double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); - double phi2_prime = term1_phi2_prime + term2_phi2_prime; - double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); - double S1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); - double S2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); - double S3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); - stm[0][0] = phi1; - stm[0][1] = phi2; - stm[0][2] = phi3; - stm[0][3] = 0.0; - stm[0][4] = 0.0; - stm[0][5] = 0.0; - stm[1][0] = phi1_prime; - stm[1][1] = phi2_prime; - stm[1][2] = phi3_prime; - stm[1][3] = 0.0; - stm[1][4] = 0.0; - stm[1][5] = 0.0; - stm[2][0] = -2*S1; - stm[2][1] = -2*S2; - stm[2][2] = -S3; - stm[2][3] = -1; - stm[2][4] = 0.0; - stm[2][5] = 0.0; - stm[3][0] = -2*phi1; - stm[3][1] = -2*phi2; - stm[3][2] = -(2*phi3 + 1); - stm[3][3] = 0.0; - stm[3][4] = 0.0; - stm[3][5] = 0.0; - stm[4][0] = 0.0; - stm[4][1] = 0.0; - stm[4][2] = 0.0; - stm[4][3] = 0.0; - stm[4][4] = cos(f_ref_rad); - stm[4][5] = sin(f_ref_rad); - stm[5][0] = 0.0; - stm[5][1] = 0.0; - stm[5][2] = 0.0; - stm[5][3] = 0.0; - stm[5][4] = -sin(f_ref_rad); - stm[5][5] = cos(f_ref_rad); - - stm_inv[0][0] = 4*S2 + phi2_prime; - stm_inv[0][1] = -phi2; - stm_inv[0][2] = 0.0; - stm_inv[0][3] = 2*S2; - stm_inv[0][4] = 0.0; - stm_inv[0][5] = 0.0; - stm_inv[1][0] = -(4*S1 + phi1_prime); - stm_inv[1][1] = phi1; - stm_inv[1][2] = 0.0; - stm_inv[1][3] = -2*S1; - stm_inv[1][4] = 0.0; - stm_inv[1][5] = 0.0; - stm_inv[2][0] = -2; - stm_inv[2][1] = 0.0; - stm_inv[2][2] = 0.0; - stm_inv[2][3] = -1; - stm_inv[2][4] = 0.0; - stm_inv[2][5] = 0.0; - stm_inv[3][0] = 2*S3 + phi3_prime; - stm_inv[3][1] = -phi3; - stm_inv[3][2] = -1; - stm_inv[3][3] = S3; - stm_inv[3][4] = 0.0; - stm_inv[3][5] = 0.0; - stm_inv[4][0] = 0.0; - stm_inv[4][1] = 0.0; - stm_inv[4][2] = 0.0; - stm_inv[4][3] = 0.0; - stm_inv[4][4] = cos(f_ref_rad); - stm_inv[4][5] = -sin(f_ref_rad); - stm_inv[5][0] = 0.0; - stm_inv[5][1] = 0.0; - stm_inv[5][2] = 0.0; - stm_inv[5][3] = 0.0; - stm_inv[5][4] = sin(f_ref_rad); - stm_inv[5][5] = cos(f_ref_rad); - return stm; } - } // namespace orbit From cab2b06a2fa6b297bd325e70a527b1e662c3fac1 Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 17:43:52 +0900 Subject: [PATCH 13/34] follow format --- .../orbit/relative_orbit_carter.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/math_physics/orbit/relative_orbit_carter.cpp b/src/math_physics/orbit/relative_orbit_carter.cpp index 2264e0276..ab125fe1e 100644 --- a/src/math_physics/orbit/relative_orbit_carter.cpp +++ b/src/math_physics/orbit/relative_orbit_carter.cpp @@ -39,7 +39,7 @@ void RelativeOrbitCarter::CalculateInitialInverseMatrix(double orbit_radius_m, d initial_inverse_matrix_[0][1] = 0.0; initial_inverse_matrix_[0][2] = 0.0; initial_inverse_matrix_[0][3] = -phi2; - initial_inverse_matrix_[0][4] = 2*S2; + initial_inverse_matrix_[0][4] = 2 * S2; initial_inverse_matrix_[0][5] = 0.0; initial_inverse_matrix_[1][0] = 0.0; initial_inverse_matrix_[1][1] = 0.0; @@ -53,13 +53,13 @@ void RelativeOrbitCarter::CalculateInitialInverseMatrix(double orbit_radius_m, d initial_inverse_matrix_[2][3] = 0.0; initial_inverse_matrix_[2][4] = 0.0; initial_inverse_matrix_[2][5] = -sin(f_ref_rad); - initial_inverse_matrix_[3][0] = -(4*S1 + phi1_prime); + initial_inverse_matrix_[3][0] = -(4 * S1 + phi1_prime); initial_inverse_matrix_[3][1] = 0.0; initial_inverse_matrix_[3][2] = 0.0; initial_inverse_matrix_[3][3] = phi1; - initial_inverse_matrix_[3][4] = -2*S1; + initial_inverse_matrix_[3][4] = -2 * S1; initial_inverse_matrix_[3][5] = 0.0; - initial_inverse_matrix_[4][0] = 2*S3 + phi3_prime; + initial_inverse_matrix_[4][0] = 2 * S3 + phi3_prime; initial_inverse_matrix_[4][1] = 0.0; initial_inverse_matrix_[4][2] = -1; initial_inverse_matrix_[4][3] = -phi3; @@ -107,16 +107,16 @@ math::Matrix<6, 6> RelativeOrbitCarter::CalculateSTM(double orbit_radius_m, doub stm[1][3] = phi2_prime; stm[1][4] = 0.0; stm[1][5] = 0.0; - stm[2][0] = -2*S1; + stm[2][0] = -2 * S1; stm[2][1] = -S3; stm[2][2] = 0.0; - stm[2][3] = -2*S2; + stm[2][3] = -2 * S2; stm[2][4] = -1; stm[2][5] = 0.0; - stm[3][0] = -2*phi1; - stm[3][1] = -(2*phi3 + 1); + stm[3][0] = -2 * phi1; + stm[3][1] = -(2 * phi3 + 1); stm[3][2] = 0.0; - stm[3][3] = -2*phi2; + stm[3][3] = -2 * phi2; stm[3][4] = 0.0; stm[3][5] = 0.0; stm[4][0] = 0.0; From 110aea8d6443f68e3f2b8eb7e8ef3c3b8aab61b1 Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 18:02:13 +0900 Subject: [PATCH 14/34] comment out old function --- src/dynamics/orbit/relative_orbit.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index bad4c53c7..43f595adf 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -143,7 +143,7 @@ void RelativeOrbit::CalculateStm(orbit::StmModel stm_model_type, const Orbit* re break; } case orbit::StmModel::kCarter: { - stm_ = orbit::CalcCarterStm(reference_sat_orbit_radius, gravity_constant_m3_s2, f_ref_rad, &reference_oe); + // stm_ = orbit::CalcCarterStm(reference_sat_orbit_radius, gravity_constant_m3_s2, f_ref_rad, &reference_oe); break; } case orbit::StmModel::kYamakawaAnkersen: { From fdaa036248959d3bc3e573a06b2f50d0daff3072 Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 18:26:28 +0900 Subject: [PATCH 15/34] add carter to relative_orbit.cpp --- src/dynamics/orbit/relative_orbit.cpp | 5 ++++- src/dynamics/orbit/relative_orbit.hpp | 4 +++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index 43f595adf..7726195b4 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -102,6 +102,9 @@ void RelativeOrbit::InitializeStmMatrix(orbit::StmModel stm_model_type, const Or case orbit::StmModel::kYamakawaAnkersen: relative_orbit_yamanaka_ankersen_.CalculateInitialInverseMatrix(f_ref_rad, &reference_oe); break; + case orbit::StmModel::kCarter: + relative_orbit_carter_->CalculateInitialInverseMatrix(reference_sat_orbit_radius, gravity_constant_m3_s2, f_ref_rad, &reference_oe); + break; default: break; @@ -143,7 +146,7 @@ void RelativeOrbit::CalculateStm(orbit::StmModel stm_model_type, const Orbit* re break; } case orbit::StmModel::kCarter: { - // stm_ = orbit::CalcCarterStm(reference_sat_orbit_radius, gravity_constant_m3_s2, f_ref_rad, &reference_oe); + stm_ = relative_orbit_carter_->CalculateSTM(gravity_constant_m3_s2, elapsed_sec, f_ref_rad, &reference_oe); break; } case orbit::StmModel::kYamakawaAnkersen: { diff --git a/src/dynamics/orbit/relative_orbit.hpp b/src/dynamics/orbit/relative_orbit.hpp index 44bb44f7b..8fca6007a 100644 --- a/src/dynamics/orbit/relative_orbit.hpp +++ b/src/dynamics/orbit/relative_orbit.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -86,7 +87,8 @@ class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> orbit::RelativeOrbitModel relative_dynamics_model_type_; //!< Relative dynamics model type orbit::StmModel stm_model_type_; //!< State Transition Matrix model type RelativeInformation* relative_information_; //!< Relative information - orbit::RelativeOrbitYamanakaAnkersen relative_orbit_yamanaka_ankersen_; //!< Relative Orbit Calcilater with Yamanaka-Ankersen's STM + orbit::RelativeOrbitYamanakaAnkersen* relative_orbit_yamanaka_ankersen_; //!< Relative Orbit Calcilater with Yamanaka-Ankersen's STM + orbit::RelativeOrbitCarter* relative_orbit_carter_; //!< Relative Orbit Calcilater with Carter's STM /** * @fn InitializeState From dba54a87c500c2d5507a5f34cde268f734171d3c Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 15:11:25 +0900 Subject: [PATCH 16/34] add empty stm --- .../orbit/relative_orbit_models.cpp | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index e96f7387a..7c7dd124f 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -126,6 +126,43 @@ math::Vector<6> CalcSsCorrectionTerm(double orbit_radius_m, double gravity_const math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, OrbitalElements* reference_oe) { math::Matrix<6, 6> stm; // ここでstmを計算してください + double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); + stm[0][0] = 0.0; + stm[0][1] = 0.0; + stm[0][2] = 0.0; + stm[0][3] = 0.0; + stm[0][4] = 0.0; + stm[0][5] = 0.0; + stm[1][0] = 0.0; + stm[1][1] = 0.0; + stm[1][2] = 0.0; + stm[1][3] = 0.0; + stm[1][4] = 0.0; + stm[1][5] = 0.0; + stm[2][0] = 0.0; + stm[2][1] = 0.0; + stm[2][2] = 0.0; + stm[2][3] = 0.0; + stm[2][4] = 0.0; + stm[2][5] = 0.0; + stm[3][0] = 0.0; + stm[3][1] = 0.0; + stm[3][2] = 0.0; + stm[3][3] = 0.0; + stm[3][4] = 0.0; + stm[3][5] = 0.0; + stm[4][0] = 0.0; + stm[4][1] = 0.0; + stm[4][2] = 0.0; + stm[4][3] = 0.0; + stm[4][4] = 0.0; + stm[4][5] = 0.0; + stm[5][0] = 0.0; + stm[5][1] = 0.0; + stm[5][2] = 0.0; + stm[5][3] = 0.0; + stm[5][4] = 0.0; + stm[5][5] = 0.0; return stm; } From 669e8178396c38ed506f0be6578d1784adf4c60b Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 15:40:48 +0900 Subject: [PATCH 17/34] add params calculation --- .../orbit/relative_orbit_models.cpp | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index 7c7dd124f..95a9444e4 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -127,6 +127,31 @@ math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constan math::Matrix<6, 6> stm; // ここでstmを計算してください double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); + double e = reference_oe->GetEccentricity(); + double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); + double k = 1 + e * cos(f_ref_rad); + double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); + double K2 = pow(1 - e * e, -2.5) * (0.5 * E_rad - 0.25 * sin(2 * E_rad) - e * pow(sin(E_rad), 3) / 3.); + double phi1 = sin(f_ref_rad) * (e * cos(f_ref_rad) + 1); + double sigma1 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); + double term1_phi2 = 2 * e * sin(f_ref_rad) * (e * cos(f_ref_rad) + 1); + double term2_phi2 = (e * pow(sin(2 * sigma1), 3) / 3 + sin(4 * sigma1) / 4 - sigma1) / pow(1 - pow(e, 2), 5.0 / 2.0); + double term3_phi2 = sin(f_ref_rad) / pow(e * cos(f_ref_rad) + 1, 3); + double term4_phi2 = cos(f_ref_rad) / (e * cos(f_ref_rad) + 1); + double phi2 = term1_phi2 * (term2_phi2 + term3_phi2) - term4_phi2; + double sigma2 = e * cos(f_ref_rad) + 1; + double term1_phi3 = pow(cos(f_ref_rad), 2); + double term2_phi3 = 6 * e * sin(f_ref_rad) * sigma2 * (e * pow(sin(2 * sigma1), 3) / 3 + sin(4 * sigma1) / 4 - sigma1) / pow(1 - pow(e, 2), 5.0 / 2.0); + double phi3 = -term1_phi3 - (term1_phi3 / sigma2) - (2 * pow(sin(f_ref_rad), 2) / pow(sigma2, 2)) - term2_phi3; + double phi1_prime = cos(f_ref_rad) * (e * cos(f_ref_rad) + 1) - e * pow(sin(f_ref_rad), 2); + double sigma4 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); + double term1_phi2_prime = sin(f_ref_rad) / sigma2; + double term2_phi2_prime = 2 * e * sin(f_ref_rad) * sigma2 * (cos(f_ref_rad) / pow(sigma2, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); + double phi2_prime = term1_phi2_prime + term2_phi2_prime; + double phi3_prime = sigma2 - 4 * e * pow(sin(f_ref_rad), 3) / pow(sigma1, 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(sigma1, 2) - 6 * e * cos(f_ref_rad) * sigma1 / pow(1 - pow(e, 2), 5.0 / 2.0); + double S_phi1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); + double S_phi2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); + double S_phi3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); stm[0][0] = 0.0; stm[0][1] = 0.0; stm[0][2] = 0.0; From 19ce846b71eb72fa8c0f6aa1f9f6ca520cfb6041 Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 16:06:46 +0900 Subject: [PATCH 18/34] refac formulation --- .../orbit/relative_orbit_models.cpp | 24 +++++++------------ 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index 95a9444e4..b240f94ca 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -129,26 +129,18 @@ math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constan double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); double e = reference_oe->GetEccentricity(); double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); - double k = 1 + e * cos(f_ref_rad); + double k = e * cos(f_ref_rad) + 1; double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); double K2 = pow(1 - e * e, -2.5) * (0.5 * E_rad - 0.25 * sin(2 * E_rad) - e * pow(sin(E_rad), 3) / 3.); - double phi1 = sin(f_ref_rad) * (e * cos(f_ref_rad) + 1); - double sigma1 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); - double term1_phi2 = 2 * e * sin(f_ref_rad) * (e * cos(f_ref_rad) + 1); - double term2_phi2 = (e * pow(sin(2 * sigma1), 3) / 3 + sin(4 * sigma1) / 4 - sigma1) / pow(1 - pow(e, 2), 5.0 / 2.0); - double term3_phi2 = sin(f_ref_rad) / pow(e * cos(f_ref_rad) + 1, 3); - double term4_phi2 = cos(f_ref_rad) / (e * cos(f_ref_rad) + 1); - double phi2 = term1_phi2 * (term2_phi2 + term3_phi2) - term4_phi2; - double sigma2 = e * cos(f_ref_rad) + 1; - double term1_phi3 = pow(cos(f_ref_rad), 2); - double term2_phi3 = 6 * e * sin(f_ref_rad) * sigma2 * (e * pow(sin(2 * sigma1), 3) / 3 + sin(4 * sigma1) / 4 - sigma1) / pow(1 - pow(e, 2), 5.0 / 2.0); - double phi3 = -term1_phi3 - (term1_phi3 / sigma2) - (2 * pow(sin(f_ref_rad), 2) / pow(sigma2, 2)) - term2_phi3; - double phi1_prime = cos(f_ref_rad) * (e * cos(f_ref_rad) + 1) - e * pow(sin(f_ref_rad), 2); + double phi1 = sin(f_ref_rad) * k; + double phi2 = 2 * e * phi1 * (sin(f_ref_rad) / pow(k, 3) - K2) - cos(f_ref_rad) / k; + double phi3 = (6 * e * phi1 * K2 - (2 * pow(sin(f_ref_rad), 2) / pow(k, 2)) - (pow(cos(f_ref_rad), 2) / k) - pow(cos(f_ref_rad), 2)); + double phi1_prime = cos(f_ref_rad) * k - e * pow(sin(f_ref_rad), 2); double sigma4 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); - double term1_phi2_prime = sin(f_ref_rad) / sigma2; - double term2_phi2_prime = 2 * e * sin(f_ref_rad) * sigma2 * (cos(f_ref_rad) / pow(sigma2, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); + double term1_phi2_prime = sin(f_ref_rad) / k; + double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); double phi2_prime = term1_phi2_prime + term2_phi2_prime; - double phi3_prime = sigma2 - 4 * e * pow(sin(f_ref_rad), 3) / pow(sigma1, 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(sigma1, 2) - 6 * e * cos(f_ref_rad) * sigma1 / pow(1 - pow(e, 2), 5.0 / 2.0); + double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); double S_phi1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); double S_phi2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); double S_phi3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); From 538e3b3a490861a059cf8115813a4ad59d6244cc Mon Sep 17 00:00:00 2001 From: YuseiAlexS Date: Mon, 23 Sep 2024 16:21:44 +0900 Subject: [PATCH 19/34] stm + stm_inv --- .../orbit/relative_orbit_models.cpp | 86 ++++++++++++++----- 1 file changed, 65 insertions(+), 21 deletions(-) diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index b240f94ca..f4912c436 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -117,13 +117,19 @@ math::Matrix<6, 6> CalcSsStm(double orbit_radius_m, double gravity_constant_m3_s return stm; } +math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, OrbitalElements* reference_oe) { + math::Matrix<6, 6> stm; + // ここでstmを計算してください + return stm; +} + math::Vector<6> CalcSsCorrectionTerm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, OrbitalElements* reference_oe) { math::Vector<6> correction_term; // ここでstmを計算してください return correction_term; } -math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, OrbitalElements* reference_oe) { +math::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe){ math::Matrix<6, 6> stm; // ここでstmを計算してください double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); @@ -141,30 +147,30 @@ math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constan double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); double phi2_prime = term1_phi2_prime + term2_phi2_prime; double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); - double S_phi1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); - double S_phi2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); - double S_phi3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); - stm[0][0] = 0.0; - stm[0][1] = 0.0; - stm[0][2] = 0.0; + double S1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); + double S2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); + double S3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); + stm[0][0] = phi1; + stm[0][1] = phi2; + stm[0][2] = phi3; stm[0][3] = 0.0; stm[0][4] = 0.0; stm[0][5] = 0.0; - stm[1][0] = 0.0; - stm[1][1] = 0.0; - stm[1][2] = 0.0; + stm[1][0] = phi1_prime; + stm[1][1] = phi2_prime; + stm[1][2] = phi3_prime; stm[1][3] = 0.0; stm[1][4] = 0.0; stm[1][5] = 0.0; - stm[2][0] = 0.0; - stm[2][1] = 0.0; - stm[2][2] = 0.0; - stm[2][3] = 0.0; + stm[2][0] = -2*S1; + stm[2][1] = -2*S2; + stm[2][2] = -S3; + stm[2][3] = -1; stm[2][4] = 0.0; stm[2][5] = 0.0; - stm[3][0] = 0.0; - stm[3][1] = 0.0; - stm[3][2] = 0.0; + stm[3][0] = -2*phi1; + stm[3][1] = -2*phi2; + stm[3][2] = -(2*phi3 + 1); stm[3][3] = 0.0; stm[3][4] = 0.0; stm[3][5] = 0.0; @@ -172,14 +178,52 @@ math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constan stm[4][1] = 0.0; stm[4][2] = 0.0; stm[4][3] = 0.0; - stm[4][4] = 0.0; - stm[4][5] = 0.0; + stm[4][4] = cos(f_ref_rad); + stm[4][5] = sin(f_ref_rad); stm[5][0] = 0.0; stm[5][1] = 0.0; stm[5][2] = 0.0; stm[5][3] = 0.0; - stm[5][4] = 0.0; - stm[5][5] = 0.0; + stm[5][4] = -sin(f_ref_rad); + stm[5][5] = cos(f_ref_rad); + + stm_inv[0][0] = 4*S2 + phi2_prime; + stm_inv[0][1] = -phi2; + stm_inv[0][2] = 0.0; + stm_inv[0][3] = 2*S2; + stm_inv[0][4] = 0.0; + stm_inv[0][5] = 0.0; + stm_inv[1][0] = -(4*S1 + phi1_prime); + stm_inv[1][1] = phi1; + stm_inv[1][2] = 0.0; + stm_inv[1][3] = -2*S1; + stm_inv[1][4] = 0.0; + stm_inv[1][5] = 0.0; + stm_inv[2][0] = -2; + stm_inv[2][1] = 0.0; + stm_inv[2][2] = 0.0; + stm_inv[2][3] = -1; + stm_inv[2][4] = 0.0; + stm_inv[2][5] = 0.0; + stm_inv[3][0] = 2*S3 + phi3_prime; + stm_inv[3][1] = -phi3; + stm_inv[3][2] = -1; + stm_inv[3][3] = S3; + stm_inv[3][4] = 0.0; + stm_inv[3][5] = 0.0; + stm_inv[4][0] = 0.0; + stm_inv[4][1] = 0.0; + stm_inv[4][2] = 0.0; + stm_inv[4][3] = 0.0; + stm_inv[4][4] = cos(f_ref_rad); + stm_inv[4][5] = -sin(f_ref_rad); + stm_inv[5][0] = 0.0; + stm_inv[5][1] = 0.0; + stm_inv[5][2] = 0.0; + stm_inv[5][3] = 0.0; + stm_inv[5][4] = sin(f_ref_rad); + stm_inv[5][5] = cos(f_ref_rad); + return stm; } From 26531210bed42509ac1ba5938688a678e943bae7 Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 15:10:25 +0900 Subject: [PATCH 20/34] add empty stm --- src/library/orbit/relative_orbit_models.cpp | 89 ++++----------------- 1 file changed, 17 insertions(+), 72 deletions(-) diff --git a/src/library/orbit/relative_orbit_models.cpp b/src/library/orbit/relative_orbit_models.cpp index 4c270e6a9..475450898 100644 --- a/src/library/orbit/relative_orbit_models.cpp +++ b/src/library/orbit/relative_orbit_models.cpp @@ -125,44 +125,27 @@ libra::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant libra::Matrix<6, 6> stm_inv; // ここでstmを計算してください double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); - double e = reference_oe->GetEccentricity(); - double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); - double k = e * cos(f_ref_rad) + 1; - double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); - double K2 = pow(1 - e * e, -2.5) * (0.5 * E_rad - 0.25 * sin(2 * E_rad) - e * pow(sin(E_rad), 3) / 3.); - double phi1 = sin(f_ref_rad) * k; - double phi2 = 2 * e * phi1 * (sin(f_ref_rad) / pow(k, 3) - K2) - cos(f_ref_rad) / k; - double phi3 = (6 * e * phi1 * K2 - (2 * pow(sin(f_ref_rad), 2) / pow(k, 2)) - (pow(cos(f_ref_rad), 2) / k) - pow(cos(f_ref_rad), 2)); - double phi1_prime = cos(f_ref_rad) * k - e * pow(sin(f_ref_rad), 2); - double sigma4 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); - double term1_phi2_prime = sin(f_ref_rad) / k; - double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); - double phi2_prime = term1_phi2_prime + term2_phi2_prime; - double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); - double S1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); - double S2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); - double S3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); - stm[0][0] = phi1; - stm[0][1] = phi2; - stm[0][2] = phi3; + stm[0][0] = 0.0; + stm[0][1] = 0.0; + stm[0][2] = 0.0; stm[0][3] = 0.0; stm[0][4] = 0.0; stm[0][5] = 0.0; - stm[1][0] = phi1_prime; - stm[1][1] = phi2_prime; - stm[1][2] = phi3_prime; + stm[1][0] = 0.0; + stm[1][1] = 0.0; + stm[1][2] = 0.0; stm[1][3] = 0.0; stm[1][4] = 0.0; stm[1][5] = 0.0; - stm[2][0] = -2*S1; - stm[2][1] = -2*S2; - stm[2][2] = -S3; - stm[2][3] = -1; + stm[2][0] = 0.0; + stm[2][1] = 0.0; + stm[2][2] = 0.0; + stm[2][3] = 0.0; stm[2][4] = 0.0; stm[2][5] = 0.0; - stm[3][0] = -2*phi1; - stm[3][1] = -2*phi2; - stm[3][2] = -(2*phi3 + 1); + stm[3][0] = 0.0; + stm[3][1] = 0.0; + stm[3][2] = 0.0; stm[3][3] = 0.0; stm[3][4] = 0.0; stm[3][5] = 0.0; @@ -170,52 +153,14 @@ libra::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant stm[4][1] = 0.0; stm[4][2] = 0.0; stm[4][3] = 0.0; - stm[4][4] = cos(f_ref_rad); - stm[4][5] = sin(f_ref_rad); + stm[4][4] = 0.0; + stm[4][5] = 0.0; stm[5][0] = 0.0; stm[5][1] = 0.0; stm[5][2] = 0.0; stm[5][3] = 0.0; - stm[5][4] = -sin(f_ref_rad); - stm[5][5] = cos(f_ref_rad); - - stm_inv[0][0] = 4*S2 + phi2_prime; - stm_inv[0][1] = -phi2; - stm_inv[0][2] = 0.0; - stm_inv[0][3] = 2*S2; - stm_inv[0][4] = 0.0; - stm_inv[0][5] = 0.0; - stm_inv[1][0] = -(4*S1 + phi1_prime); - stm_inv[1][1] = phi1; - stm_inv[1][2] = 0.0; - stm_inv[1][3] = -2*S1; - stm_inv[1][4] = 0.0; - stm_inv[1][5] = 0.0; - stm_inv[2][0] = -2; - stm_inv[2][1] = 0.0; - stm_inv[2][2] = 0.0; - stm_inv[2][3] = -1; - stm_inv[2][4] = 0.0; - stm_inv[2][5] = 0.0; - stm_inv[3][0] = 2*S3 + phi3_prime; - stm_inv[3][1] = -phi3; - stm_inv[3][2] = -1; - stm_inv[3][3] = S3; - stm_inv[3][4] = 0.0; - stm_inv[3][5] = 0.0; - stm_inv[4][0] = 0.0; - stm_inv[4][1] = 0.0; - stm_inv[4][2] = 0.0; - stm_inv[4][3] = 0.0; - stm_inv[4][4] = cos(f_ref_rad); - stm_inv[4][5] = -sin(f_ref_rad); - stm_inv[5][0] = 0.0; - stm_inv[5][1] = 0.0; - stm_inv[5][2] = 0.0; - stm_inv[5][3] = 0.0; - stm_inv[5][4] = sin(f_ref_rad); - stm_inv[5][5] = cos(f_ref_rad); - + stm[5][4] = 0.0; + stm[5][5] = 0.0; return stm; } From 8c0f77f7054b0f1f1818925de8f032c72ac7f79e Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 15:40:48 +0900 Subject: [PATCH 21/34] add params calculation --- src/library/orbit/relative_orbit_models.cpp | 25 +++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/library/orbit/relative_orbit_models.cpp b/src/library/orbit/relative_orbit_models.cpp index 475450898..d44ec3b7e 100644 --- a/src/library/orbit/relative_orbit_models.cpp +++ b/src/library/orbit/relative_orbit_models.cpp @@ -125,6 +125,31 @@ libra::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant libra::Matrix<6, 6> stm_inv; // ここでstmを計算してください double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); + double e = reference_oe->GetEccentricity(); + double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); + double k = 1 + e * cos(f_ref_rad); + double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); + double K2 = pow(1 - e * e, -2.5) * (0.5 * E_rad - 0.25 * sin(2 * E_rad) - e * pow(sin(E_rad), 3) / 3.); + double phi1 = sin(f_ref_rad) * (e * cos(f_ref_rad) + 1); + double sigma1 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); + double term1_phi2 = 2 * e * sin(f_ref_rad) * (e * cos(f_ref_rad) + 1); + double term2_phi2 = (e * pow(sin(2 * sigma1), 3) / 3 + sin(4 * sigma1) / 4 - sigma1) / pow(1 - pow(e, 2), 5.0 / 2.0); + double term3_phi2 = sin(f_ref_rad) / pow(e * cos(f_ref_rad) + 1, 3); + double term4_phi2 = cos(f_ref_rad) / (e * cos(f_ref_rad) + 1); + double phi2 = term1_phi2 * (term2_phi2 + term3_phi2) - term4_phi2; + double sigma2 = e * cos(f_ref_rad) + 1; + double term1_phi3 = pow(cos(f_ref_rad), 2); + double term2_phi3 = 6 * e * sin(f_ref_rad) * sigma2 * (e * pow(sin(2 * sigma1), 3) / 3 + sin(4 * sigma1) / 4 - sigma1) / pow(1 - pow(e, 2), 5.0 / 2.0); + double phi3 = -term1_phi3 - (term1_phi3 / sigma2) - (2 * pow(sin(f_ref_rad), 2) / pow(sigma2, 2)) - term2_phi3; + double phi1_prime = cos(f_ref_rad) * (e * cos(f_ref_rad) + 1) - e * pow(sin(f_ref_rad), 2); + double sigma4 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); + double term1_phi2_prime = sin(f_ref_rad) / sigma2; + double term2_phi2_prime = 2 * e * sin(f_ref_rad) * sigma2 * (cos(f_ref_rad) / pow(sigma2, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); + double phi2_prime = term1_phi2_prime + term2_phi2_prime; + double phi3_prime = sigma2 - 4 * e * pow(sin(f_ref_rad), 3) / pow(sigma1, 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(sigma1, 2) - 6 * e * cos(f_ref_rad) * sigma1 / pow(1 - pow(e, 2), 5.0 / 2.0); + double S_phi1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); + double S_phi2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); + double S_phi3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); stm[0][0] = 0.0; stm[0][1] = 0.0; stm[0][2] = 0.0; From 08e3274ce2cf952bc56ca53a71f07a6be83e44fc Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 16:06:46 +0900 Subject: [PATCH 22/34] refac formulation --- src/library/orbit/relative_orbit_models.cpp | 24 +++++++-------------- 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/src/library/orbit/relative_orbit_models.cpp b/src/library/orbit/relative_orbit_models.cpp index d44ec3b7e..0e43a2d36 100644 --- a/src/library/orbit/relative_orbit_models.cpp +++ b/src/library/orbit/relative_orbit_models.cpp @@ -127,26 +127,18 @@ libra::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); double e = reference_oe->GetEccentricity(); double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); - double k = 1 + e * cos(f_ref_rad); + double k = e * cos(f_ref_rad) + 1; double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); double K2 = pow(1 - e * e, -2.5) * (0.5 * E_rad - 0.25 * sin(2 * E_rad) - e * pow(sin(E_rad), 3) / 3.); - double phi1 = sin(f_ref_rad) * (e * cos(f_ref_rad) + 1); - double sigma1 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); - double term1_phi2 = 2 * e * sin(f_ref_rad) * (e * cos(f_ref_rad) + 1); - double term2_phi2 = (e * pow(sin(2 * sigma1), 3) / 3 + sin(4 * sigma1) / 4 - sigma1) / pow(1 - pow(e, 2), 5.0 / 2.0); - double term3_phi2 = sin(f_ref_rad) / pow(e * cos(f_ref_rad) + 1, 3); - double term4_phi2 = cos(f_ref_rad) / (e * cos(f_ref_rad) + 1); - double phi2 = term1_phi2 * (term2_phi2 + term3_phi2) - term4_phi2; - double sigma2 = e * cos(f_ref_rad) + 1; - double term1_phi3 = pow(cos(f_ref_rad), 2); - double term2_phi3 = 6 * e * sin(f_ref_rad) * sigma2 * (e * pow(sin(2 * sigma1), 3) / 3 + sin(4 * sigma1) / 4 - sigma1) / pow(1 - pow(e, 2), 5.0 / 2.0); - double phi3 = -term1_phi3 - (term1_phi3 / sigma2) - (2 * pow(sin(f_ref_rad), 2) / pow(sigma2, 2)) - term2_phi3; - double phi1_prime = cos(f_ref_rad) * (e * cos(f_ref_rad) + 1) - e * pow(sin(f_ref_rad), 2); + double phi1 = sin(f_ref_rad) * k; + double phi2 = 2 * e * phi1 * (sin(f_ref_rad) / pow(k, 3) - K2) - cos(f_ref_rad) / k; + double phi3 = (6 * e * phi1 * K2 - (2 * pow(sin(f_ref_rad), 2) / pow(k, 2)) - (pow(cos(f_ref_rad), 2) / k) - pow(cos(f_ref_rad), 2)); + double phi1_prime = cos(f_ref_rad) * k - e * pow(sin(f_ref_rad), 2); double sigma4 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); - double term1_phi2_prime = sin(f_ref_rad) / sigma2; - double term2_phi2_prime = 2 * e * sin(f_ref_rad) * sigma2 * (cos(f_ref_rad) / pow(sigma2, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); + double term1_phi2_prime = sin(f_ref_rad) / k; + double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); double phi2_prime = term1_phi2_prime + term2_phi2_prime; - double phi3_prime = sigma2 - 4 * e * pow(sin(f_ref_rad), 3) / pow(sigma1, 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(sigma1, 2) - 6 * e * cos(f_ref_rad) * sigma1 / pow(1 - pow(e, 2), 5.0 / 2.0); + double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); double S_phi1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); double S_phi2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); double S_phi3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); From 1c349e5775a74fb7394f8a9788ad572d109450a2 Mon Sep 17 00:00:00 2001 From: YuseiAlexS Date: Mon, 23 Sep 2024 16:21:44 +0900 Subject: [PATCH 23/34] stm + stm_inv --- src/library/orbit/relative_orbit_models.cpp | 78 +++++++++++++++------ 1 file changed, 58 insertions(+), 20 deletions(-) diff --git a/src/library/orbit/relative_orbit_models.cpp b/src/library/orbit/relative_orbit_models.cpp index 0e43a2d36..4c270e6a9 100644 --- a/src/library/orbit/relative_orbit_models.cpp +++ b/src/library/orbit/relative_orbit_models.cpp @@ -139,30 +139,30 @@ libra::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); double phi2_prime = term1_phi2_prime + term2_phi2_prime; double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); - double S_phi1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); - double S_phi2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); - double S_phi3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); - stm[0][0] = 0.0; - stm[0][1] = 0.0; - stm[0][2] = 0.0; + double S1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); + double S2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); + double S3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); + stm[0][0] = phi1; + stm[0][1] = phi2; + stm[0][2] = phi3; stm[0][3] = 0.0; stm[0][4] = 0.0; stm[0][5] = 0.0; - stm[1][0] = 0.0; - stm[1][1] = 0.0; - stm[1][2] = 0.0; + stm[1][0] = phi1_prime; + stm[1][1] = phi2_prime; + stm[1][2] = phi3_prime; stm[1][3] = 0.0; stm[1][4] = 0.0; stm[1][5] = 0.0; - stm[2][0] = 0.0; - stm[2][1] = 0.0; - stm[2][2] = 0.0; - stm[2][3] = 0.0; + stm[2][0] = -2*S1; + stm[2][1] = -2*S2; + stm[2][2] = -S3; + stm[2][3] = -1; stm[2][4] = 0.0; stm[2][5] = 0.0; - stm[3][0] = 0.0; - stm[3][1] = 0.0; - stm[3][2] = 0.0; + stm[3][0] = -2*phi1; + stm[3][1] = -2*phi2; + stm[3][2] = -(2*phi3 + 1); stm[3][3] = 0.0; stm[3][4] = 0.0; stm[3][5] = 0.0; @@ -170,14 +170,52 @@ libra::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant stm[4][1] = 0.0; stm[4][2] = 0.0; stm[4][3] = 0.0; - stm[4][4] = 0.0; - stm[4][5] = 0.0; + stm[4][4] = cos(f_ref_rad); + stm[4][5] = sin(f_ref_rad); stm[5][0] = 0.0; stm[5][1] = 0.0; stm[5][2] = 0.0; stm[5][3] = 0.0; - stm[5][4] = 0.0; - stm[5][5] = 0.0; + stm[5][4] = -sin(f_ref_rad); + stm[5][5] = cos(f_ref_rad); + + stm_inv[0][0] = 4*S2 + phi2_prime; + stm_inv[0][1] = -phi2; + stm_inv[0][2] = 0.0; + stm_inv[0][3] = 2*S2; + stm_inv[0][4] = 0.0; + stm_inv[0][5] = 0.0; + stm_inv[1][0] = -(4*S1 + phi1_prime); + stm_inv[1][1] = phi1; + stm_inv[1][2] = 0.0; + stm_inv[1][3] = -2*S1; + stm_inv[1][4] = 0.0; + stm_inv[1][5] = 0.0; + stm_inv[2][0] = -2; + stm_inv[2][1] = 0.0; + stm_inv[2][2] = 0.0; + stm_inv[2][3] = -1; + stm_inv[2][4] = 0.0; + stm_inv[2][5] = 0.0; + stm_inv[3][0] = 2*S3 + phi3_prime; + stm_inv[3][1] = -phi3; + stm_inv[3][2] = -1; + stm_inv[3][3] = S3; + stm_inv[3][4] = 0.0; + stm_inv[3][5] = 0.0; + stm_inv[4][0] = 0.0; + stm_inv[4][1] = 0.0; + stm_inv[4][2] = 0.0; + stm_inv[4][3] = 0.0; + stm_inv[4][4] = cos(f_ref_rad); + stm_inv[4][5] = -sin(f_ref_rad); + stm_inv[5][0] = 0.0; + stm_inv[5][1] = 0.0; + stm_inv[5][2] = 0.0; + stm_inv[5][3] = 0.0; + stm_inv[5][4] = sin(f_ref_rad); + stm_inv[5][5] = cos(f_ref_rad); + return stm; } From 7ad0a07b46358785d1fceaeeffefe92af5744b6b Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 16:43:51 +0900 Subject: [PATCH 24/34] add class --- src/math_physics/orbit/relative_orbit_models.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index f4912c436..ef9f5a162 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -131,6 +131,7 @@ math::Vector<6> CalcSsCorrectionTerm(double orbit_radius_m, double gravity_const math::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe){ math::Matrix<6, 6> stm; + math::Matrix<6, 6> stm_inv; // ここでstmを計算してください double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); double e = reference_oe->GetEccentricity(); From e954c2a2546319a5ebbdbd7261e9c2364fadea54 Mon Sep 17 00:00:00 2001 From: michinarikake <> Date: Mon, 23 Sep 2024 17:37:56 +0900 Subject: [PATCH 25/34] follow format --- .../orbit/relative_orbit_models.cpp | 105 ------------------ 1 file changed, 105 deletions(-) diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index ef9f5a162..f888bf396 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -123,111 +123,6 @@ math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constan return stm; } -math::Vector<6> CalcSsCorrectionTerm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, OrbitalElements* reference_oe) { - math::Vector<6> correction_term; - // ここでstmを計算してください - return correction_term; -} - -math::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe){ - math::Matrix<6, 6> stm; - math::Matrix<6, 6> stm_inv; - // ここでstmを計算してください - double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); - double e = reference_oe->GetEccentricity(); - double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); - double k = e * cos(f_ref_rad) + 1; - double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); - double K2 = pow(1 - e * e, -2.5) * (0.5 * E_rad - 0.25 * sin(2 * E_rad) - e * pow(sin(E_rad), 3) / 3.); - double phi1 = sin(f_ref_rad) * k; - double phi2 = 2 * e * phi1 * (sin(f_ref_rad) / pow(k, 3) - K2) - cos(f_ref_rad) / k; - double phi3 = (6 * e * phi1 * K2 - (2 * pow(sin(f_ref_rad), 2) / pow(k, 2)) - (pow(cos(f_ref_rad), 2) / k) - pow(cos(f_ref_rad), 2)); - double phi1_prime = cos(f_ref_rad) * k - e * pow(sin(f_ref_rad), 2); - double sigma4 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); - double term1_phi2_prime = sin(f_ref_rad) / k; - double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); - double phi2_prime = term1_phi2_prime + term2_phi2_prime; - double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); - double S1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); - double S2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); - double S3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); - stm[0][0] = phi1; - stm[0][1] = phi2; - stm[0][2] = phi3; - stm[0][3] = 0.0; - stm[0][4] = 0.0; - stm[0][5] = 0.0; - stm[1][0] = phi1_prime; - stm[1][1] = phi2_prime; - stm[1][2] = phi3_prime; - stm[1][3] = 0.0; - stm[1][4] = 0.0; - stm[1][5] = 0.0; - stm[2][0] = -2*S1; - stm[2][1] = -2*S2; - stm[2][2] = -S3; - stm[2][3] = -1; - stm[2][4] = 0.0; - stm[2][5] = 0.0; - stm[3][0] = -2*phi1; - stm[3][1] = -2*phi2; - stm[3][2] = -(2*phi3 + 1); - stm[3][3] = 0.0; - stm[3][4] = 0.0; - stm[3][5] = 0.0; - stm[4][0] = 0.0; - stm[4][1] = 0.0; - stm[4][2] = 0.0; - stm[4][3] = 0.0; - stm[4][4] = cos(f_ref_rad); - stm[4][5] = sin(f_ref_rad); - stm[5][0] = 0.0; - stm[5][1] = 0.0; - stm[5][2] = 0.0; - stm[5][3] = 0.0; - stm[5][4] = -sin(f_ref_rad); - stm[5][5] = cos(f_ref_rad); - - stm_inv[0][0] = 4*S2 + phi2_prime; - stm_inv[0][1] = -phi2; - stm_inv[0][2] = 0.0; - stm_inv[0][3] = 2*S2; - stm_inv[0][4] = 0.0; - stm_inv[0][5] = 0.0; - stm_inv[1][0] = -(4*S1 + phi1_prime); - stm_inv[1][1] = phi1; - stm_inv[1][2] = 0.0; - stm_inv[1][3] = -2*S1; - stm_inv[1][4] = 0.0; - stm_inv[1][5] = 0.0; - stm_inv[2][0] = -2; - stm_inv[2][1] = 0.0; - stm_inv[2][2] = 0.0; - stm_inv[2][3] = -1; - stm_inv[2][4] = 0.0; - stm_inv[2][5] = 0.0; - stm_inv[3][0] = 2*S3 + phi3_prime; - stm_inv[3][1] = -phi3; - stm_inv[3][2] = -1; - stm_inv[3][3] = S3; - stm_inv[3][4] = 0.0; - stm_inv[3][5] = 0.0; - stm_inv[4][0] = 0.0; - stm_inv[4][1] = 0.0; - stm_inv[4][2] = 0.0; - stm_inv[4][3] = 0.0; - stm_inv[4][4] = cos(f_ref_rad); - stm_inv[4][5] = -sin(f_ref_rad); - stm_inv[5][0] = 0.0; - stm_inv[5][1] = 0.0; - stm_inv[5][2] = 0.0; - stm_inv[5][3] = 0.0; - stm_inv[5][4] = sin(f_ref_rad); - stm_inv[5][5] = cos(f_ref_rad); - - return stm; -} - math::Matrix<6, 6> CalcYamakawaAnkersenStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe) { math::Matrix<6, 6> stm; // ここでstmを計算してください From 5b00201a95f60e15c1d23655d67898c7598a7956 Mon Sep 17 00:00:00 2001 From: YuseiAlexS Date: Tue, 24 Sep 2024 10:10:37 +0900 Subject: [PATCH 26/34] remove functions from relative_orbit_models --- src/math_physics/orbit/relative_orbit_models.cpp | 5 ----- src/math_physics/orbit/relative_orbit_models.hpp | 11 ----------- 2 files changed, 16 deletions(-) diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index f888bf396..a50bd195f 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -123,9 +123,4 @@ math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constan return stm; } -math::Matrix<6, 6> CalcYamakawaAnkersenStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe) { - math::Matrix<6, 6> stm; - // ここでstmを計算してください - return stm; -} } // namespace orbit diff --git a/src/math_physics/orbit/relative_orbit_models.hpp b/src/math_physics/orbit/relative_orbit_models.hpp index 18f6f03ac..2fbfc6726 100644 --- a/src/math_physics/orbit/relative_orbit_models.hpp +++ b/src/math_physics/orbit/relative_orbit_models.hpp @@ -89,17 +89,6 @@ math::Vector<6> CalcSsCorrectionTerm(double orbit_radius_m, double gravity_const */ math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, OrbitalElements* reference_oe); -/** - * @fn CalcCarterStm - * @brief Calculate Carter State Transition Matrix - * @param [in] orbit_radius_m: Orbit radius [m] - * @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 - * @return State Transition Matrix - */ -math::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe); - } // namespace orbit #endif // S2E_LIBRARY_ORBIT_RELATIVE_ORBIT_MODEL_HPP_ From 9b507af5ce944ee92d3f0c7cfd3c0033eaeae870 Mon Sep 17 00:00:00 2001 From: YuseiAlexS Date: Tue, 24 Sep 2024 10:27:26 +0900 Subject: [PATCH 27/34] fix CmakeLists --- ExtLibraries/CMakeLists.txt | 2 +- src/math_physics/CMakeLists.txt | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/ExtLibraries/CMakeLists.txt b/ExtLibraries/CMakeLists.txt index f95bf41b0..1723ecb87 100644 --- a/ExtLibraries/CMakeLists.txt +++ b/ExtLibraries/CMakeLists.txt @@ -26,6 +26,6 @@ string(REPLACE "\\" "/" SETTINGS_DIR ${SETTINGS_DIR}) message("ExtLibraries install dir: ${EXT_LIB_DIR}") -#add_subdirectory(nrlmsise00) +add_subdirectory(nrlmsise00) add_subdirectory(cspice) add_subdirectory(lunar_gravity_field) diff --git a/src/math_physics/CMakeLists.txt b/src/math_physics/CMakeLists.txt index 94b5a665b..f9b9f4388 100644 --- a/src/math_physics/CMakeLists.txt +++ b/src/math_physics/CMakeLists.txt @@ -33,6 +33,7 @@ add_library(${PROJECT_NAME} STATIC orbit/kepler_orbit.cpp orbit/relative_orbit_models.cpp orbit/relative_orbit_yamanaka_ankersen.cpp + orbit/relative_orbit_carter.cpp orbit/interpolation_orbit.cpp orbit/sgp4/sgp4ext.cpp orbit/sgp4/sgp4io.cpp From c002f8f1bf9d21f9709204ac42eb9664e7a29a5b Mon Sep 17 00:00:00 2001 From: YuseiAlexS Date: Tue, 24 Sep 2024 11:10:43 +0900 Subject: [PATCH 28/34] fix small --- src/library/orbit/relative_orbit_models.cpp | 226 ------------------ .../orbit/relative_orbit_carter.cpp | 10 +- .../orbit/relative_orbit_carter.hpp | 8 +- 3 files changed, 9 insertions(+), 235 deletions(-) delete mode 100644 src/library/orbit/relative_orbit_models.cpp diff --git a/src/library/orbit/relative_orbit_models.cpp b/src/library/orbit/relative_orbit_models.cpp deleted file mode 100644 index 4c270e6a9..000000000 --- a/src/library/orbit/relative_orbit_models.cpp +++ /dev/null @@ -1,226 +0,0 @@ -/** - * @file relative_orbit_models.cpp - * @brief Functions for relative orbit - */ -#include "relative_orbit_models.hpp" - -#include -#include "../external/sgp4/sgp4unit.h" // for getgravconst() - -libra::Matrix<6, 6> CalcHillSystemMatrix(double orbit_radius_m, double gravity_constant_m3_s2) { - libra::Matrix<6, 6> system_matrix; - - double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); - system_matrix[0][0] = 0.0; - system_matrix[0][1] = 0.0; - system_matrix[0][2] = 0.0; - system_matrix[0][3] = 1.0; - system_matrix[0][4] = 0.0; - system_matrix[0][5] = 0.0; - system_matrix[1][0] = 0.0; - system_matrix[1][1] = 0.0; - system_matrix[1][2] = 0.0; - system_matrix[1][3] = 0.0; - system_matrix[1][4] = 1.0; - system_matrix[1][5] = 0.0; - system_matrix[2][0] = 0.0; - system_matrix[2][1] = 0.0; - system_matrix[2][2] = 0.0; - system_matrix[2][3] = 0.0; - system_matrix[2][4] = 0.0; - system_matrix[2][5] = 1.0; - system_matrix[3][0] = 3.0 * n * n; - system_matrix[3][1] = 0.0; - system_matrix[3][2] = 0.0; - system_matrix[3][3] = 0.0; - system_matrix[3][4] = 2.0 * n; - system_matrix[3][5] = 0.0; - system_matrix[4][0] = 0.0; - system_matrix[4][1] = 0.0; - system_matrix[4][2] = 0.0; - system_matrix[4][3] = -2.0 * n; - system_matrix[4][4] = 0.0; - system_matrix[4][5] = 0.0; - system_matrix[5][0] = 0.0; - system_matrix[5][1] = 0.0; - system_matrix[5][2] = -n * n; - system_matrix[5][3] = 0.0; - system_matrix[5][4] = 0.0; - system_matrix[5][5] = 0.0; - - return system_matrix; -} - -libra::Matrix<6, 6> CalcHcwStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s) { - libra::Matrix<6, 6> stm; - - double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); - double t = elapsed_time_s; - stm[0][0] = 4.0 - 3.0 * cos(n * t); - stm[0][1] = 0.0; - stm[0][2] = 0.0; - stm[0][3] = sin(n * t) / n; - stm[0][4] = 2.0 / n - 2.0 * cos(n * t) / n; - stm[0][5] = 0.0; - stm[1][0] = -6.0 * n * t + 6 * sin(n * t); - stm[1][1] = 1.0; - stm[1][2] = 0.0; - stm[1][3] = -2.0 / n + 2.0 * cos(n * t) / n; - stm[1][4] = 4.0 * sin(n * t) / n - 3.0 * t; - stm[1][5] = 0.0; - stm[2][0] = 0.0; - stm[2][1] = 0.0; - stm[2][2] = cos(n * t); - stm[2][3] = 0.0; - stm[2][4] = 0.0; - stm[2][5] = sin(n * t) / n; - stm[3][0] = 3.0 * n * sin(n * t); - stm[3][1] = 0.0; - stm[3][2] = 0.0; - stm[3][3] = cos(n * t); - stm[3][4] = 2 * sin(n * t); - stm[3][5] = 0.0; - stm[4][0] = -6.0 * n + 6.0 * n * cos(n * t); - stm[4][1] = 0.0; - stm[4][2] = 0.0; - stm[4][3] = -2.0 * sin(n * t); - stm[4][4] = -3.0 + 4.0 * cos(n * t); - stm[4][5] = 0.0; - stm[5][0] = 0.0; - stm[5][1] = 0.0; - stm[5][2] = -n * sin(n * t); - stm[5][3] = 0.0; - stm[5][4] = 0.0; - stm[5][5] = cos(n * t); - return stm; -} - -// 定数やreferenceの軌道要素は以下のように取得できます -// gravconsttype whichconst = wgs72; -// double mu_km3_s2, radius_earth_km, tumin, xke, j2, j3, j4, j3oj2; -// getgravconst(whichconst, tumin, mu_km3_s2, radius_earth_km, xke, j2, j3, j4, j3oj2); -// double semi_major_axis_m = reference_oe->GetSemiMajorAxis_m(); -// double eccentricity = reference_oe->GetEccentricity(); - -libra::Matrix<6, 6> CalcMeltonStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, OrbitalElements* reference_oe) { - libra::Matrix<6, 6> stm; - // ここでstmを計算してください - return stm; -} - -libra::Matrix<6, 6> CalcSsStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, OrbitalElements* reference_oe) { - libra::Matrix<6, 6> stm; - // ここでstmを計算してください - return stm; -} - -libra::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, OrbitalElements* reference_oe) { - libra::Matrix<6, 6> stm; - // ここでstmを計算してください - return stm; -} - -libra::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, double f_ref_0_rad, OrbitalElements* reference_oe){ - libra::Matrix<6, 6> stm; - libra::Matrix<6, 6> stm_inv; - // ここでstmを計算してください - double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); - double e = reference_oe->GetEccentricity(); - double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); - double k = e * cos(f_ref_rad) + 1; - double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); - double K2 = pow(1 - e * e, -2.5) * (0.5 * E_rad - 0.25 * sin(2 * E_rad) - e * pow(sin(E_rad), 3) / 3.); - double phi1 = sin(f_ref_rad) * k; - double phi2 = 2 * e * phi1 * (sin(f_ref_rad) / pow(k, 3) - K2) - cos(f_ref_rad) / k; - double phi3 = (6 * e * phi1 * K2 - (2 * pow(sin(f_ref_rad), 2) / pow(k, 2)) - (pow(cos(f_ref_rad), 2) / k) - pow(cos(f_ref_rad), 2)); - double phi1_prime = cos(f_ref_rad) * k - e * pow(sin(f_ref_rad), 2); - double sigma4 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); - double term1_phi2_prime = sin(f_ref_rad) / k; - double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); - double phi2_prime = term1_phi2_prime + term2_phi2_prime; - double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); - double S1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); - double S2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); - double S3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); - stm[0][0] = phi1; - stm[0][1] = phi2; - stm[0][2] = phi3; - stm[0][3] = 0.0; - stm[0][4] = 0.0; - stm[0][5] = 0.0; - stm[1][0] = phi1_prime; - stm[1][1] = phi2_prime; - stm[1][2] = phi3_prime; - stm[1][3] = 0.0; - stm[1][4] = 0.0; - stm[1][5] = 0.0; - stm[2][0] = -2*S1; - stm[2][1] = -2*S2; - stm[2][2] = -S3; - stm[2][3] = -1; - stm[2][4] = 0.0; - stm[2][5] = 0.0; - stm[3][0] = -2*phi1; - stm[3][1] = -2*phi2; - stm[3][2] = -(2*phi3 + 1); - stm[3][3] = 0.0; - stm[3][4] = 0.0; - stm[3][5] = 0.0; - stm[4][0] = 0.0; - stm[4][1] = 0.0; - stm[4][2] = 0.0; - stm[4][3] = 0.0; - stm[4][4] = cos(f_ref_rad); - stm[4][5] = sin(f_ref_rad); - stm[5][0] = 0.0; - stm[5][1] = 0.0; - stm[5][2] = 0.0; - stm[5][3] = 0.0; - stm[5][4] = -sin(f_ref_rad); - stm[5][5] = cos(f_ref_rad); - - stm_inv[0][0] = 4*S2 + phi2_prime; - stm_inv[0][1] = -phi2; - stm_inv[0][2] = 0.0; - stm_inv[0][3] = 2*S2; - stm_inv[0][4] = 0.0; - stm_inv[0][5] = 0.0; - stm_inv[1][0] = -(4*S1 + phi1_prime); - stm_inv[1][1] = phi1; - stm_inv[1][2] = 0.0; - stm_inv[1][3] = -2*S1; - stm_inv[1][4] = 0.0; - stm_inv[1][5] = 0.0; - stm_inv[2][0] = -2; - stm_inv[2][1] = 0.0; - stm_inv[2][2] = 0.0; - stm_inv[2][3] = -1; - stm_inv[2][4] = 0.0; - stm_inv[2][5] = 0.0; - stm_inv[3][0] = 2*S3 + phi3_prime; - stm_inv[3][1] = -phi3; - stm_inv[3][2] = -1; - stm_inv[3][3] = S3; - stm_inv[3][4] = 0.0; - stm_inv[3][5] = 0.0; - stm_inv[4][0] = 0.0; - stm_inv[4][1] = 0.0; - stm_inv[4][2] = 0.0; - stm_inv[4][3] = 0.0; - stm_inv[4][4] = cos(f_ref_rad); - stm_inv[4][5] = -sin(f_ref_rad); - stm_inv[5][0] = 0.0; - stm_inv[5][1] = 0.0; - stm_inv[5][2] = 0.0; - stm_inv[5][3] = 0.0; - stm_inv[5][4] = sin(f_ref_rad); - stm_inv[5][5] = cos(f_ref_rad); - - return stm; -} - -libra::Matrix<6, 6> CalcYamakawaAnkersenStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe){ - libra::Matrix<6, 6> stm; - // ここでstmを計算してください - return stm; -} diff --git a/src/math_physics/orbit/relative_orbit_carter.cpp b/src/math_physics/orbit/relative_orbit_carter.cpp index ab125fe1e..ffb9fe32c 100644 --- a/src/math_physics/orbit/relative_orbit_carter.cpp +++ b/src/math_physics/orbit/relative_orbit_carter.cpp @@ -1,6 +1,6 @@ /** - * @file relative_orbit_models.cpp - * @brief Functions to calculate Yamanaka-ANkersen STM for relative orbit + * @file relative_orbit_carter.cpp + * @brief Functions to calculate Carter's STM for relative orbit */ #include "relative_orbit_carter.hpp" @@ -16,17 +16,17 @@ RelativeOrbitCarter::~RelativeOrbitCarter() {} void RelativeOrbitCarter::CalculateInitialInverseMatrix(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe) { - double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); + // double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); double e = reference_oe->GetEccentricity(); double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); double k = e * cos(f_ref_rad) + 1; - double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); + // double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); double K2 = pow(1 - e * e, -2.5) * (0.5 * E_rad - 0.25 * sin(2 * E_rad) - e * pow(sin(E_rad), 3) / 3.); double phi1 = sin(f_ref_rad) * k; double phi2 = 2 * e * phi1 * (sin(f_ref_rad) / pow(k, 3) - K2) - cos(f_ref_rad) / k; double phi3 = (6 * e * phi1 * K2 - (2 * pow(sin(f_ref_rad), 2) / pow(k, 2)) - (pow(cos(f_ref_rad), 2) / k) - pow(cos(f_ref_rad), 2)); double phi1_prime = cos(f_ref_rad) * k - e * pow(sin(f_ref_rad), 2); - double sigma4 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); + // double sigma4 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); double term1_phi2_prime = sin(f_ref_rad) / k; double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); double phi2_prime = term1_phi2_prime + term2_phi2_prime; diff --git a/src/math_physics/orbit/relative_orbit_carter.hpp b/src/math_physics/orbit/relative_orbit_carter.hpp index 9bcee57f1..5aaa05b1c 100644 --- a/src/math_physics/orbit/relative_orbit_carter.hpp +++ b/src/math_physics/orbit/relative_orbit_carter.hpp @@ -1,10 +1,10 @@ /** - * @file relative_orbit_yamanaka_ankersen.hpp - * @brief Functions to calculate Yamanaka-ANkersen STM for relative orbit + * @file relative_orbit_carter.hpp + * @brief Functions to calculate Carter's STM for relative orbit */ -#ifndef S2E_LIBRARY_ORBIT_RELATIVE_ORBIT_YAMANAKA_ANKERSEN_HPP_ -#define S2E_LIBRARY_ORBIT_RELATIVE_ORBIT_YAMANAKA_ANKERSEN_HPP_ +#ifndef S2E_LIBRARY_ORBIT_RELATIVE_ORBIT_CARTER_HPP_ +#define S2E_LIBRARY_ORBIT_RELATIVE_ORBIT_CARTER_HPP_ #include "../math/matrix.hpp" #include "./orbital_elements.hpp" From f112393a318e5dbf808d28b167b400c8088e1723 Mon Sep 17 00:00:00 2001 From: YuseiAlexS Date: Tue, 24 Sep 2024 11:12:07 +0900 Subject: [PATCH 29/34] revert CalcSsCorrectionTerm --- src/math_physics/orbit/relative_orbit_models.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index a50bd195f..9a5e4c641 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -117,6 +117,12 @@ math::Matrix<6, 6> CalcSsStm(double orbit_radius_m, double gravity_constant_m3_s return stm; } +math::Vector<6> CalcSsCorrectionTerm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, OrbitalElements* reference_oe) { + math::Vector<6> correction_term; + // ここでstmを計算してください + return correction_term; +} + math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, OrbitalElements* reference_oe) { math::Matrix<6, 6> stm; // ここでstmを計算してください From 49c67deda3444159e88e5a5b361ac3a5e614de53 Mon Sep 17 00:00:00 2001 From: Satoshi Ikari Date: Tue, 24 Sep 2024 11:21:00 +0900 Subject: [PATCH 30/34] Fix --- settings/sample_satellite/satellite_sub.ini | 2 +- src/dynamics/orbit/relative_orbit.cpp | 4 ++-- src/dynamics/orbit/relative_orbit.hpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/settings/sample_satellite/satellite_sub.ini b/settings/sample_satellite/satellite_sub.ini index 7703cbe47..32d399efd 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 = 5 +stm_model_type = 4 // 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 diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index 7726195b4..55544edfd 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_yamanaka_ankersen_.CalculateInitialInverseMatrix(f_ref_rad, &reference_oe); break; case orbit::StmModel::kCarter: - relative_orbit_carter_->CalculateInitialInverseMatrix(reference_sat_orbit_radius, gravity_constant_m3_s2, f_ref_rad, &reference_oe); + relative_orbit_carter_.CalculateInitialInverseMatrix(reference_sat_orbit_radius, gravity_constant_m3_s2, f_ref_rad, &reference_oe); break; default: @@ -146,7 +146,7 @@ void RelativeOrbit::CalculateStm(orbit::StmModel stm_model_type, const Orbit* re break; } case orbit::StmModel::kCarter: { - stm_ = relative_orbit_carter_->CalculateSTM(gravity_constant_m3_s2, elapsed_sec, f_ref_rad, &reference_oe); + stm_ = relative_orbit_carter_.CalculateSTM(gravity_constant_m3_s2, elapsed_sec, f_ref_rad, &reference_oe); break; } case orbit::StmModel::kYamakawaAnkersen: { diff --git a/src/dynamics/orbit/relative_orbit.hpp b/src/dynamics/orbit/relative_orbit.hpp index 8fca6007a..c16a119e3 100644 --- a/src/dynamics/orbit/relative_orbit.hpp +++ b/src/dynamics/orbit/relative_orbit.hpp @@ -87,8 +87,8 @@ class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> orbit::RelativeOrbitModel relative_dynamics_model_type_; //!< Relative dynamics model type orbit::StmModel stm_model_type_; //!< State Transition Matrix model type RelativeInformation* relative_information_; //!< Relative information - orbit::RelativeOrbitYamanakaAnkersen* relative_orbit_yamanaka_ankersen_; //!< Relative Orbit Calcilater with Yamanaka-Ankersen's STM - orbit::RelativeOrbitCarter* relative_orbit_carter_; //!< Relative Orbit Calcilater with Carter's STM + orbit::RelativeOrbitYamanakaAnkersen relative_orbit_yamanaka_ankersen_; //!< Relative Orbit Calcilater with Yamanaka-Ankersen's STM + orbit::RelativeOrbitCarter relative_orbit_carter_; //!< Relative Orbit Calcilater with Carter's STM /** * @fn InitializeState From 140b41f94b4e4456e1138ac74be5bb3693fb7d1c Mon Sep 17 00:00:00 2001 From: YuseiAlexS Date: Tue, 24 Sep 2024 11:23:23 +0900 Subject: [PATCH 31/34] remove gravity_constant_m3_s2 --- src/dynamics/orbit/relative_orbit.cpp | 2 +- src/math_physics/orbit/relative_orbit_carter.cpp | 4 +--- src/math_physics/orbit/relative_orbit_carter.hpp | 3 +-- 3 files changed, 3 insertions(+), 6 deletions(-) diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index 55544edfd..b56411d06 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_yamanaka_ankersen_.CalculateInitialInverseMatrix(f_ref_rad, &reference_oe); break; case orbit::StmModel::kCarter: - relative_orbit_carter_.CalculateInitialInverseMatrix(reference_sat_orbit_radius, gravity_constant_m3_s2, f_ref_rad, &reference_oe); + relative_orbit_carter_.CalculateInitialInverseMatrix(reference_sat_orbit_radius, f_ref_rad, &reference_oe); break; default: diff --git a/src/math_physics/orbit/relative_orbit_carter.cpp b/src/math_physics/orbit/relative_orbit_carter.cpp index ffb9fe32c..85dd6ca3e 100644 --- a/src/math_physics/orbit/relative_orbit_carter.cpp +++ b/src/math_physics/orbit/relative_orbit_carter.cpp @@ -14,9 +14,7 @@ RelativeOrbitCarter::RelativeOrbitCarter() {} RelativeOrbitCarter::~RelativeOrbitCarter() {} -void RelativeOrbitCarter::CalculateInitialInverseMatrix(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, - OrbitalElements* reference_oe) { - // double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); +void RelativeOrbitCarter::CalculateInitialInverseMatrix(double orbit_radius_m, double f_ref_rad, OrbitalElements* reference_oe) { double e = reference_oe->GetEccentricity(); double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); double k = e * cos(f_ref_rad) + 1; diff --git a/src/math_physics/orbit/relative_orbit_carter.hpp b/src/math_physics/orbit/relative_orbit_carter.hpp index 5aaa05b1c..e4421ea67 100644 --- a/src/math_physics/orbit/relative_orbit_carter.hpp +++ b/src/math_physics/orbit/relative_orbit_carter.hpp @@ -32,11 +32,10 @@ class RelativeOrbitCarter { * @fn CalculateInitialInverseMatrix * @brief Calculate position and velocity with Kepler orbit propagation * @param [in] orbit_radius_m: Orbit radius [m] - * @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 orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe); + void CalculateInitialInverseMatrix(double orbit_radius_m, double f_ref_rad, OrbitalElements* reference_oe); /** * @fn CalculateSTM From 9984597a35e5a9e25373baeb33b3ef849549b0d4 Mon Sep 17 00:00:00 2001 From: TomokiMochizuki Date: Tue, 24 Sep 2024 11:56:38 +0900 Subject: [PATCH 32/34] update Matrix --- .../orbit/relative_orbit_carter.cpp | 173 +++++++++--------- 1 file changed, 87 insertions(+), 86 deletions(-) diff --git a/src/math_physics/orbit/relative_orbit_carter.cpp b/src/math_physics/orbit/relative_orbit_carter.cpp index 85dd6ca3e..b5668de12 100644 --- a/src/math_physics/orbit/relative_orbit_carter.cpp +++ b/src/math_physics/orbit/relative_orbit_carter.cpp @@ -16,62 +16,63 @@ RelativeOrbitCarter::~RelativeOrbitCarter() {} void RelativeOrbitCarter::CalculateInitialInverseMatrix(double orbit_radius_m, double f_ref_rad, OrbitalElements* reference_oe) { double e = reference_oe->GetEccentricity(); - double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); - double k = e * cos(f_ref_rad) + 1; + double E_rad = 2.0 * atan(sqrt((1.0 - e) / (1.0 + e)) * tan(f_ref_rad / 2.0)); + double k = e * cos(f_ref_rad) + 1.0; // double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); - double K2 = pow(1 - e * e, -2.5) * (0.5 * E_rad - 0.25 * sin(2 * E_rad) - e * pow(sin(E_rad), 3) / 3.); + double K2 = pow(1.0 - e * e, -2.5) * (0.5 * E_rad - 0.25 * sin(2.0 * E_rad) - e * pow(sin(E_rad), 3.0) / 3.0); double phi1 = sin(f_ref_rad) * k; - double phi2 = 2 * e * phi1 * (sin(f_ref_rad) / pow(k, 3) - K2) - cos(f_ref_rad) / k; - double phi3 = (6 * e * phi1 * K2 - (2 * pow(sin(f_ref_rad), 2) / pow(k, 2)) - (pow(cos(f_ref_rad), 2) / k) - pow(cos(f_ref_rad), 2)); - double phi1_prime = cos(f_ref_rad) * k - e * pow(sin(f_ref_rad), 2); + double phi2 = 2.0 * e * phi1 * (sin(f_ref_rad) / pow(k, 3) - K2) - cos(f_ref_rad) / k; + double phi3 = (6.0 * e * phi1 * K2 - (2.0 * pow(sin(f_ref_rad), 2.0) / pow(k, 2.0)) - (pow(cos(f_ref_rad), 2.0) / k) - pow(cos(f_ref_rad), 2.0)); + double phi1_prime = cos(f_ref_rad) * k - e * pow(sin(f_ref_rad), 2.0); // double sigma4 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); double term1_phi2_prime = sin(f_ref_rad) / k; - double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); + double term2_phi2_prime = 2.0 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); double phi2_prime = term1_phi2_prime + term2_phi2_prime; - double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); + double phi3_prime = k - 4.0 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2.0, 3.0) + 2.0 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2.0, 2.0) - + 6.0 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); double S1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); - double S2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); - double S3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); - initial_inverse_matrix_[0][0] = 4*S2 + phi2_prime; - initial_inverse_matrix_[0][1] = 0.0; - initial_inverse_matrix_[0][2] = 0.0; - initial_inverse_matrix_[0][3] = -phi2; - initial_inverse_matrix_[0][4] = 2 * S2; - initial_inverse_matrix_[0][5] = 0.0; - initial_inverse_matrix_[1][0] = 0.0; - initial_inverse_matrix_[1][1] = 0.0; - initial_inverse_matrix_[1][2] = 0.0; - initial_inverse_matrix_[1][3] = -2; - initial_inverse_matrix_[1][4] = -1; - initial_inverse_matrix_[1][5] = 0.0; - initial_inverse_matrix_[2][0] = 0.0; - initial_inverse_matrix_[2][1] = 0.0; - initial_inverse_matrix_[2][2] = cos(f_ref_rad); - initial_inverse_matrix_[2][3] = 0.0; - initial_inverse_matrix_[2][4] = 0.0; - initial_inverse_matrix_[2][5] = -sin(f_ref_rad); - initial_inverse_matrix_[3][0] = -(4 * S1 + phi1_prime); - initial_inverse_matrix_[3][1] = 0.0; - initial_inverse_matrix_[3][2] = 0.0; - initial_inverse_matrix_[3][3] = phi1; - initial_inverse_matrix_[3][4] = -2 * S1; - initial_inverse_matrix_[3][5] = 0.0; - initial_inverse_matrix_[4][0] = 2 * S3 + phi3_prime; - initial_inverse_matrix_[4][1] = 0.0; - initial_inverse_matrix_[4][2] = -1; - initial_inverse_matrix_[4][3] = -phi3; - initial_inverse_matrix_[4][4] = S3; - initial_inverse_matrix_[4][5] = 0.0; - initial_inverse_matrix_[5][0] = 0.0; - initial_inverse_matrix_[5][1] = 0.0; - initial_inverse_matrix_[5][2] = sin(f_ref_rad); - initial_inverse_matrix_[5][3] = 0.0; - initial_inverse_matrix_[5][4] = 0.0; - initial_inverse_matrix_[5][5] = cos(f_ref_rad); + double S2 = 3.0 * e * k * k * K2 - (sin(f_ref_rad) / k); + double S3 = -6.0 * k * k * K2 - ((2.0 + k) / (2.0 * k)) * sin(2.0 * f_ref_rad); + initial_inverse_matrix_[0][0] = 4.0 * S2 + phi2_prime; + initial_inverse_matrix_[0][1] = 0.0; + initial_inverse_matrix_[0][2] = 0.0; + initial_inverse_matrix_[0][3] = -phi2; + initial_inverse_matrix_[0][4] = 2.0 * S2; + initial_inverse_matrix_[0][5] = 0.0; + initial_inverse_matrix_[1][0] = -2.0; + initial_inverse_matrix_[1][1] = 0.0; + initial_inverse_matrix_[1][2] = 0.0; + initial_inverse_matrix_[1][3] = 0.0; + initial_inverse_matrix_[1][4] = -1.0; + initial_inverse_matrix_[1][5] = 0.0; + initial_inverse_matrix_[2][0] = 0.0; + initial_inverse_matrix_[2][1] = 0.0; + initial_inverse_matrix_[2][2] = cos(f_ref_rad); + initial_inverse_matrix_[2][3] = 0.0; + initial_inverse_matrix_[2][4] = 0.0; + initial_inverse_matrix_[2][5] = -sin(f_ref_rad); + initial_inverse_matrix_[3][0] = -(4.0 * S1 + phi1_prime); + initial_inverse_matrix_[3][1] = 0.0; + initial_inverse_matrix_[3][2] = 0.0; + initial_inverse_matrix_[3][3] = phi1; + initial_inverse_matrix_[3][4] = -2.0 * S1; + initial_inverse_matrix_[3][5] = 0.0; + initial_inverse_matrix_[4][0] = 2.0 * S3 + phi3_prime; + initial_inverse_matrix_[4][1] = -1.0; + initial_inverse_matrix_[4][2] = 0.0; + initial_inverse_matrix_[4][3] = -phi3; + initial_inverse_matrix_[4][4] = S3; + initial_inverse_matrix_[4][5] = 0.0; + initial_inverse_matrix_[5][0] = 0.0; + initial_inverse_matrix_[5][1] = 0.0; + initial_inverse_matrix_[5][2] = sin(f_ref_rad); + initial_inverse_matrix_[5][3] = 0.0; + initial_inverse_matrix_[5][4] = 0.0; + initial_inverse_matrix_[5][5] = cos(f_ref_rad); } -math::Matrix<6, 6> RelativeOrbitCarter::CalculateSTM(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe){ +math::Matrix<6, 6> RelativeOrbitCarter::CalculateSTM(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, + OrbitalElements* reference_oe) { math::Matrix<6, 6> stm; // ここでstmを計算してください double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); @@ -88,47 +89,47 @@ math::Matrix<6, 6> RelativeOrbitCarter::CalculateSTM(double orbit_radius_m, doub double term1_phi2_prime = sin(f_ref_rad) / k; double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); double phi2_prime = term1_phi2_prime + term2_phi2_prime; - double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); + double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - + 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); double S1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); double S2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); - double S3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); + double S3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); stm[0][0] = phi1; - stm[0][1] = phi3; - stm[0][2] = 0.0; - stm[0][3] = phi2; - stm[0][4] = 0.0; - stm[0][5] = 0.0; - stm[1][0] = phi1_prime; - stm[1][1] = phi3_prime; - stm[1][2] = 0.0; - stm[1][3] = phi2_prime; - stm[1][4] = 0.0; - stm[1][5] = 0.0; - stm[2][0] = -2 * S1; - stm[2][1] = -S3; - stm[2][2] = 0.0; - stm[2][3] = -2 * S2; - stm[2][4] = -1; - stm[2][5] = 0.0; - stm[3][0] = -2 * phi1; - stm[3][1] = -(2 * phi3 + 1); - stm[3][2] = 0.0; - stm[3][3] = -2 * phi2; - stm[3][4] = 0.0; - stm[3][5] = 0.0; - stm[4][0] = 0.0; - stm[4][1] = 0.0; - stm[4][2] = 0.0; - stm[4][3] = 0.0; - stm[4][4] = cos(f_ref_rad); - stm[4][5] = sin(f_ref_rad); - stm[5][0] = 0.0; - stm[5][1] = 0.0; - stm[5][2] = 0.0; - stm[5][3] = 0.0; - stm[5][4] = -sin(f_ref_rad); - stm[5][5] = cos(f_ref_rad); + stm[0][1] = phi3; + stm[0][2] = 0.0; + stm[0][3] = phi2; + stm[0][4] = 0.0; + stm[0][5] = 0.0; + stm[1][0] = -2 * S1; + stm[1][1] = -S3; + stm[1][2] = 0.0; + stm[1][3] = -2 * S2; + stm[1][4] = -1; + stm[1][5] = 0.0; + stm[2][0] = 0.0; + stm[2][1] = 0.0; + stm[2][2] = cos(f_ref_rad); + stm[2][3] = 0.0; + stm[2][4] = 0.0; + stm[2][5] = sin(f_ref_rad); + stm[3][0] = phi1_prime; + stm[3][1] = phi3_prime; + stm[3][2] = 0.0; + stm[3][3] = phi2_prime; + stm[3][4] = 0.0; + stm[3][5] = 0.0; + stm[4][0] = -2 * phi1; + stm[4][1] = -(2 * phi3 + 1); + stm[4][2] = 0.0; + stm[4][3] = -2 * phi2; + stm[4][4] = 0.0; + stm[4][5] = 0.0; + stm[5][0] = 0.0; + stm[5][1] = 0.0; + stm[5][2] = -sin(f_ref_rad); + stm[5][3] = 0.0; + stm[5][4] = 0.0; + stm[5][5] = cos(f_ref_rad); return stm * initial_inverse_matrix_; } From 8b866baabe71229dc4c23c9a4c38cf5906b0fe7f Mon Sep 17 00:00:00 2001 From: TomokiMochizuki Date: Tue, 24 Sep 2024 21:04:58 +0900 Subject: [PATCH 33/34] update Matrices for Carter's STM --- src/dynamics/orbit/relative_orbit.cpp | 6 +- .../orbit/relative_orbit_carter.cpp | 71 ++++++++------- .../orbit/relative_orbit_carter.hpp | 4 +- .../orbit/relative_orbit_models.cpp | 86 +++++++++++++++++++ .../orbit/relative_orbit_models.hpp | 24 ++++++ 5 files changed, 154 insertions(+), 37 deletions(-) diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index b56411d06..94ed20d5f 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -99,12 +99,12 @@ void RelativeOrbit::InitializeStmMatrix(orbit::StmModel stm_model_type, const Or double f_ref_rad = phi_rad - arg_perigee_rad; switch (stm_model_type) { + case orbit::StmModel::kCarter: + 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); break; - case orbit::StmModel::kCarter: - relative_orbit_carter_.CalculateInitialInverseMatrix(reference_sat_orbit_radius, f_ref_rad, &reference_oe); - break; default: break; diff --git a/src/math_physics/orbit/relative_orbit_carter.cpp b/src/math_physics/orbit/relative_orbit_carter.cpp index b5668de12..0236b0fe9 100644 --- a/src/math_physics/orbit/relative_orbit_carter.cpp +++ b/src/math_physics/orbit/relative_orbit_carter.cpp @@ -6,6 +6,7 @@ #include +#include "./relative_orbit_models.hpp" #include "./sgp4/sgp4unit.h" // for getgravconst() namespace orbit { @@ -14,25 +15,28 @@ RelativeOrbitCarter::RelativeOrbitCarter() {} RelativeOrbitCarter::~RelativeOrbitCarter() {} -void RelativeOrbitCarter::CalculateInitialInverseMatrix(double orbit_radius_m, double f_ref_rad, OrbitalElements* reference_oe) { +void RelativeOrbitCarter::CalculateInitialInverseMatrix(double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe) { double e = reference_oe->GetEccentricity(); + double a = reference_oe->GetSemiMajorAxis_m(); + double h = pow(a * (1.0 - pow(e, 2)) * gravity_constant_m3_s2, 0.5); // angular momentum + double E_rad = 2.0 * atan(sqrt((1.0 - e) / (1.0 + e)) * tan(f_ref_rad / 2.0)); double k = e * cos(f_ref_rad) + 1.0; // double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); - double K2 = pow(1.0 - e * e, -2.5) * (0.5 * E_rad - 0.25 * sin(2.0 * E_rad) - e * pow(sin(E_rad), 3.0) / 3.0); + double K2 = pow(1.0 - pow(e, 2.0), -2.5) * (0.5 * E_rad - 0.25 * sin(2.0 * E_rad) - e * pow(sin(E_rad), 3.0) / 3.0); double phi1 = sin(f_ref_rad) * k; - double phi2 = 2.0 * e * phi1 * (sin(f_ref_rad) / pow(k, 3) - K2) - cos(f_ref_rad) / k; - double phi3 = (6.0 * e * phi1 * K2 - (2.0 * pow(sin(f_ref_rad), 2.0) / pow(k, 2.0)) - (pow(cos(f_ref_rad), 2.0) / k) - pow(cos(f_ref_rad), 2.0)); + double phi2 = 2.0 * e * phi1 * (sin(f_ref_rad) / pow(k, 3) - 3.0 * e * K2) - cos(f_ref_rad) / k; + double phi3 = 6.0 * e * phi1 * K2 - 2.0 * pow(sin(f_ref_rad), 2.0) / pow(k, 2.0) - pow(cos(f_ref_rad), 2.0) / k - pow(cos(f_ref_rad), 2.0); double phi1_prime = cos(f_ref_rad) * k - e * pow(sin(f_ref_rad), 2.0); - // double sigma4 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); - double term1_phi2_prime = sin(f_ref_rad) / k; - double term2_phi2_prime = 2.0 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); - double phi2_prime = term1_phi2_prime + term2_phi2_prime; - double phi3_prime = k - 4.0 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2.0, 3.0) + 2.0 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2.0, 2.0) - - 6.0 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); - double S1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); - double S2 = 3.0 * e * k * k * K2 - (sin(f_ref_rad) / k); - double S3 = -6.0 * k * k * K2 - ((2.0 + k) / (2.0 * k)) * sin(2.0 * f_ref_rad); + double phi2_prime = -6.0 * pow(e, 2.0) * phi1_prime * K2 + + 2.0 * e * sin(f_ref_rad) * (2.0 * cos(f_ref_rad) - 3.0 * e * pow(sin(f_ref_rad), 2.0) + 2.0 * e) / pow(k, 3.0) + + sin(f_ref_rad) / pow(k, 2.0); + double phi3_prime = 6.0 * e * phi1_prime * K2 - (6.0 * e * pow(sin(f_ref_rad), 3.0) - 4.0 * sin(f_ref_rad) * (e + cos(f_ref_rad))) / pow(k, 3.0) + + 0.5 * sin(2 * f_ref_rad) * (2.0 + (2.0 + e * cos(f_ref_rad)) * pow(k, 2.0)); + double S1 = -cos(f_ref_rad) * (1.0 + 0.5 * e * cos(f_ref_rad)); + double S2 = 3.0 * e * pow(k, 2.0) * K2 - sin(f_ref_rad) / k; + double S3 = -6.0 * pow(k, 2.0) * K2 - (2.0 + k) / 2.0 / k * sin(2.0 * f_ref_rad); + initial_inverse_matrix_[0][0] = 4.0 * S2 + phi2_prime; initial_inverse_matrix_[0][1] = 0.0; initial_inverse_matrix_[0][2] = 0.0; @@ -69,31 +73,34 @@ void RelativeOrbitCarter::CalculateInitialInverseMatrix(double orbit_radius_m, d initial_inverse_matrix_[5][3] = 0.0; initial_inverse_matrix_[5][4] = 0.0; initial_inverse_matrix_[5][5] = cos(f_ref_rad); + + initial_inverse_matrix_ = + initial_inverse_matrix_ * orbit::CalcStateTransformationMatrixLvlhToTschaunerHampel(gravity_constant_m3_s2, e, h, f_ref_rad); } math::Matrix<6, 6> RelativeOrbitCarter::CalculateSTM(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe) { math::Matrix<6, 6> stm; - // ここでstmを計算してください - double n = sqrt(gravity_constant_m3_s2 / pow(orbit_radius_m, 3)); double e = reference_oe->GetEccentricity(); - double E_rad = 2 * atan(sqrt((1 - e) / (1 + e)) * tan(f_ref_rad / 2)); - double k = e * cos(f_ref_rad) + 1; - double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); - double K2 = pow(1 - e * e, -2.5) * (0.5 * E_rad - 0.25 * sin(2 * E_rad) - e * pow(sin(E_rad), 3) / 3.); + double a = reference_oe->GetSemiMajorAxis_m(); + double h = pow(a * (1 - pow(e, 2)) * gravity_constant_m3_s2, 0.5); // angular momentum + double E_rad = 2.0 * atan(sqrt((1.0 - e) / (1.0 + e)) * tan(f_ref_rad / 2.0)); + double k = e * cos(f_ref_rad) + 1.0; + // double K1 = pow(1 - e * e, -2.5) * (-1.5 * e * E_rad + (1 + e * e) * sin(E_rad) - e * sin(2. * E_rad) / 4.); + double K2 = pow(1.0 - pow(e, 2.0), -2.5) * (0.5 * E_rad - 0.25 * sin(2.0 * E_rad) - e * pow(sin(E_rad), 3.0) / 3.0); double phi1 = sin(f_ref_rad) * k; - double phi2 = 2 * e * phi1 * (sin(f_ref_rad) / pow(k, 3) - K2) - cos(f_ref_rad) / k; - double phi3 = (6 * e * phi1 * K2 - (2 * pow(sin(f_ref_rad), 2) / pow(k, 2)) - (pow(cos(f_ref_rad), 2) / k) - pow(cos(f_ref_rad), 2)); - double phi1_prime = cos(f_ref_rad) * k - e * pow(sin(f_ref_rad), 2); - double sigma4 = atan(tan(f_ref_rad / 2) * sqrt(-(e - 1) / (e + 1))); - double term1_phi2_prime = sin(f_ref_rad) / k; - double term2_phi2_prime = 2 * e * sin(f_ref_rad) * k * (cos(f_ref_rad) / pow(k, 3)) - (cos(f_ref_rad) / pow(1 - pow(e, 2), 5.0 / 2.0)); - double phi2_prime = term1_phi2_prime + term2_phi2_prime; - double phi3_prime = k - 4 * e * pow(sin(f_ref_rad), 3) / pow(E_rad / 2., 3) + 2 * cos(f_ref_rad) * sin(f_ref_rad) / pow(E_rad / 2., 2) - - 6 * e * cos(f_ref_rad) * E_rad / 2. / pow(1 - pow(e, 2), 5.0 / 2.0); - double S1 = -cos(f_ref_rad) - (e / 2.0) * pow(cos(f_ref_rad), 2); - double S2 = 3 * e * k * k * K2 - (sin(f_ref_rad) / k); - double S3 = -6 * k * k * K2 - ((2 + k) / (2 * k)) * sin(2 * f_ref_rad); + double phi2 = 2.0 * e * phi1 * (sin(f_ref_rad) / pow(k, 3) - 3.0 * e * K2) - cos(f_ref_rad) / k; + double phi3 = 6.0 * e * phi1 * K2 - 2.0 * pow(sin(f_ref_rad), 2.0) / pow(k, 2.0) - pow(cos(f_ref_rad), 2.0) / k - pow(cos(f_ref_rad), 2.0); + double phi1_prime = cos(f_ref_rad) * k - e * pow(sin(f_ref_rad), 2.0); + double phi2_prime = -6.0 * pow(e, 2.0) * phi1_prime * K2 + + 2.0 * e * sin(f_ref_rad) * (2.0 * cos(f_ref_rad) - 3.0 * e * pow(sin(f_ref_rad), 2.0) + 2.0 * e) / pow(k, 3.0) + + sin(f_ref_rad) / pow(k, 2.0); + double phi3_prime = 6.0 * e * phi1_prime * K2 - (6.0 * e * pow(sin(f_ref_rad), 3.0) - 4.0 * sin(f_ref_rad) * (e + cos(f_ref_rad))) / pow(k, 3.0) + + 0.5 * sin(2 * f_ref_rad) * (2.0 + (2.0 + e * cos(f_ref_rad)) * pow(k, 2.0)); + double S1 = -cos(f_ref_rad) * (1.0 + 0.5 * e * cos(f_ref_rad)); + double S2 = 3.0 * e * pow(k, 2.0) * K2 - sin(f_ref_rad) / k; + double S3 = -6.0 * pow(k, 2.0) * K2 - (2.0 + k) / 2.0 / k * sin(2.0 * f_ref_rad); + stm[0][0] = phi1; stm[0][1] = phi3; stm[0][2] = 0.0; @@ -130,7 +137,7 @@ math::Matrix<6, 6> RelativeOrbitCarter::CalculateSTM(double orbit_radius_m, doub stm[5][3] = 0.0; stm[5][4] = 0.0; stm[5][5] = cos(f_ref_rad); - return stm * initial_inverse_matrix_; + return orbit::CalcStateTransformationMatrixTschaunerHampelToLvlh(gravity_constant_m3_s2, e, h, f_ref_rad) * stm * initial_inverse_matrix_; } } // namespace orbit diff --git a/src/math_physics/orbit/relative_orbit_carter.hpp b/src/math_physics/orbit/relative_orbit_carter.hpp index e4421ea67..2f39fe351 100644 --- a/src/math_physics/orbit/relative_orbit_carter.hpp +++ b/src/math_physics/orbit/relative_orbit_carter.hpp @@ -31,11 +31,11 @@ class RelativeOrbitCarter { /** * @fn CalculateInitialInverseMatrix * @brief Calculate position and velocity with Kepler orbit propagation - * @param [in] orbit_radius_m: Orbit radius [m] + * @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 orbit_radius_m, double f_ref_rad, OrbitalElements* reference_oe); + void CalculateInitialInverseMatrix(double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe); /** * @fn CalculateSTM diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index 9a5e4c641..1d801674f 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -129,4 +129,90 @@ math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constan return stm; } +math::Matrix<6, 6> CalcStateTransformationMatrixLvlhToTschaunerHampel(const double gravity_constant_m3_s2, const double eccentricity, + const double angular_momentum_kg_m2_s, const double true_anomaly_rad) { + math::Matrix<6, 6> transition_matrix; + transition_matrix[0][0] = gravity_constant_m3_s2 * (1.0 + eccentricity * cos(true_anomaly_rad)) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[0][1] = 0.0; + transition_matrix[0][2] = 0.0; + transition_matrix[0][3] = 0.0; + transition_matrix[0][4] = 0.0; + transition_matrix[0][5] = 0.0; + transition_matrix[1][0] = 0.0; + transition_matrix[1][1] = gravity_constant_m3_s2 * (1.0 + eccentricity * cos(true_anomaly_rad)) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[1][2] = 0.0; + transition_matrix[1][3] = 0.0; + transition_matrix[1][4] = 0.0; + transition_matrix[1][5] = 0.0; + transition_matrix[2][0] = 0.0; + transition_matrix[2][1] = 0.0; + transition_matrix[2][2] = gravity_constant_m3_s2 * (1.0 + eccentricity * cos(true_anomaly_rad)) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[2][3] = 0.0; + transition_matrix[2][4] = 0.0; + transition_matrix[2][5] = 0.0; + transition_matrix[3][0] = -gravity_constant_m3_s2 * eccentricity * sin(true_anomaly_rad) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[3][1] = 0.0; + transition_matrix[3][2] = 0.0; + transition_matrix[3][3] = pow(angular_momentum_kg_m2_s, 1.5) / gravity_constant_m3_s2 / (1.0 + eccentricity * cos(true_anomaly_rad)); + transition_matrix[3][4] = 0.0; + transition_matrix[3][5] = 0.0; + transition_matrix[4][0] = 0.0; + transition_matrix[4][1] = -gravity_constant_m3_s2 * eccentricity * sin(true_anomaly_rad) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[4][2] = 0.0; + transition_matrix[4][3] = 0.0; + transition_matrix[4][4] = pow(angular_momentum_kg_m2_s, 1.5) / gravity_constant_m3_s2 / (1.0 + eccentricity * cos(true_anomaly_rad)); + transition_matrix[4][5] = 0.0; + transition_matrix[5][0] = 0.0; + transition_matrix[5][1] = 0.0; + transition_matrix[5][2] = -gravity_constant_m3_s2 * eccentricity * sin(true_anomaly_rad) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[5][3] = 0.0; + transition_matrix[5][4] = 0.0; + transition_matrix[5][5] = pow(angular_momentum_kg_m2_s, 1.5) / gravity_constant_m3_s2 / (1.0 + eccentricity * cos(true_anomaly_rad)); + + return transition_matrix; +} + +math::Matrix<6, 6> CalcStateTransformationMatrixTschaunerHampelToLvlh(const double gravity_constant_m3_s2, const double eccentricity, + const double angular_momentum_kg_m2_s, const double true_anomaly_rad) { + math::Matrix<6, 6> transition_matrix; + transition_matrix[0][0] = pow(angular_momentum_kg_m2_s, 1.5) / gravity_constant_m3_s2 / (1.0 + eccentricity * cos(true_anomaly_rad)); + transition_matrix[0][1] = 0.0; + transition_matrix[0][2] = 0.0; + transition_matrix[0][3] = 0.0; + transition_matrix[0][4] = 0.0; + transition_matrix[0][5] = 0.0; + transition_matrix[1][0] = 0.0; + transition_matrix[1][1] = pow(angular_momentum_kg_m2_s, 1.5) / gravity_constant_m3_s2 / (1.0 + eccentricity * cos(true_anomaly_rad)); + transition_matrix[1][2] = 0.0; + transition_matrix[1][3] = 0.0; + transition_matrix[1][4] = 0.0; + transition_matrix[1][5] = 0.0; + transition_matrix[2][0] = 0.0; + transition_matrix[2][1] = 0.0; + transition_matrix[2][2] = pow(angular_momentum_kg_m2_s, 1.5) / gravity_constant_m3_s2 / (1.0 + eccentricity * cos(true_anomaly_rad)); + transition_matrix[2][3] = 0.0; + transition_matrix[2][4] = 0.0; + transition_matrix[2][5] = 0.0; + transition_matrix[3][0] = gravity_constant_m3_s2 * eccentricity * sin(true_anomaly_rad) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[3][1] = 0.0; + transition_matrix[3][2] = 0.0; + transition_matrix[3][3] = gravity_constant_m3_s2 * (1.0 + eccentricity * cos(true_anomaly_rad)) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[3][4] = 0.0; + transition_matrix[3][5] = 0.0; + transition_matrix[4][0] = 0.0; + transition_matrix[4][1] = gravity_constant_m3_s2 * eccentricity * sin(true_anomaly_rad) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[4][2] = 0.0; + transition_matrix[4][3] = 0.0; + transition_matrix[4][4] = gravity_constant_m3_s2 * (1.0 + eccentricity * cos(true_anomaly_rad)) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[4][5] = 0.0; + transition_matrix[5][0] = 0.0; + transition_matrix[5][1] = 0.0; + transition_matrix[5][2] = gravity_constant_m3_s2 * eccentricity * sin(true_anomaly_rad) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[5][3] = 0.0; + transition_matrix[5][4] = 0.0; + transition_matrix[5][5] = gravity_constant_m3_s2 * (1.0 + eccentricity * cos(true_anomaly_rad)) / pow(angular_momentum_kg_m2_s, 1.5); + + return transition_matrix; +} + } // namespace orbit diff --git a/src/math_physics/orbit/relative_orbit_models.hpp b/src/math_physics/orbit/relative_orbit_models.hpp index 2fbfc6726..c87f89844 100644 --- a/src/math_physics/orbit/relative_orbit_models.hpp +++ b/src/math_physics/orbit/relative_orbit_models.hpp @@ -89,6 +89,30 @@ math::Vector<6> CalcSsCorrectionTerm(double orbit_radius_m, double gravity_const */ math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, OrbitalElements* reference_oe); +/** + * @fn CalcStateTransformationMatrixLvlhToTschaunerHampel + * @brief Calculate state tranformation matrix from the state variables in the LVLH frame to the state variables for Tschauner-Hampel equation. + * @param [in] gravity_constant_m3_s2: Gravity constant of the center body [m3/s2] + * @param [in] eccentricity: Eccentricity [] + * @param [in] angular_momentum_kg_m2_s: Angular momentum of the spacecraft [kg*m2/s] + * @param [in] true_anomaly_rad: True anomaly of the spacecraft [rad] + * @param + */ +math::Matrix<6, 6> CalcStateTransformationMatrixLvlhToTschaunerHampel(const double gravity_constant_m3_s2, const double eccentricity, + const double angular_momentum_kg_m2_s, const double true_anomaly_rad); + +/** + * @fn CalcStateTransformationMatrixTschaunerHampelToLvlh + * @brief Calculate state tranformation matrix from the state variables for Tschauner-Hampel equation to the state variables in the LVLH frame. + * @param [in] gravity_constant_m3_s2: Gravity constant of the center body [m3/s2] + * @param [in] eccentricity: Eccentricity [] + * @param [in] angular_momentum_kg_m2_s: Angular momentum of the spacecraft [kg*m2/s] + * @param [in] true_anomaly_rad: True anomaly of the spacecraft [rad] + * @param + */ +math::Matrix<6, 6> CalcStateTransformationMatrixTschaunerHampelToLvlh(const double gravity_constant_m3_s2, const double eccentricity, + const double angular_momentum_kg_m2_s, const double true_anomaly_rad); + } // namespace orbit #endif // S2E_LIBRARY_ORBIT_RELATIVE_ORBIT_MODEL_HPP_ From 469d8d6525df35a46e9e0612f44974c59d0fc68d Mon Sep 17 00:00:00 2001 From: TomokiMochizuki Date: Tue, 24 Sep 2024 22:58:30 +0900 Subject: [PATCH 34/34] fix transition matrix --- .../orbit/relative_orbit_models.cpp | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index 1d801674f..fcca12bb8 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -132,42 +132,42 @@ math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constan math::Matrix<6, 6> CalcStateTransformationMatrixLvlhToTschaunerHampel(const double gravity_constant_m3_s2, const double eccentricity, const double angular_momentum_kg_m2_s, const double true_anomaly_rad) { math::Matrix<6, 6> transition_matrix; - transition_matrix[0][0] = gravity_constant_m3_s2 * (1.0 + eccentricity * cos(true_anomaly_rad)) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[0][0] = 1.0 + eccentricity * cos(true_anomaly_rad); transition_matrix[0][1] = 0.0; transition_matrix[0][2] = 0.0; transition_matrix[0][3] = 0.0; transition_matrix[0][4] = 0.0; transition_matrix[0][5] = 0.0; transition_matrix[1][0] = 0.0; - transition_matrix[1][1] = gravity_constant_m3_s2 * (1.0 + eccentricity * cos(true_anomaly_rad)) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[1][1] = 1.0 + eccentricity * cos(true_anomaly_rad); transition_matrix[1][2] = 0.0; transition_matrix[1][3] = 0.0; transition_matrix[1][4] = 0.0; transition_matrix[1][5] = 0.0; transition_matrix[2][0] = 0.0; transition_matrix[2][1] = 0.0; - transition_matrix[2][2] = gravity_constant_m3_s2 * (1.0 + eccentricity * cos(true_anomaly_rad)) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[2][2] = 1.0 + eccentricity * cos(true_anomaly_rad); transition_matrix[2][3] = 0.0; transition_matrix[2][4] = 0.0; transition_matrix[2][5] = 0.0; - transition_matrix[3][0] = -gravity_constant_m3_s2 * eccentricity * sin(true_anomaly_rad) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[3][0] = -eccentricity * sin(true_anomaly_rad); transition_matrix[3][1] = 0.0; transition_matrix[3][2] = 0.0; - transition_matrix[3][3] = pow(angular_momentum_kg_m2_s, 1.5) / gravity_constant_m3_s2 / (1.0 + eccentricity * cos(true_anomaly_rad)); + transition_matrix[3][3] = pow(angular_momentum_kg_m2_s, 3.0) / pow(gravity_constant_m3_s2, 2.0) / (1.0 + eccentricity * cos(true_anomaly_rad)); transition_matrix[3][4] = 0.0; transition_matrix[3][5] = 0.0; transition_matrix[4][0] = 0.0; - transition_matrix[4][1] = -gravity_constant_m3_s2 * eccentricity * sin(true_anomaly_rad) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[4][1] = -eccentricity * sin(true_anomaly_rad); transition_matrix[4][2] = 0.0; transition_matrix[4][3] = 0.0; - transition_matrix[4][4] = pow(angular_momentum_kg_m2_s, 1.5) / gravity_constant_m3_s2 / (1.0 + eccentricity * cos(true_anomaly_rad)); + transition_matrix[4][4] = pow(angular_momentum_kg_m2_s, 3.0) / pow(gravity_constant_m3_s2, 2.0) / (1.0 + eccentricity * cos(true_anomaly_rad)); transition_matrix[4][5] = 0.0; transition_matrix[5][0] = 0.0; transition_matrix[5][1] = 0.0; - transition_matrix[5][2] = -gravity_constant_m3_s2 * eccentricity * sin(true_anomaly_rad) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[5][2] = -eccentricity * sin(true_anomaly_rad); transition_matrix[5][3] = 0.0; transition_matrix[5][4] = 0.0; - transition_matrix[5][5] = pow(angular_momentum_kg_m2_s, 1.5) / gravity_constant_m3_s2 / (1.0 + eccentricity * cos(true_anomaly_rad)); + transition_matrix[5][5] = pow(angular_momentum_kg_m2_s, 3.0) / pow(gravity_constant_m3_s2, 2.0) / (1.0 + eccentricity * cos(true_anomaly_rad)); return transition_matrix; } @@ -175,42 +175,42 @@ math::Matrix<6, 6> CalcStateTransformationMatrixLvlhToTschaunerHampel(const doub math::Matrix<6, 6> CalcStateTransformationMatrixTschaunerHampelToLvlh(const double gravity_constant_m3_s2, const double eccentricity, const double angular_momentum_kg_m2_s, const double true_anomaly_rad) { math::Matrix<6, 6> transition_matrix; - transition_matrix[0][0] = pow(angular_momentum_kg_m2_s, 1.5) / gravity_constant_m3_s2 / (1.0 + eccentricity * cos(true_anomaly_rad)); + transition_matrix[0][0] = 1.0 / (1.0 + eccentricity * cos(true_anomaly_rad)); transition_matrix[0][1] = 0.0; transition_matrix[0][2] = 0.0; transition_matrix[0][3] = 0.0; transition_matrix[0][4] = 0.0; transition_matrix[0][5] = 0.0; transition_matrix[1][0] = 0.0; - transition_matrix[1][1] = pow(angular_momentum_kg_m2_s, 1.5) / gravity_constant_m3_s2 / (1.0 + eccentricity * cos(true_anomaly_rad)); + transition_matrix[1][1] = 1.0 / (1.0 + eccentricity * cos(true_anomaly_rad)); transition_matrix[1][2] = 0.0; transition_matrix[1][3] = 0.0; transition_matrix[1][4] = 0.0; transition_matrix[1][5] = 0.0; transition_matrix[2][0] = 0.0; transition_matrix[2][1] = 0.0; - transition_matrix[2][2] = pow(angular_momentum_kg_m2_s, 1.5) / gravity_constant_m3_s2 / (1.0 + eccentricity * cos(true_anomaly_rad)); + transition_matrix[2][2] = 1.0 / (1.0 + eccentricity * cos(true_anomaly_rad)); transition_matrix[2][3] = 0.0; transition_matrix[2][4] = 0.0; transition_matrix[2][5] = 0.0; - transition_matrix[3][0] = gravity_constant_m3_s2 * eccentricity * sin(true_anomaly_rad) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[3][0] = pow(gravity_constant_m3_s2, 2.0) * eccentricity * sin(true_anomaly_rad) / pow(angular_momentum_kg_m2_s, 3.0); transition_matrix[3][1] = 0.0; transition_matrix[3][2] = 0.0; - transition_matrix[3][3] = gravity_constant_m3_s2 * (1.0 + eccentricity * cos(true_anomaly_rad)) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[3][3] = pow(gravity_constant_m3_s2, 2.0) * (1.0 + eccentricity * cos(true_anomaly_rad)) / pow(angular_momentum_kg_m2_s, 3.0); transition_matrix[3][4] = 0.0; transition_matrix[3][5] = 0.0; transition_matrix[4][0] = 0.0; - transition_matrix[4][1] = gravity_constant_m3_s2 * eccentricity * sin(true_anomaly_rad) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[4][1] = pow(gravity_constant_m3_s2, 2.0) * eccentricity * sin(true_anomaly_rad) / pow(angular_momentum_kg_m2_s, 3.0); transition_matrix[4][2] = 0.0; transition_matrix[4][3] = 0.0; - transition_matrix[4][4] = gravity_constant_m3_s2 * (1.0 + eccentricity * cos(true_anomaly_rad)) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[4][4] = pow(gravity_constant_m3_s2, 2.0) * (1.0 + eccentricity * cos(true_anomaly_rad)) / pow(angular_momentum_kg_m2_s, 3.0); transition_matrix[4][5] = 0.0; transition_matrix[5][0] = 0.0; transition_matrix[5][1] = 0.0; - transition_matrix[5][2] = gravity_constant_m3_s2 * eccentricity * sin(true_anomaly_rad) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[5][2] = pow(gravity_constant_m3_s2, 2.0) * eccentricity * sin(true_anomaly_rad) / pow(angular_momentum_kg_m2_s, 3.0); transition_matrix[5][3] = 0.0; transition_matrix[5][4] = 0.0; - transition_matrix[5][5] = gravity_constant_m3_s2 * (1.0 + eccentricity * cos(true_anomaly_rad)) / pow(angular_momentum_kg_m2_s, 1.5); + transition_matrix[5][5] = pow(gravity_constant_m3_s2, 2.0) * (1.0 + eccentricity * cos(true_anomaly_rad)) / pow(angular_momentum_kg_m2_s, 3.0); return transition_matrix; }