[WIP] Feature/ss stm

scripts/Plot/plot_relative_position_rtn.py

@@ -0,0 +1,83 @@
+#
+# Plot Satellite Relative Position on RTN frame
+#
+# arg[1] : read_file_tag : time tag for default CSV output log file. ex. 220627_142946
+#
+
+#
+# Import
+#
+# plots
+import numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+# local function
+from common import find_latest_log_tag
+from common import read_3d_vector_from_csv
+# csv read
+import pandas
+# arguments
+import argparse
+
+#
+# Read Arguments
+#
+aparser = argparse.ArgumentParser()
+
+aparser.add_argument('--logs-dir', type=str, help='logs directory like "../../logs"', default='../../logs')
+aparser.add_argument('--file-tag', type=str, help='log file tag like 220627_142946')
+aparser.add_argument('--no-gui', action='store_true')
+
+args = aparser.parse_args()
+
+# log file path
+path_to_logs = args.logs_dir
+
+read_file_tag = args.file_tag
+if read_file_tag == None:
+  print("file tag does not found. use latest the latest log file.")
+  read_file_tag = find_latest_log_tag(path_to_logs)
+
+print("log: " + read_file_tag)
+
+#
+# CSV file name
+#
+read_file_name  = path_to_logs + '/' + 'logs_' + read_file_tag + '/' + read_file_tag + '_default.csv'
+
+#
+# Data read and edit
+#
+# Read S2E CSV
+d1 = read_3d_vector_from_csv(read_file_name, 'satellite1_position_from_satellite0_rtn', 'm')
+# Add satellites if you need
+# d2 = read_3d_vector_from_csv(read_file_name, 'satellite2_position_from_satellite0_rtn', 'm')
+
+# Edit data if you need
+
+#
+# Plot
+#
+fig = plt.figure(figsize=(5,5))
+ax = fig.add_subplot(111, projection='3d')
+ax.set_title("Relative Position of Satellites in RTN frame")
+ax.set_xlabel("Radial [m]")
+ax.set_ylabel("Transverse [m]")
+ax.set_zlabel("Normal [m]")
+
+# Add plot settings if you need
+#ax.set_xlim(-100, 100)
+#ax.set_ylim(-100, 100)
+#ax.set_zlim(-100, 100)
+
+ax.plot(0,0,0, marker="*", c="green", markersize=10, label="Sat0")
+ax.plot(d1[0],d1[1],d1[2], marker="x", linestyle='None', c="red", label="Sat1")
+# Add satellites if you need
+# ax.plot(d2[0],d2[1],d2[2], marker="o", linestyle='None', c="blue", label="Sat2")
+
+ax.legend()
+
+if args.no_gui:
+  plt.savefig(read_file_tag + "_relative_position_rtn.png")
+else:
+  plt.show()

settings/sample_satellite/satellite.ini

@@ -79,12 +79,12 @@ initial_velocity_i_m_s(2) = -4429.2361258448807
 ///////////////////////////////////////////////////////////////////////////
 
 // Initial value definition for ORBITAL_ELEMENTS initialize mode ////////
-semi_major_axis_m = 6794500.0
-eccentricity = 0.0015
-inclination_rad = 0.9012
-raan_rad = 0.1411
-argument_of_perigee_rad = 1.7952
-epoch_jday = 2.458940966402607e6
+semi_major_axis_m = 6928000.0
+eccentricity = 0.0
+inclination_rad = 1.57079633
+raan_rad = 1.57079633
+argument_of_perigee_rad = 0.0
+epoch_jday = 2458850.0
 ///////////////////////////////////////////////////////////////////////////////
 
 

settings/sample_satellite/satellite_sub.ini

@@ -79,15 +79,14 @@ initial_velocity_i_m_s(2) = -4429.2361258448807
 ///////////////////////////////////////////////////////////////////////////
 
 // Initial value definition for ORBITAL_ELEMENTS initialize mode ////////
-semi_major_axis_m = 6794500.0
-eccentricity = 0.0015
-inclination_rad = 0.9012
-raan_rad = 0.1411
-argument_of_perigee_rad = 1.7952
-epoch_jday = 2.458940966402607e6
+semi_major_axis_m = 6928000.0
+eccentricity = 0.0
+inclination_rad = 1.57079633
+raan_rad = 1.57079633
+argument_of_perigee_rad = 0.0
+epoch_jday = 2458849.9999999694823
 ///////////////////////////////////////////////////////////////////////////////
 
