Feature/add relative position stm sabatini

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;
 }
+

src/dynamics/orbit/relative_orbit.hpp

@@ -8,6 +8,7 @@
 
 #include <math_physics/math/ordinary_differential_equation.hpp>
 #include <math_physics/orbit/relative_orbit_models.hpp>
+#include <math_physics/orbit/relative_orbit_sabatini.hpp>
 #include <math_physics/orbit/relative_orbit_yamanaka_ankersen.hpp>
 #include <simulation/multiple_spacecraft/relative_information.hpp>
 #include <string>
@@ -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_
+

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

src/math_physics/orbit/relative_orbit_models.cpp

@@ -123,16 +123,11 @@ 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を計算してください
   return stm;
 }
 
 }  // namespace orbit
+

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_
+

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 <environment/global/physical_constants.hpp>
+
+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
+

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_