diff --git a/src/dynamics/orbit/relative_orbit.hpp b/src/dynamics/orbit/relative_orbit.hpp index 4377ce5e7..101de90cc 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 @@ -87,7 +88,7 @@ class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> 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::RelativeOrbitYamanakaCarter* relative_orbit_carter_; //!< Relative Orbit Calcilater with Carter's STM + orbit::RelativeOrbitCarter* relative_orbit_carter_; //!< Relative Orbit Calcilater with Carter's STM /** * @fn InitializeState 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"