-
 // Settings for SGP4 ///////////////////////////////////////////////
 // TLE
 // Example: ISS
@@ -104,8 +103,8 @@ relative_orbit_update_method = 1
 // 0: Hill
 relative_dynamics_model_type = 0
 // STM Relative Dynamics model type (only valid for STM update)
-// 0: HCW
-stm_model_type = 0
+// 0: HCW, 1: Melton, 2: SS, 3: Sabatini, 4: Carter, 5: Yamanaka-Ankersen
+stm_model_type = 5
 // 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

settings/sample_simulation_base.ini

@@ -3,7 +3,7 @@
 simulation_start_time_utc = 2020/04/01 12:00:00.0
 
 // Simulation duration [sec]
-simulation_duration_s = 200
+simulation_duration_s = 5000
 
 // Simulation step time [sec]
 // Minimum time step for the entire simulation

src/dynamics/orbit/relative_orbit.cpp

@@ -58,6 +58,8 @@ void RelativeOrbit::InitializeState(math::Vector<3> relative_position_lvlh_m, ma
                           gravity_constant_m3_s2);
   } else  // update_method_ == STM
   {
+    InitializeStmMatrix(stm_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()),
+                        gravity_constant_m3_s2, 0.0);
     CalculateStm(stm_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), gravity_constant_m3_s2,
                  0.0);
   }
@@ -80,6 +82,32 @@ void RelativeOrbit::CalculateSystemMatrix(orbit::RelativeOrbitModel relative_dyn
   }
 }
 
