From a39423c59667e30a3dcdeec502b5a8d1a53aa8b2 Mon Sep 17 00:00:00 2001 From: fukudakazuya Date: Tue, 24 Sep 2024 12:03:08 +0900 Subject: [PATCH 1/3] add relative position stm sabatini --- src/dynamics/orbit/relative_orbit.cpp | 3 +- .../orbit/relative_orbit_models.cpp | 82 ++++++++++++++++++- .../orbit/relative_orbit_models.hpp | 3 +- 3 files changed, 84 insertions(+), 4 deletions(-) diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index bad4c53c7..4d6b98d51 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -113,6 +113,7 @@ void RelativeOrbit::CalculateStm(orbit::StmModel stm_model_type, const Orbit* re orbit::OrbitalElements reference_oe = orbit::OrbitalElements(gravity_constant_m3_s2_, elapsed_sec, reference_sat_orbit->GetPosition_i_m(), reference_sat_orbit->GetVelocity_i_m_s()); math::Vector<3> position_i_m = reference_sat_orbit->GetPosition_i_m(); + math::Vector<3> velocity_i_m_s = reference_sat_orbit->GetVelocity_i_m_s(); double reference_sat_orbit_radius = position_i_m.CalcNorm(); // Temporary codes for the integration by true anomaly double raan_rad = reference_oe.GetRaan_rad(); @@ -139,7 +140,7 @@ void RelativeOrbit::CalculateStm(orbit::StmModel stm_model_type, const Orbit* re break; } case orbit::StmModel::kSabatini: { - stm_ = orbit::CalcSabatiniStm(reference_sat_orbit_radius, gravity_constant_m3_s2, elapsed_sec, &reference_oe); + stm_ = orbit::CalcSabatiniStm(reference_sat_orbit_radius, gravity_constant_m3_s2, elapsed_sec, f_ref_rad, position_i_m, velocity_i_m_s, &reference_oe); break; } case orbit::StmModel::kCarter: { diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index 77d3ce5c3..139f00377 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -123,9 +123,87 @@ math::Vector<6> CalcSsCorrectionTerm(double orbit_radius_m, double gravity_const 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> CalcSabatiniStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, double f_ref_rad, math::Vector<3> position_i_m_s, math::Vector<3> velocity_i_m_s, OrbitalElements* reference_oe) { + math::Matrix<6, 6> A; math::Matrix<6, 6> stm; - // ここでstmを計算してください + + const double mu = gravity_constant_m3_s2; + + const double j2 = 0.001082616; + const double Re = 6378.137e3; + + double t = elapsed_time_s; + double theta = reference_oe->GetArgPerigee_rad() + f_ref_rad; + double i = reference_oe->GetInclination_rad(); + math::Vector<3> r_vec = position_i_m_s; + math::Vector<3> v_vec = velocity_i_m_s; + math::Vector<3> h_0_vec; + + if (elapsed_time_s == 0) { + initial_orbit_radius_m_ = orbit_radius_m; + initial_angular_momentum_m2_s_ = OuterProduct(r_vec, v_vec); + } + + double r_0 = initial_orbit_radius_m_; + double h_0 = initial_angular_momentum_m2_s_.CalcNorm(); + + + double n = sqrt(mu / pow(r_0, 3)); + double X = 3 * j2 * pow(Re, 2) / (2 * r_0) * (1.0 / 3.0 * pow(sin(i), 2) * pow(cos(theta), 2) + 1.0 / 3.0 * pow(sin(i), 2) - 1.0 + (1.0 - 2.0 / 3.0 * pow(sin(i), 2)) * cos(theta)); + double r = r_0 + X; + double f_h = -3.0 / 2.0 * j2 * mu * pow(Re, 2) / pow(r, 4) * sin(theta) * sin(2 * i); + double h = h_0 + 3.0 / 4.0 * mu * j2 * pow(Re, 2) / (r_0 * h_0) * pow(sin(i), 2) * (cos(2 * theta) - 1); + double w_x = r / h * f_h; + double w_z = h / pow(r, 2); + double h_dot = -3.0 / 2.0 * j2 * mu * pow(Re, 2) / pow(r, 3) * sin(2 * theta) * pow(sin(i), 2); + + double r_dot = 3.0 * j2 * pow(Re, 2) / (2 * r_0) * (-1.0 / 3.0 * n * pow(sin(i), 2) * sin(2 * theta) - n * (1 - 2.0 / 3.0 * pow(sin(i), 2)) * sin(theta)); + double w_x_dot = -3 / 2 * j2 * mu * pow(Re, 2) * sin(2 * i) * (n * h * pow(r, 3) * cos(theta) - (h_dot * pow(r, 3) + 3 * h * pow(r, 2) * r_dot) * sin(theta)) / (pow(h, 2) * pow(r, 6)); + double w_z_dot = (h_dot * r - 2 * h * r_dot) / pow(r, 3); + double K = (6.0 * j2 * mu * pow(Re, 2)) / pow(r, 5); + A[0][0] = 0; + A[0][1] = 0; + A[0][2] = 0; + A[0][3] = 1; + A[0][4] = 0; + A[0][5] = 0; + A[1][0] = 0; + A[1][1] = 0; + A[1][2] = 0; + A[1][3] = 0; + A[1][4] = 1; + A[1][5] = 0; + A[2][0] = 0; + A[2][1] = 0; + A[2][2] = 0; + A[2][3] = 0; + A[2][4] = 0; + A[2][5] = 1; + A[3][0] = pow(w_z, 2) + 2.0 * mu / pow(r, 3) + K * (1 - 3 * pow(sin(i), 2) * pow(sin(theta), 2)); + A[3][1] = w_z_dot + K * pow(sin(i), 2) * sin(2 * theta); + A[3][2] = -w_x * w_z + K * sin(2 * i) * sin(theta); + A[3][3] = 0; + A[3][4] = - 2 * w_z; + A[3][5] = 0; + A[4][0] = -w_z_dot + K * sin(2 * i) * sin(theta); + A[4][1] = pow(w_z, 2) + pow(w_x, 2) - mu / pow(r, 3) + K * (-1.0 / 4.0 + pow(sin(i), 2) * (7.0 / 4.0 * pow(sin(theta), 2) - 1.0 / 2.0)); + A[4][2] = w_x_dot + K * (-1.0 / 4.0 - sin(2 * i) * cos(theta)); + A[4][3] = - 2 * w_z; + A[4][4] = 0; + A[4][5] = 2 * w_x; + A[5][0] = -w_x * w_z + K * sin(2 * i) * sin(theta); + A[5][1] = -w_x_dot + K * (-1.0 / 4.0) * sin(2 * i) * cos(theta); + A[5][2] = pow(w_x, 2) - mu / pow(r, 3) + + K * ( (-3.0 / 4.0) + pow(sin(i), 2) + * ( (5.0 / 4.0) * pow(sin(theta), 2) + 1.0 / 2.0) ); + A[5][3] = 0; + A[5][4] = - 2 * w_x; + A[5][5] = 0; + + math::Matrix<6, 6> I; + I = math::MakeIdentityMatrix<6>(); + double dt = 0.01; + stm = I + dt * A + t / 2.0 * A * A + t / 6.0 * A * A * A + t / 24.0 * A * A * A * A + t / 120.0 * A * A * A * A * A + t / 720.0 * A * A * A * A * A * A; return stm; } diff --git a/src/math_physics/orbit/relative_orbit_models.hpp b/src/math_physics/orbit/relative_orbit_models.hpp index 18f6f03ac..1524acf03 100644 --- a/src/math_physics/orbit/relative_orbit_models.hpp +++ b/src/math_physics/orbit/relative_orbit_models.hpp @@ -8,6 +8,7 @@ #include "../math/matrix.hpp" #include "../math/vector.hpp" +#include "../../dynamics/orbit/orbit.hpp" #include "./orbital_elements.hpp" namespace orbit { @@ -87,7 +88,7 @@ math::Vector<6> CalcSsCorrectionTerm(double orbit_radius_m, double gravity_const * @param [in] reference_oe: Orbital elements of reference satellite * @return State Transition Matrix */ -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> CalcSabatiniStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, double f_ref_rad, math::Vector<3> position_i_m_s, math::Vector<3> velocity_i_m_s, OrbitalElements* reference_oe); /** * @fn CalcCarterStm From f53d914a4c5d6fea429522b84b2eddd9a67c40c0 Mon Sep 17 00:00:00 2001 From: fukudakazuya Date: Sat, 5 Oct 2024 18:27:18 +0900 Subject: [PATCH 2/3] add sabatini's system matrix --- src/dynamics/orbit/relative_orbit.cpp | 35 ++++++-- src/dynamics/orbit/relative_orbit.hpp | 5 +- src/math_physics/CMakeLists.txt | 1 + .../orbit/relative_orbit_models.cpp | 84 ----------------- .../orbit/relative_orbit_models.hpp | 13 +-- .../orbit/relative_orbit_sabatini.cpp | 89 +++++++++++++++++++ .../orbit/relative_orbit_sabatini.hpp | 50 +++++++++++ 7 files changed, 173 insertions(+), 104 deletions(-) create mode 100644 src/math_physics/orbit/relative_orbit_sabatini.cpp create mode 100644 src/math_physics/orbit/relative_orbit_sabatini.hpp diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index 4d6b98d51..1d12bd4bf 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -19,7 +19,8 @@ RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, update_method_(update_method), relative_dynamics_model_type_(relative_dynamics_model_type), stm_model_type_(stm_model_type), - relative_information_(relative_information) { + relative_information_(relative_information), + relative_orbit_sabatini_(relative_information->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetPosition_i_m(), relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetVelocity_i_m_s()) { propagate_mode_ = OrbitPropagateMode::kRelativeOrbit; propagation_time_s_ = 0.0; @@ -54,7 +55,7 @@ void RelativeOrbit::InitializeState(math::Vector<3> relative_position_lvlh_m, ma if (update_method_ == kRk4) { Setup(initial_time_s, initial_state_); - CalculateSystemMatrix(relative_dynamics_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), + CalculateSystemMatrix(relative_dynamics_model_type_, 0.0, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), gravity_constant_m3_s2); } else // update_method_ == STM { @@ -68,12 +69,27 @@ void RelativeOrbit::InitializeState(math::Vector<3> relative_position_lvlh_m, ma TransformEcefToGeodetic(); } -void RelativeOrbit::CalculateSystemMatrix(orbit::RelativeOrbitModel relative_dynamics_model_type, const Orbit* reference_sat_orbit, +void RelativeOrbit::CalculateSystemMatrix(orbit::RelativeOrbitModel relative_dynamics_model_type, double elapsed_time, const Orbit* reference_sat_orbit, double gravity_constant_m3_s2) { switch (relative_dynamics_model_type) { case orbit::RelativeOrbitModel::kHill: { double reference_sat_orbit_radius = reference_sat_orbit->GetPosition_i_m().CalcNorm(); system_matrix_ = orbit::CalcHillSystemMatrix(reference_sat_orbit_radius, gravity_constant_m3_s2); + break; + } + case orbit::RelativeOrbitModel::kSabatini: { + orbit::OrbitalElements reference_oe = orbit::OrbitalElements(gravity_constant_m3_s2_, elapsed_time, reference_sat_orbit->GetPosition_i_m(), reference_sat_orbit->GetVelocity_i_m_s()); + math::Vector<3> position_i_m = reference_sat_orbit->GetPosition_i_m(); + double raan_rad = reference_oe.GetRaan_rad(); + double inclination_rad = reference_oe.GetInclination_rad(); + double arg_perigee_rad = reference_oe.GetArgPerigee_rad(); + double x_p_m = position_i_m[0] * cos(raan_rad) + position_i_m[1] * sin(raan_rad); + double tmp_m = -position_i_m[0] * sin(raan_rad) + position_i_m[1] * cos(raan_rad); + double y_p_m = tmp_m * cos(inclination_rad) + position_i_m[2] * sin(inclination_rad); + double phi_rad = atan2(y_p_m, x_p_m); + double f_ref_rad = phi_rad - arg_perigee_rad; + system_matrix_ = relative_orbit_sabatini_.CalcSabatiniSystemMatrix(gravity_constant_m3_s2_, f_ref_rad, &reference_oe); + break; } default: { // NOT REACHED @@ -139,10 +155,6 @@ void RelativeOrbit::CalculateStm(orbit::StmModel stm_model_type, const Orbit* re correction_term_ = orbit::CalcSsCorrectionTerm(reference_sat_orbit_radius, gravity_constant_m3_s2, elapsed_sec, &reference_oe); break; } - case orbit::StmModel::kSabatini: { - stm_ = orbit::CalcSabatiniStm(reference_sat_orbit_radius, gravity_constant_m3_s2, elapsed_sec, f_ref_rad, position_i_m, velocity_i_m_s, &reference_oe); - break; - } case orbit::StmModel::kCarter: { stm_ = orbit::CalcCarterStm(reference_sat_orbit_radius, gravity_constant_m3_s2, f_ref_rad, &reference_oe); break; @@ -166,6 +178,15 @@ void RelativeOrbit::Propagate(const double end_time_s, const double current_time spacecraft_acceleration_i_m_s2_ *= 0.0; // Disturbance acceleration are not considered in relative orbit propagation if (update_method_ == kRk4) { + switch (relative_dynamics_model_type_) { + case orbit::RelativeOrbitModel::kSabatini: { + CalculateSystemMatrix(relative_dynamics_model_type_, end_time_s, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), gravity_constant_m3_s2_); + } + default: { + // NOT REACHED + break; + } + } PropagateRk4(end_time_s); } else // update_method_ == STM { diff --git a/src/dynamics/orbit/relative_orbit.hpp b/src/dynamics/orbit/relative_orbit.hpp index 44bb44f7b..fce1e6a19 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,6 +87,7 @@ 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::RelativeOrbitSabatini relative_orbit_sabatini_; //!< Relative Orbit Calculater with Sabatini's STM orbit::RelativeOrbitYamanakaAnkersen relative_orbit_yamanaka_ankersen_; //!< Relative Orbit Calcilater with Yamanaka-Ankersen's STM /** @@ -102,10 +104,11 @@ class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> * @fn CalculateSystemMatrix * @brief Calculate system matrix * @param [in] relative_dynamics_model_type: Relative dynamics model type + * @param [in] elapsed_time: Elapsed time [sec] * @param [in] reference_sat_orbit: Orbit information of reference satellite * @param [in] gravity_constant_m3_s2: Gravity constant of the center body [m3/s2] */ - void CalculateSystemMatrix(orbit::RelativeOrbitModel relative_dynamics_model_type, const Orbit* reference_sat_orbit, double gravity_constant_m3_s2); + void CalculateSystemMatrix(orbit::RelativeOrbitModel relative_dynamics_model_type, double elapsed_time, const Orbit* reference_sat_orbit, double gravity_constant_m3_s2); /** * @fn InitializeStmMatrix * @brief Calculate State Transition Matrix diff --git a/src/math_physics/CMakeLists.txt b/src/math_physics/CMakeLists.txt index 94b5a665b..61eec4fc0 100644 --- a/src/math_physics/CMakeLists.txt +++ b/src/math_physics/CMakeLists.txt @@ -32,6 +32,7 @@ add_library(${PROJECT_NAME} STATIC orbit/orbital_elements.cpp orbit/kepler_orbit.cpp orbit/relative_orbit_models.cpp + orbit/relative_orbit_sabatini.cpp orbit/relative_orbit_yamanaka_ankersen.cpp orbit/interpolation_orbit.cpp orbit/sgp4/sgp4ext.cpp diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index 139f00377..419fa4a3f 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -123,90 +123,6 @@ math::Vector<6> CalcSsCorrectionTerm(double orbit_radius_m, double gravity_const return correction_term; } -math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, double f_ref_rad, math::Vector<3> position_i_m_s, math::Vector<3> velocity_i_m_s, OrbitalElements* reference_oe) { - math::Matrix<6, 6> A; - math::Matrix<6, 6> stm; - - const double mu = gravity_constant_m3_s2; - - const double j2 = 0.001082616; - const double Re = 6378.137e3; - - double t = elapsed_time_s; - double theta = reference_oe->GetArgPerigee_rad() + f_ref_rad; - double i = reference_oe->GetInclination_rad(); - math::Vector<3> r_vec = position_i_m_s; - math::Vector<3> v_vec = velocity_i_m_s; - math::Vector<3> h_0_vec; - - if (elapsed_time_s == 0) { - initial_orbit_radius_m_ = orbit_radius_m; - initial_angular_momentum_m2_s_ = OuterProduct(r_vec, v_vec); - } - - double r_0 = initial_orbit_radius_m_; - double h_0 = initial_angular_momentum_m2_s_.CalcNorm(); - - - double n = sqrt(mu / pow(r_0, 3)); - double X = 3 * j2 * pow(Re, 2) / (2 * r_0) * (1.0 / 3.0 * pow(sin(i), 2) * pow(cos(theta), 2) + 1.0 / 3.0 * pow(sin(i), 2) - 1.0 + (1.0 - 2.0 / 3.0 * pow(sin(i), 2)) * cos(theta)); - double r = r_0 + X; - double f_h = -3.0 / 2.0 * j2 * mu * pow(Re, 2) / pow(r, 4) * sin(theta) * sin(2 * i); - double h = h_0 + 3.0 / 4.0 * mu * j2 * pow(Re, 2) / (r_0 * h_0) * pow(sin(i), 2) * (cos(2 * theta) - 1); - double w_x = r / h * f_h; - double w_z = h / pow(r, 2); - double h_dot = -3.0 / 2.0 * j2 * mu * pow(Re, 2) / pow(r, 3) * sin(2 * theta) * pow(sin(i), 2); - - double r_dot = 3.0 * j2 * pow(Re, 2) / (2 * r_0) * (-1.0 / 3.0 * n * pow(sin(i), 2) * sin(2 * theta) - n * (1 - 2.0 / 3.0 * pow(sin(i), 2)) * sin(theta)); - double w_x_dot = -3 / 2 * j2 * mu * pow(Re, 2) * sin(2 * i) * (n * h * pow(r, 3) * cos(theta) - (h_dot * pow(r, 3) + 3 * h * pow(r, 2) * r_dot) * sin(theta)) / (pow(h, 2) * pow(r, 6)); - double w_z_dot = (h_dot * r - 2 * h * r_dot) / pow(r, 3); - double K = (6.0 * j2 * mu * pow(Re, 2)) / pow(r, 5); - A[0][0] = 0; - A[0][1] = 0; - A[0][2] = 0; - A[0][3] = 1; - A[0][4] = 0; - A[0][5] = 0; - A[1][0] = 0; - A[1][1] = 0; - A[1][2] = 0; - A[1][3] = 0; - A[1][4] = 1; - A[1][5] = 0; - A[2][0] = 0; - A[2][1] = 0; - A[2][2] = 0; - A[2][3] = 0; - A[2][4] = 0; - A[2][5] = 1; - A[3][0] = pow(w_z, 2) + 2.0 * mu / pow(r, 3) + K * (1 - 3 * pow(sin(i), 2) * pow(sin(theta), 2)); - A[3][1] = w_z_dot + K * pow(sin(i), 2) * sin(2 * theta); - A[3][2] = -w_x * w_z + K * sin(2 * i) * sin(theta); - A[3][3] = 0; - A[3][4] = - 2 * w_z; - A[3][5] = 0; - A[4][0] = -w_z_dot + K * sin(2 * i) * sin(theta); - A[4][1] = pow(w_z, 2) + pow(w_x, 2) - mu / pow(r, 3) + K * (-1.0 / 4.0 + pow(sin(i), 2) * (7.0 / 4.0 * pow(sin(theta), 2) - 1.0 / 2.0)); - A[4][2] = w_x_dot + K * (-1.0 / 4.0 - sin(2 * i) * cos(theta)); - A[4][3] = - 2 * w_z; - A[4][4] = 0; - A[4][5] = 2 * w_x; - A[5][0] = -w_x * w_z + K * sin(2 * i) * sin(theta); - A[5][1] = -w_x_dot + K * (-1.0 / 4.0) * sin(2 * i) * cos(theta); - A[5][2] = pow(w_x, 2) - mu / pow(r, 3) - + K * ( (-3.0 / 4.0) + pow(sin(i), 2) - * ( (5.0 / 4.0) * pow(sin(theta), 2) + 1.0 / 2.0) ); - A[5][3] = 0; - A[5][4] = - 2 * w_x; - A[5][5] = 0; - - math::Matrix<6, 6> I; - I = math::MakeIdentityMatrix<6>(); - double dt = 0.01; - stm = I + dt * A + t / 2.0 * A * A + t / 6.0 * A * A * A + t / 24.0 * A * A * A * A + t / 120.0 * A * A * A * A * A + t / 720.0 * A * A * A * A * A * A; - 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> stm; // ここでstmを計算してください diff --git a/src/math_physics/orbit/relative_orbit_models.hpp b/src/math_physics/orbit/relative_orbit_models.hpp index 1524acf03..3cc274278 100644 --- a/src/math_physics/orbit/relative_orbit_models.hpp +++ b/src/math_physics/orbit/relative_orbit_models.hpp @@ -17,7 +17,7 @@ namespace orbit { * @enum RelativeOrbitModel * @brief Relative orbit model */ -enum class RelativeOrbitModel { kHill = 0 }; +enum class RelativeOrbitModel { kHill = 0, kSabatini = 1 }; /** * @enum StmModel @@ -79,17 +79,6 @@ math::Matrix<6, 6> CalcSsStm(double orbit_radius_m, double gravity_constant_m3_s */ math::Vector<6> CalcSsCorrectionTerm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, OrbitalElements* reference_oe); -/** - * @fn CalcSabatiniStm - * @brief Calculate Sabatani 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] elapsed_time_s: Elapsed time [s] - * @param [in] reference_oe: Orbital elements of reference satellite - * @return State Transition Matrix - */ -math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constant_m3_s2, double elapsed_time_s, double f_ref_rad, math::Vector<3> position_i_m_s, math::Vector<3> velocity_i_m_s, OrbitalElements* reference_oe); - /** * @fn CalcCarterStm * @brief Calculate Carter State Transition Matrix diff --git a/src/math_physics/orbit/relative_orbit_sabatini.cpp b/src/math_physics/orbit/relative_orbit_sabatini.cpp new file mode 100644 index 000000000..9b46ddde2 --- /dev/null +++ b/src/math_physics/orbit/relative_orbit_sabatini.cpp @@ -0,0 +1,89 @@ +/** + * @file relative_orbit_sabatini.cpp + * @brief Functions to calculate Yamanaka-Ankersen STM for relative orbit + */ +#include "relative_orbit_sabatini.hpp" + +#include + + + +namespace orbit { + +RelativeOrbitSabatini::RelativeOrbitSabatini(math::Vector<3> position_i_m, math::Vector<3> velocity_i_m_s) { + initial_orbit_radius_m_ = position_i_m.CalcNorm(); + initial_angular_momentum_m2_s_ = OuterProduct(position_i_m, velocity_i_m_s).CalcNorm(); +} + +RelativeOrbitSabatini::~RelativeOrbitSabatini() {} + +math::Matrix<6, 6> RelativeOrbitSabatini::CalcSabatiniSystemMatrix(double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe) { + const double mu = gravity_constant_m3_s2; + + const double j2 = 0.001082616; + const double Re = 6378.137e3; + + double r_0 = initial_orbit_radius_m_; + double h_0 = initial_angular_momentum_m2_s_; + + double theta = reference_oe->GetArgPerigee_rad() + f_ref_rad; + double i = reference_oe->GetInclination_rad(); + + double n = sqrt(mu / pow(r_0, 3)); + double X = 3 * j2 * pow(Re, 2) / (2 * r_0) * (1.0 / 3.0 * pow(sin(i), 2) * pow(cos(theta), 2) + 1.0 / 3.0 * pow(sin(i), 2) - 1.0 + (1.0 - 2.0 / 3.0 * pow(sin(i), 2)) * cos(theta)); + double r = r_0 + X; + double f_h = -3.0 / 2.0 * j2 * mu * pow(Re, 2) / pow(r, 4) * sin(theta) * sin(2 * i); + double h = h_0 + 3.0 / 4.0 * mu * j2 * pow(Re, 2) / (r_0 * h_0) * pow(sin(i), 2) * (cos(2 * theta) - 1); + double w_x = r / h * f_h; + double w_z = h / pow(r, 2); + double h_dot = -3.0 / 2.0 * j2 * mu * pow(Re, 2) / pow(r, 3) * sin(2 * theta) * pow(sin(i), 2); + double r_dot = 3.0 * j2 * pow(Re, 2) / (2 * r_0) * (-1.0 / 3.0 * n * pow(sin(i), 2) * sin(2 * theta) - n * (1 - 2.0 / 3.0 * pow(sin(i), 2)) * sin(theta)); + double w_x_dot = -3 / 2 * j2 * mu * pow(Re, 2) * sin(2 * i) * (n * h * pow(r, 3) * cos(theta) - (h_dot * pow(r, 3) + 3 * h * pow(r, 2) * r_dot) * sin(theta)) / (pow(h, 2) * pow(r, 6)); + double w_z_dot = (h_dot * r - 2 * h * r_dot) / pow(r, 3); + double K = (6.0 * j2 * mu * pow(Re, 2)) / pow(r, 5); + + math::Matrix<6, 6> system_matrix; + + system_matrix[0][0] = 0; + system_matrix[0][1] = 0; + system_matrix[0][2] = 0; + system_matrix[0][3] = 1; + system_matrix[0][4] = 0; + system_matrix[0][5] = 0; + system_matrix[1][0] = 0; + system_matrix[1][1] = 0; + system_matrix[1][2] = 0; + system_matrix[1][3] = 0; + system_matrix[1][4] = 1; + system_matrix[1][5] = 0; + system_matrix[2][0] = 0; + system_matrix[2][1] = 0; + system_matrix[2][2] = 0; + system_matrix[2][3] = 0; + system_matrix[2][4] = 0; + system_matrix[2][5] = 1; + system_matrix[3][0] = pow(w_z, 2) + 2.0 * mu / pow(r, 3) + K * (1 - 3 * pow(sin(i), 2) * pow(sin(theta), 2)); + system_matrix[3][1] = w_z_dot + K * pow(sin(i), 2) * sin(2 * theta); + system_matrix[3][2] = -w_x * w_z + K * sin(2 * i) * sin(theta); + system_matrix[3][3] = 0; + system_matrix[3][4] = - 2 * w_z; + system_matrix[3][5] = 0; + system_matrix[4][0] = -w_z_dot + K * sin(2 * i) * sin(theta); + system_matrix[4][1] = pow(w_z, 2) + pow(w_x, 2) - mu / pow(r, 3) + K * (-1.0 / 4.0 + pow(sin(i), 2) * (7.0 / 4.0 * pow(sin(theta), 2) - 1.0 / 2.0)); + system_matrix[4][2] = w_x_dot + K * (-1.0 / 4.0 - sin(2 * i) * cos(theta)); + system_matrix[4][3] = - 2 * w_z; + system_matrix[4][4] = 0; + system_matrix[4][5] = 2 * w_x; + system_matrix[5][0] = -w_x * w_z + K * sin(2 * i) * sin(theta); + system_matrix[5][1] = -w_x_dot + K * (-1.0 / 4.0) * sin(2 * i) * cos(theta); + system_matrix[5][2] = pow(w_x, 2) - mu / pow(r, 3) + + K * ( (-3.0 / 4.0) + pow(sin(i), 2) + * ( (5.0 / 4.0) * pow(sin(theta), 2) + 1.0 / 2.0) ); + system_matrix[5][3] = 0; + system_matrix[5][4] = - 2 * w_x; + system_matrix[5][5] = 0; + + return system_matrix; +} + +} // namespace orbit diff --git a/src/math_physics/orbit/relative_orbit_sabatini.hpp b/src/math_physics/orbit/relative_orbit_sabatini.hpp new file mode 100644 index 000000000..d7445fc48 --- /dev/null +++ b/src/math_physics/orbit/relative_orbit_sabatini.hpp @@ -0,0 +1,50 @@ +/** + * @file relative_orbit_sabatini.hpp + * @brief Functions to calculate sabatini STM for relative orbit + */ + +#ifndef S2E_LIBRARY_ORBIT_RELATIVE_ORBIT_SABATINI_HPP_ +#define S2E_LIBRARY_ORBIT_RELATIVE_ORBIT_SABATINI_HPP_ + +#include "../math/matrix.hpp" +#include "../math/vector.hpp" +#include "./orbital_elements.hpp" + +namespace orbit { + +/** + * @class RelativeOrbitSabatini + * @brief Class to calculate Sabatini relative orbital STM + */ +class RelativeOrbitSabatini { + public: + /** + * @fn RelativeOrbitSabatini + * @param [in] position_i_m: Initial value of position in the inertial frame [m] + * @param [in] velocity_i_m_s: Initial value of velocity in the inertial frame [m/s] + * @brief Default Constructor + */ + RelativeOrbitSabatini(math::Vector<3> position_i_m, math::Vector<3> velocity_i_m_s); + /** + * @fn ~RelativeOrbitSabatini + * @brief Destructor + */ + ~RelativeOrbitSabatini(); + + /** + * @fn CalculateSystemMatrix + * @brief Calculate system matrix for relative orbit + * @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> CalcSabatiniSystemMatrix(double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe); + + private: + double initial_orbit_radius_m_; //!< Initial orbit radius [m] + double initial_angular_momentum_m2_s_; //!< Initial angular momentum [m2/s] +}; + +} // namespace orbit + +#endif // S2E_LIBRARY_ORBIT_RELATIVE_ORBIT_SABATINI_HPP_ \ No newline at end of file From 81119d4bc1c255169f7c8127dead7f424e860b71 Mon Sep 17 00:00:00 2001 From: fukudakazuya Date: Sat, 5 Oct 2024 18:59:54 +0900 Subject: [PATCH 3/3] modify format --- src/dynamics/orbit/relative_orbit.cpp | 26 +++++++++++-------- src/dynamics/orbit/relative_orbit.hpp | 16 +++++++----- .../orbit/relative_orbit_models.cpp | 1 + .../orbit/relative_orbit_models.hpp | 3 ++- .../orbit/relative_orbit_sabatini.cpp | 22 ++++++++-------- .../orbit/relative_orbit_sabatini.hpp | 6 ++--- 6 files changed, 41 insertions(+), 33 deletions(-) diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index 1d12bd4bf..e09807411 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -20,7 +20,8 @@ RelativeOrbit::RelativeOrbit(const CelestialInformation* celestial_information, relative_dynamics_model_type_(relative_dynamics_model_type), stm_model_type_(stm_model_type), relative_information_(relative_information), - relative_orbit_sabatini_(relative_information->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetPosition_i_m(), relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetVelocity_i_m_s()) { + relative_orbit_sabatini_(relative_information->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetPosition_i_m(), + relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit().GetVelocity_i_m_s()) { propagate_mode_ = OrbitPropagateMode::kRelativeOrbit; propagation_time_s_ = 0.0; @@ -69,8 +70,8 @@ void RelativeOrbit::InitializeState(math::Vector<3> relative_position_lvlh_m, ma TransformEcefToGeodetic(); } -void RelativeOrbit::CalculateSystemMatrix(orbit::RelativeOrbitModel relative_dynamics_model_type, double elapsed_time, const Orbit* reference_sat_orbit, - double gravity_constant_m3_s2) { +void RelativeOrbit::CalculateSystemMatrix(orbit::RelativeOrbitModel relative_dynamics_model_type, double elapsed_time, + const Orbit* reference_sat_orbit, double gravity_constant_m3_s2) { switch (relative_dynamics_model_type) { case orbit::RelativeOrbitModel::kHill: { double reference_sat_orbit_radius = reference_sat_orbit->GetPosition_i_m().CalcNorm(); @@ -78,7 +79,8 @@ void RelativeOrbit::CalculateSystemMatrix(orbit::RelativeOrbitModel relative_dyn break; } case orbit::RelativeOrbitModel::kSabatini: { - orbit::OrbitalElements reference_oe = orbit::OrbitalElements(gravity_constant_m3_s2_, elapsed_time, reference_sat_orbit->GetPosition_i_m(), reference_sat_orbit->GetVelocity_i_m_s()); + orbit::OrbitalElements reference_oe = orbit::OrbitalElements(gravity_constant_m3_s2_, elapsed_time, reference_sat_orbit->GetPosition_i_m(), + reference_sat_orbit->GetVelocity_i_m_s()); math::Vector<3> position_i_m = reference_sat_orbit->GetPosition_i_m(); double raan_rad = reference_oe.GetRaan_rad(); double inclination_rad = reference_oe.GetInclination_rad(); @@ -179,14 +181,15 @@ void RelativeOrbit::Propagate(const double end_time_s, const double current_time if (update_method_ == kRk4) { switch (relative_dynamics_model_type_) { - case orbit::RelativeOrbitModel::kSabatini: { - CalculateSystemMatrix(relative_dynamics_model_type_, end_time_s, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), gravity_constant_m3_s2_); + case orbit::RelativeOrbitModel::kSabatini: { + CalculateSystemMatrix(relative_dynamics_model_type_, end_time_s, + &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), gravity_constant_m3_s2_); + } + default: { + // NOT REACHED + break; + } } - default: { - // NOT REACHED - break; - } - } PropagateRk4(end_time_s); } else // update_method_ == STM { @@ -242,3 +245,4 @@ void RelativeOrbit::DerivativeFunction(double t, const math::Vector<6>& state, rhs = system_matrix_ * state; (void)t; } + diff --git a/src/dynamics/orbit/relative_orbit.hpp b/src/dynamics/orbit/relative_orbit.hpp index fce1e6a19..40af42d23 100644 --- a/src/dynamics/orbit/relative_orbit.hpp +++ b/src/dynamics/orbit/relative_orbit.hpp @@ -8,8 +8,8 @@ #include #include -#include #include +#include #include #include @@ -83,11 +83,11 @@ class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> math::Vector<3> relative_position_lvlh_m_; //!< Relative position in the LVLH frame math::Vector<3> relative_velocity_lvlh_m_s_; //!< Relative velocity in the LVLH frame - RelativeOrbitUpdateMethod update_method_; //!< Update method - 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::RelativeOrbitSabatini relative_orbit_sabatini_; //!< Relative Orbit Calculater with Sabatini's STM + RelativeOrbitUpdateMethod update_method_; //!< Update method + 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::RelativeOrbitSabatini relative_orbit_sabatini_; //!< Relative Orbit Calculater with Sabatini's STM orbit::RelativeOrbitYamanakaAnkersen relative_orbit_yamanaka_ankersen_; //!< Relative Orbit Calcilater with Yamanaka-Ankersen's STM /** @@ -108,7 +108,8 @@ class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> * @param [in] reference_sat_orbit: Orbit information of reference satellite * @param [in] gravity_constant_m3_s2: Gravity constant of the center body [m3/s2] */ - void CalculateSystemMatrix(orbit::RelativeOrbitModel relative_dynamics_model_type, double elapsed_time, const Orbit* reference_sat_orbit, double gravity_constant_m3_s2); + void CalculateSystemMatrix(orbit::RelativeOrbitModel relative_dynamics_model_type, double elapsed_time, const Orbit* reference_sat_orbit, + double gravity_constant_m3_s2); /** * @fn InitializeStmMatrix * @brief Calculate State Transition Matrix @@ -142,3 +143,4 @@ class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> }; #endif // S2E_DYNAMICS_ORBIT_RELATIVE_ORBIT_HPP_ + diff --git a/src/math_physics/orbit/relative_orbit_models.cpp b/src/math_physics/orbit/relative_orbit_models.cpp index 419fa4a3f..20a068110 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -130,3 +130,4 @@ math::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_ } } // namespace orbit + diff --git a/src/math_physics/orbit/relative_orbit_models.hpp b/src/math_physics/orbit/relative_orbit_models.hpp index 3cc274278..18d779231 100644 --- a/src/math_physics/orbit/relative_orbit_models.hpp +++ b/src/math_physics/orbit/relative_orbit_models.hpp @@ -6,9 +6,9 @@ #ifndef S2E_LIBRARY_ORBIT_RELATIVE_ORBIT_MODEL_HPP_ #define S2E_LIBRARY_ORBIT_RELATIVE_ORBIT_MODEL_HPP_ +#include "../../dynamics/orbit/orbit.hpp" #include "../math/matrix.hpp" #include "../math/vector.hpp" -#include "../../dynamics/orbit/orbit.hpp" #include "./orbital_elements.hpp" namespace orbit { @@ -93,3 +93,4 @@ math::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_ } // namespace orbit #endif // S2E_LIBRARY_ORBIT_RELATIVE_ORBIT_MODEL_HPP_ + diff --git a/src/math_physics/orbit/relative_orbit_sabatini.cpp b/src/math_physics/orbit/relative_orbit_sabatini.cpp index 9b46ddde2..2548ef683 100644 --- a/src/math_physics/orbit/relative_orbit_sabatini.cpp +++ b/src/math_physics/orbit/relative_orbit_sabatini.cpp @@ -6,8 +6,6 @@ #include - - namespace orbit { RelativeOrbitSabatini::RelativeOrbitSabatini(math::Vector<3> position_i_m, math::Vector<3> velocity_i_m_s) { @@ -30,15 +28,18 @@ math::Matrix<6, 6> RelativeOrbitSabatini::CalcSabatiniSystemMatrix(double gravit double i = reference_oe->GetInclination_rad(); double n = sqrt(mu / pow(r_0, 3)); - double X = 3 * j2 * pow(Re, 2) / (2 * r_0) * (1.0 / 3.0 * pow(sin(i), 2) * pow(cos(theta), 2) + 1.0 / 3.0 * pow(sin(i), 2) - 1.0 + (1.0 - 2.0 / 3.0 * pow(sin(i), 2)) * cos(theta)); + double X = 3 * j2 * pow(Re, 2) / (2 * r_0) * + (1.0 / 3.0 * pow(sin(i), 2) * pow(cos(theta), 2) + 1.0 / 3.0 * pow(sin(i), 2) - 1.0 + (1.0 - 2.0 / 3.0 * pow(sin(i), 2)) * cos(theta)); double r = r_0 + X; double f_h = -3.0 / 2.0 * j2 * mu * pow(Re, 2) / pow(r, 4) * sin(theta) * sin(2 * i); double h = h_0 + 3.0 / 4.0 * mu * j2 * pow(Re, 2) / (r_0 * h_0) * pow(sin(i), 2) * (cos(2 * theta) - 1); double w_x = r / h * f_h; double w_z = h / pow(r, 2); double h_dot = -3.0 / 2.0 * j2 * mu * pow(Re, 2) / pow(r, 3) * sin(2 * theta) * pow(sin(i), 2); - double r_dot = 3.0 * j2 * pow(Re, 2) / (2 * r_0) * (-1.0 / 3.0 * n * pow(sin(i), 2) * sin(2 * theta) - n * (1 - 2.0 / 3.0 * pow(sin(i), 2)) * sin(theta)); - double w_x_dot = -3 / 2 * j2 * mu * pow(Re, 2) * sin(2 * i) * (n * h * pow(r, 3) * cos(theta) - (h_dot * pow(r, 3) + 3 * h * pow(r, 2) * r_dot) * sin(theta)) / (pow(h, 2) * pow(r, 6)); + double r_dot = + 3.0 * j2 * pow(Re, 2) / (2 * r_0) * (-1.0 / 3.0 * n * pow(sin(i), 2) * sin(2 * theta) - n * (1 - 2.0 / 3.0 * pow(sin(i), 2)) * sin(theta)); + double w_x_dot = -3 / 2 * j2 * mu * pow(Re, 2) * sin(2 * i) * + (n * h * pow(r, 3) * cos(theta) - (h_dot * pow(r, 3) + 3 * h * pow(r, 2) * r_dot) * sin(theta)) / (pow(h, 2) * pow(r, 6)); double w_z_dot = (h_dot * r - 2 * h * r_dot) / pow(r, 3); double K = (6.0 * j2 * mu * pow(Re, 2)) / pow(r, 5); @@ -66,24 +67,23 @@ math::Matrix<6, 6> RelativeOrbitSabatini::CalcSabatiniSystemMatrix(double gravit system_matrix[3][1] = w_z_dot + K * pow(sin(i), 2) * sin(2 * theta); system_matrix[3][2] = -w_x * w_z + K * sin(2 * i) * sin(theta); system_matrix[3][3] = 0; - system_matrix[3][4] = - 2 * w_z; + system_matrix[3][4] = -2 * w_z; system_matrix[3][5] = 0; system_matrix[4][0] = -w_z_dot + K * sin(2 * i) * sin(theta); system_matrix[4][1] = pow(w_z, 2) + pow(w_x, 2) - mu / pow(r, 3) + K * (-1.0 / 4.0 + pow(sin(i), 2) * (7.0 / 4.0 * pow(sin(theta), 2) - 1.0 / 2.0)); system_matrix[4][2] = w_x_dot + K * (-1.0 / 4.0 - sin(2 * i) * cos(theta)); - system_matrix[4][3] = - 2 * w_z; + system_matrix[4][3] = -2 * w_z; system_matrix[4][4] = 0; system_matrix[4][5] = 2 * w_x; system_matrix[5][0] = -w_x * w_z + K * sin(2 * i) * sin(theta); system_matrix[5][1] = -w_x_dot + K * (-1.0 / 4.0) * sin(2 * i) * cos(theta); - system_matrix[5][2] = pow(w_x, 2) - mu / pow(r, 3) - + K * ( (-3.0 / 4.0) + pow(sin(i), 2) - * ( (5.0 / 4.0) * pow(sin(theta), 2) + 1.0 / 2.0) ); + system_matrix[5][2] = pow(w_x, 2) - mu / pow(r, 3) + K * ((-3.0 / 4.0) + pow(sin(i), 2) * ((5.0 / 4.0) * pow(sin(theta), 2) + 1.0 / 2.0)); system_matrix[5][3] = 0; - system_matrix[5][4] = - 2 * w_x; + system_matrix[5][4] = -2 * w_x; system_matrix[5][5] = 0; return system_matrix; } } // namespace orbit + diff --git a/src/math_physics/orbit/relative_orbit_sabatini.hpp b/src/math_physics/orbit/relative_orbit_sabatini.hpp index d7445fc48..f7e3e0c22 100644 --- a/src/math_physics/orbit/relative_orbit_sabatini.hpp +++ b/src/math_physics/orbit/relative_orbit_sabatini.hpp @@ -41,10 +41,10 @@ class RelativeOrbitSabatini { math::Matrix<6, 6> CalcSabatiniSystemMatrix(double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe); private: - double initial_orbit_radius_m_; //!< Initial orbit radius [m] - double initial_angular_momentum_m2_s_; //!< Initial angular momentum [m2/s] + double initial_orbit_radius_m_; //!< Initial orbit radius [m] + double initial_angular_momentum_m2_s_; //!< Initial angular momentum [m2/s] }; } // namespace orbit -#endif // S2E_LIBRARY_ORBIT_RELATIVE_ORBIT_SABATINI_HPP_ \ No newline at end of file +#endif // S2E_LIBRARY_ORBIT_RELATIVE_ORBIT_SABATINI_HPP_