[WIP] add Relative Orbit Propagator with Yamanaka-Ankersen's 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/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

@@ -135,9 +135,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

src/math_physics/orbit/relative_orbit_models.hpp

@@ -100,17 +100,6 @@ math::Matrix<6, 6> CalcSabatiniStm(double orbit_radius_m, double gravity_constan
  */
 math::Matrix<6, 6> CalcCarterStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe);
 
-/**
- * @fn CalcYamakawaAnkersenStm
- * @brief Calculate Yamakawa-Ankersen 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] f_ref_rad: True anomaly of the reference satellite [rad]
- * @param [in] reference_oe: Orbital elements of reference satellite
- * @return State Transition Matrix
- */
-math::Matrix<6, 6> CalcYamakawaAnkersenStm(double orbit_radius_m, double gravity_constant_m3_s2, double f_ref_rad, OrbitalElements* reference_oe);
-
 }  // namespace orbit
 
 #endif  // S2E_LIBRARY_ORBIT_RELATIVE_ORBIT_MODEL_HPP_