+void RelativeOrbit::InitializeStmMatrix(orbit::StmModel stm_model_type, const Orbit* reference_sat_orbit, double gravity_constant_m3_s2,
+                                        double elapsed_sec) {
+  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();
+  double reference_sat_orbit_radius = position_i_m.CalcNorm();
+  // Temporary codes for the integration by true anomaly
+  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;
+
+  switch (stm_model_type) {
+    case orbit::StmModel::kYamakawaAnkersen:
+      relative_orbit_yamanaka_ankersen_.CalculateInitialInverseMatrix(f_ref_rad, &reference_oe);
+      break;
+
+    default:
+      break;
+  }
+}
+
 void RelativeOrbit::CalculateStm(orbit::StmModel stm_model_type, const Orbit* reference_sat_orbit, double gravity_constant_m3_s2,
                                  double elapsed_sec) {
   orbit::OrbitalElements reference_oe =
@@ -119,7 +147,7 @@ void RelativeOrbit::CalculateStm(orbit::StmModel stm_model_type, const Orbit* re
       break;
     }
     case orbit::StmModel::kYamakawaAnkersen: {
-      stm_ = orbit::CalcYamakawaAnkersenStm(reference_sat_orbit_radius, gravity_constant_m3_s2, f_ref_rad, &reference_oe);
+      stm_ = relative_orbit_yamanaka_ankersen_.CalculateSTM(gravity_constant_m3_s2, elapsed_sec, f_ref_rad, &reference_oe);
       break;
     }
     default: {

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_yamanaka_ankersen.hpp>
 #include <simulation/multiple_spacecraft/relative_information.hpp>
 #include <string>
 
@@ -81,10 +82,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::RelativeOrbitYamanakaAnkersen relative_orbit_yamanaka_ankersen_;  //!< Relative Orbit Calcilater with Yamanaka-Ankersen's STM
 
   /**
    * @fn InitializeState
@@ -104,6 +106,15 @@ class RelativeOrbit : public Orbit, public math::OrdinaryDifferentialEquation<6>
    * @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);
+  /**
+   * @fn InitializeStmMatrix
+   * @brief Calculate State Transition Matrix
+   * @param [in] stm_model_type: STM model type
+   * @param [in] reference_sat_orbit: Orbit information of reference satellite
+   * @param [in] gravity_constant_m3_s2: Gravity constant of the center body [m3/s2]
+   * @param [in] elapsed_sec: Elapsed time [sec]
+   */
+  void InitializeStmMatrix(orbit::StmModel stm_model_type, const Orbit* reference_sat_orbit, double gravity_constant_m3_s2, double elapsed_sec);
   /**
    * @fn CalculateStm
    * @brief Calculate State Transition Matrix

src/environment/global/physical_constants.hpp

@@ -24,6 +24,7 @@ DEFINE_PHYSICAL_CONSTANT(boltzmann_constant_J_K, 1.380649e-23L)              //!
 DEFINE_PHYSICAL_CONSTANT(stefan_boltzmann_constant_W_m2K4, 5.670374419e-8L)  //!< Stefan-Boltzmann constant [W/m2K4]
 DEFINE_PHYSICAL_CONSTANT(absolute_zero_degC, -273.15L)                       //!< Absolute zero [degC]
 DEFINE_PHYSICAL_CONSTANT(solar_constant_W_m2, 1.366e3L)                      //!< Solar constant [W/m2]
+DEFINE_PHYSICAL_CONSTANT(J2_earth, 1.08262668e-3L)                           //!< J2 value of the Earth
 }  // namespace physics
 
 inline namespace astronomy {

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_yamanaka_ankersen.cpp
   orbit/interpolation_orbit.cpp
   orbit/sgp4/sgp4ext.cpp
   orbit/sgp4/sgp4io.cpp

src/math_physics/orbit/relative_orbit_models.cpp

@@ -113,13 +113,119 @@ math::Matrix<6, 6> CalcMeltonStm(double orbit_radius_m, double gravity_constant_
 
 math::Matrix<6, 6> CalcSsStm(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 / orbit_radius_m);
+  double t = elapsed_time_s;
+  double s = 3 * environment::J2_earth * pow(environment::earth_equatorial_radius_m, 2.0) * (1 + 3 * cos(2 * reference_oe->GetInclination_rad())) /
+             (8 * pow(orbit_radius_m, 2.0));
+  if (s > 1) {
+    double c = sqrt(1 + s);
+    double xs = pow(sinh(n * t * sqrt(pow(c, 2.0) - 2) / 2), 2.0);
+    double vxs = sinh(n * t * sqrt(pow(c, 2.0) - 2));
+    double ys1 = n * (pow(c, 2.0) - 2);
+    double ys2 = 5 * pow(c, 2.0) * n;
+    double ys3 = sinh(n * t * sqrt(pow(c, 2.0) - 2));
+    double ys4 = sqrt(pow(c, 2.0) - 2);
+    double vys = tanh(n * t * sqrt(pow(c, 2.0) - 2) / 2);
+
+    stm[0][0] = 1 + (10 * pow(c, 2.0) - 4) / (pow(c, 2.0) - 2) * xs;
+    stm[0][1] = 0.0;
+    stm[0][2] = 0.0;
+    stm[0][3] = sinh(n * t * sqrt(pow(c, 2.0) - 2)) / (n * sqrt(pow(c, 2.0) - 2));
+    stm[0][4] = 4 * c * xs / ((pow(c, 2.0) - 2) * n);
+    stm[0][5] = 0.0;
+    stm[1][0] = (2 * c * ys3 / (ys1 * ys4) - 2 * c * t / (pow(c, 2.0) - 2)) * (2 * n - ys2);
+    stm[1][1] = 1.0;
+    stm[1][2] = 0.0;
+    stm[1][3] = -4 * c * pow(sinh(n * t * ys4 / 2), 2.0) / ys1;
+    stm[1][4] = -(4 * pow(c, 2.0) * ys3 / (n * pow(pow(c, 2.0) - 2, 1.5)) + t * (2 * n - ys2) / ys1);
+    stm[1][5] = 0.0;
+    stm[2][0] = 0.0;
+    stm[2][1] = 0.0;
+    stm[2][2] = cos(c * n * t);
+    stm[2][3] = 0.0;
+    stm[2][4] = 0.0;
+    stm[2][5] = sin(c * n * t) / (c * n);
+    stm[3][0] = (5 * pow(c, 2.0) - 2) * n * vxs / sqrt(pow(c, 2.0) - 2);
+    stm[3][1] = 0.0;
+    stm[3][2] = 0.0;
+    stm[3][3] = cosh(n * t * sqrt(pow(c, 2.0) - 2));
+    stm[3][4] = 2 * c * vxs / sqrt(pow(c, 2.0) - 2);
+    stm[3][5] = 0.0;
+    stm[4][0] = 2 * c * pow(n, 2.0) * vys * (10 * pow(c, 2.0) - 4) / ((pow(vys, 2.0) - 1) * sqrt(pow(c, 2.0) - 2));
+    stm[4][1] = 0.0;
+    stm[4][2] = 0.0;
+    stm[4][3] = 4 * c * n / (pow(vys, 2.0) - 1) + 2 * c * n;
+    stm[4][4] = 8 * pow(c, 2.0) * n * vys / ((pow(vys, 2.0) - 1) * sqrt(pow(c, 2.0) - 2));
+    stm[4][5] = 0.0;
+    stm[5][0] = 0.0;
+    stm[5][1] = 0.0;
+    stm[5][2] = -c * n * sin(c * n * t);
+    stm[5][3] = 0.0;
+    stm[5][4] = 0.0;
+    stm[5][5] = cos(c * n * t);
+  } else if (s < 1) {
+    double c = sqrt(1 + s);
+    double xs = -pow(sin(n * t * sqrt(2 - pow(c, 2.0)) / 2), 2.0);
+    double vxs = sin(n * t * sqrt(2 - pow(c, 2.0)));
+    double ys1 = n * (pow(c, 2.0) - 2);
+    double ys2 = 5 * pow(c, 2.0) * n;
+    double ys3 = sin(n * t * sqrt(2 - pow(c, 2.0)));
+    double ys4 = sqrt(2 - pow(c, 2.0));
+    double vys = tan(n * t * sqrt(2 - pow(c, 2.0)) / 2);
+
+    stm[0][0] = 1 + (10 * pow(c, 2.0) - 4) / (pow(c, 2.0) - 2) * xs;
+    stm[0][1] = 0.0;
+    stm[0][2] = 0.0;
+    stm[0][3] = sin(n * t * sqrt(2 - pow(c, 2.0))) / (n * sqrt(2 - pow(c, 2.0)));
+    stm[0][4] = 4 * c * xs / ((pow(c, 2.0) - 2) * n);
+    stm[0][5] = 0.0;
+    stm[1][0] = (2 * c * ys3 / (ys1 * ys4) - 2 * c * t / (pow(c, 2.0) - 2)) * (2 * n - ys2);
+    stm[1][1] = 1.0;
+    stm[1][2] = 0.0;
+    stm[1][3] = 4 * c * pow(sin(n * t * ys4 / 2), 2.0) / ys1;
+    stm[1][4] = -(-4 * pow(c, 2.0) * ys3 / (n * pow(2-pow(c, 2.0), 1.5)) + t * (2 * n - ys2) / ys1);
+    stm[1][5] = 0.0;
+    stm[2][0] = 0.0;
+    stm[2][1] = 0.0;
+    stm[2][2] = cos(c * n * t);
+    stm[2][3] = 0.0;
+    stm[2][4] = 0.0;
+    stm[2][5] = sin(c * n * t) / (c * n);
+    stm[3][0] = (5 * pow(c, 2.0) - 2) * n * vxs / sqrt(2 - pow(c, 2.0));
+    stm[3][1] = 0.0;
+    stm[3][2] = 0.0;
+    stm[3][3] = cos(n * t * sqrt(2 - pow(c, 2.0)));
+    stm[3][4] = 2 * c * vxs / sqrt(2 - pow(c, 2.0));
+    stm[3][5] = 0.0;
+    stm[4][0] = 2 * c * pow(n, 2.0) * vys * (10 * pow(c, 2.0) - 4) / ((pow(vys, 2.0) - 1) * sqrt(2 - pow(c, 2.0)));
+    stm[4][1] = 0.0;
+    stm[4][2] = 0.0;
+    stm[4][3] = 4 * c * n / (pow(vys, 2.0) - 1) + 2 * c * n;
+    stm[4][4] = 8 * pow(c, 2.0) * n * vys / ((pow(vys, 2.0) - 1) * sqrt(2 - pow(c, 2.0)));
+    stm[4][5] = 0.0;
+    stm[5][0] = 0.0;
+    stm[5][1] = 0.0;
+    stm[5][2] = -c * n * sin(c * n * t);
+    stm[5][3] = 0.0;
+    stm[5][4] = 0.0;
+    stm[5][5] = cos(c * n * t);
+  } else {
+    std::cout << "[Error] SsSTM: initial condition for \"s\" is not appropiate; \"s\" should not be 1." << std::endl;
+  }
+
   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を計算してください
+
+  correction_term[0] = 0.0;
+  correction_term[1] = 0.0;
+  correction_term[2] = 0.0;
+  correction_term[3] = 0.0;
+  correction_term[4] = 0.0;
+  correction_term[5] = 0.0;
   return correction_term;
 }
 
@@ -135,9 +241,4 @@ math::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_
   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