diff --git a/src/dynamics/orbit/relative_orbit.cpp b/src/dynamics/orbit/relative_orbit.cpp index bad4c53c7..e09807411 100644 --- a/src/dynamics/orbit/relative_orbit.cpp +++ b/src/dynamics/orbit/relative_orbit.cpp @@ -19,7 +19,9 @@ 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 +56,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 +70,28 @@ 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, - 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(); 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 @@ -113,6 +131,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(); @@ -138,10 +157,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, &reference_oe); - break; - } case orbit::StmModel::kCarter: { stm_ = orbit::CalcCarterStm(reference_sat_orbit_radius, gravity_constant_m3_s2, f_ref_rad, &reference_oe); break; @@ -165,6 +180,16 @@ 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 { @@ -220,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 44bb44f7b..40af42d23 100644 --- a/src/dynamics/orbit/relative_orbit.hpp +++ b/src/dynamics/orbit/relative_orbit.hpp @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -82,10 +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 + 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 /** @@ -102,10 +104,12 @@ 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 @@ -139,3 +143,4 @@ class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6> }; #endif // S2E_DYNAMICS_ORBIT_RELATIVE_ORBIT_HPP_ + 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 77d3ce5c3..20a068110 100644 --- a/src/math_physics/orbit/relative_orbit_models.cpp +++ b/src/math_physics/orbit/relative_orbit_models.cpp @@ -123,12 +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, OrbitalElements* reference_oe) { - math::Matrix<6, 6> stm; - // ここでstmを計算してください - 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を計算してください @@ -136,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 18f6f03ac..18d779231 100644 --- a/src/math_physics/orbit/relative_orbit_models.hpp +++ b/src/math_physics/orbit/relative_orbit_models.hpp @@ -6,6 +6,7 @@ #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 "./orbital_elements.hpp" @@ -16,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 @@ -78,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, OrbitalElements* reference_oe); - /** * @fn CalcCarterStm * @brief Calculate Carter State Transition Matrix @@ -103,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 new file mode 100644 index 000000000..2548ef683 --- /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..f7e3e0c22 --- /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_