[WIP] Feature/add ralative position 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

@@ -60,13 +60,13 @@ logging = ENABLE
 // RELATIVE : Relative dynamics (for formation flying simulation)
 // KEPLER   : Kepler orbit propagation without disturbances and thruster maneuver
 // ENCKE    : Encke orbit propagation with disturbances and thruster maneuver
-propagate_mode = RK4
+propagate_mode = KEPLER
 
 // Orbit initialize mode for RK4, KEPLER, and ENCKE
 // DEFAULT             : Use default initialize method (RK4 and ENCKE use pos/vel, KEPLER uses init_mode_kepler)
 // POSITION_VELOCITY_I : Initialize with position and velocity in the inertial frame
 // ORBITAL_ELEMENTS    : Initialize with orbital elements
-initialize_mode = POSITION_VELOCITY_I
+initialize_mode = ORBITAL_ELEMENTS
 
 // Initial value definition for POSITION_VELOCITY_I initialize mode ////////
 initial_position_i_m(0) = -2111769.7723711144
@@ -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

@@ -0,0 +1,153 @@
+[ATTITUDE]
+// Attitude propagation mode
+// RK4 : Attitude Propagation with RK4 including disturbances and control torque
+// CANTILEVER_VIBRATION : Attitude Propagation with the consideration of the cantilever vibration (flexible structure) including disturbances and control torque.
+// CONTROLLED : Attitude Calculation with Controlled Attitude mode. All disturbances and control torque are ignored.
+propagate_mode = RK4
+
+// Initialize Attitude mode
+// MANUAL : Initialize Quaternion_i2b manually below 
+// CONTROLLED : Initialize attitude with given condition. Valid except when Attitude propagation mode is CONTROLLED.
+initialize_mode = MANUAL
+
+// Initial angular velocity at body frame [rad/s]
+initial_angular_velocity_b_rad_s(0) = 0.0
+initial_angular_velocity_b_rad_s(1) = 0.0
+initial_angular_velocity_b_rad_s(2) = 0.0
+
+// Initial quaternion inertial frame to body frame (real part, imaginary part)
+// This value also used in INERTIAL_STABILIZE mode of ControlledAttitude
+initial_quaternion_i2b(0) = 0.0
+initial_quaternion_i2b(1) = 0.0
+initial_quaternion_i2b(2) = 0.0
+initial_quaternion_i2b(3) = 1.0
+
+// Initial torque at body frame [Nm]
+// Note: The initial torque added just for the first propagation step
+initial_torque_b_Nm(0) = +0.000
+initial_torque_b_Nm(1) = -0.000
+initial_torque_b_Nm(2) =  0.000
+
+[CONTROLLED_ATTITUDE]
+// Mode definitions
+// INERTIAL_STABILIZE
+// SUN_POINTING
+// EARTH_CENTER_POINTING
+// VELOCITY_DIRECTION_POINTING
+// GROUND_SPEED_DIRECTION_POINTING
+// ORBIT_NORMAL_POINTING
+main_mode = INERTIAL_STABILIZE
+sub_mode = SUN_POINTING
+
+// Pointing direction @ body frame for main pointing mode
+main_pointing_direction_b(0) = 1.0
+main_pointing_direction_b(1) = 0.0
+main_pointing_direction_b(2) = 0.0
+
+// Pointing direction @ body frame for sub pointing mode
+// main_pointing_direction_b and sub_pointing_direction_b should separate larger than 30 degrees.
+sub_pointing_direction_b(0) = 0.0
+sub_pointing_direction_b(1) = 0.0
+sub_pointing_direction_b(2) = 1.0
+
+[ORBIT]
+calculation = ENABLE
+logging = ENABLE
+
+// Orbit propagation mode
+// RK4      : RK4 propagation with disturbances and thruster maneuver
+// SGP4     : SGP4 propagation using TLE without thruster maneuver
+// RELATIVE : Relative dynamics (for formation flying simulation)
+// KEPLER   : Kepler orbit propagation without disturbances and thruster maneuver
+// ENCKE    : Encke orbit propagation with disturbances and thruster maneuver
+propagate_mode = RELATIVE
+
+// Orbit initialize mode for RK4, KEPLER, and ENCKE
+// DEFAULT             : Use default initialize method (RK4 and ENCKE use pos/vel, KEPLER uses init_mode_kepler)
+// POSITION_VELOCITY_I : Initialize with position and velocity in the inertial frame
+// ORBITAL_ELEMENTS    : Initialize with orbital elements
+initialize_mode = DEFAULT
+
+// Initial value definition for POSITION_VELOCITY_I initialize mode ////////
+initial_position_i_m(0) = -2111769.7723711144
+initial_position_i_m(1) = -5360353.2254375768
+initial_position_i_m(2) = 3596181.6497774957
+
+initial_velocity_i_m_s(0) = 4200.4344740455268
+initial_velocity_i_m_s(1) = -4637.540129059361
+initial_velocity_i_m_s(2) = -4429.2361258448807
+///////////////////////////////////////////////////////////////////////////
+
+// Initial value definition for ORBITAL_ELEMENTS initialize mode ////////
+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
+tle1=1 25544U 98067A   20076.51604214  .00016717  00000-0  10270-3 0  9005
+tle2=2 25544  51.6412  86.9962 0006063  30.9353 329.2153 15.49228202 17647
+// World Geodetic System
+wgs_setting = 2 // 0: wgs72old, 1: wgs72, 2: wgs84
+//////////////////////////////////////////////////////////////////////////
+
+// Settings for relative orbit propagation ////////////////////////////
+// Relative Orbit Update Method (0 means RK4, 1 means STM)
+relative_orbit_update_method = 1
+// RK4 Relative Dynamics model type (only valid for RK4 update)
+// 0: Hill
+relative_dynamics_model_type = 0
+// STM Relative Dynamics model type (only valid for STM update)
+// 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
+initial_relative_position_lvlh_m(1) = 100.0
+initial_relative_position_lvlh_m(2) = 0.0
+// initial satellite velocity relative to the reference satellite in LVLH frame[m/s]
+initial_relative_velocity_lvlh_m_s(0) = 0.0
+initial_relative_velocity_lvlh_m_s(1) = 0.0
+initial_relative_velocity_lvlh_m_s(2) = 0.0
+// information of reference satellite
+reference_satellite_id = 0
+///////////////////////////////////////////////////////////////////////////////
+
+// Settings for Encke mode ///////////
+error_tolerance = 0.0001
+///////////////////////////////////////////////////////////////////////////////
+
+
+[THERMAL]
+calculation = DISABLE
+debug = DISABLE
+solar_calc_setting = DISABLE
+thermal_file_directory = SETTINGS_DIR_FROM_EXE/sample_satellite/thermal_csv_files/
+
+[SETTING_FILES]
+local_environment_file = SETTINGS_DIR_FROM_EXE/sample_satellite/local_environment.ini
+disturbance_file  = SETTINGS_DIR_FROM_EXE/sample_satellite/disturbance.ini
+structure_file = SETTINGS_DIR_FROM_EXE/sample_satellite/structure.ini
+
+[COMPONENT_FILES]
+gyro_file = SETTINGS_DIR_FROM_EXE/sample_satellite/components/gyro_sensor.ini
+magnetometer_file = SETTINGS_DIR_FROM_EXE/sample_satellite/components/magnetometer.ini
+stt_file = SETTINGS_DIR_FROM_EXE/sample_satellite/components/star_sensor.ini
+ss_file = SETTINGS_DIR_FROM_EXE/sample_satellite/components/sun_sensor.ini
+gnss_file = SETTINGS_DIR_FROM_EXE/sample_satellite/components/gnss_receiver.ini
+magnetorquer_file = SETTINGS_DIR_FROM_EXE/sample_satellite/components/magnetorquer.ini
+rw_file = SETTINGS_DIR_FROM_EXE/sample_satellite/components/reaction_wheel.ini
+thruster_file = SETTINGS_DIR_FROM_EXE/sample_satellite/components/thruster.ini
+force_generator_file = SETTINGS_DIR_FROM_EXE/sample_satellite/components/force_generator.ini
+torque_generator_file = SETTINGS_DIR_FROM_EXE/sample_satellite/components/torque_generator.ini
+angular_velocity_observer_file = SETTINGS_DIR_FROM_EXE/sample_satellite/components/angular_velocity_observer.ini
+attitude_observer_file = SETTINGS_DIR_FROM_EXE/sample_satellite/components/attitude_observer.ini
+orbit_observer_file = SETTINGS_DIR_FROM_EXE/sample_satellite/components/orbit_observer.ini
+antenna_file = SETTINGS_DIR_FROM_EXE/sample_satellite/components/spacecraft_antenna.ini
+component_interference_file = SETTINGS_DIR_FROM_EXE/sample_satellite/components/component_interference.ini
+telescope_file = SETTINGS_DIR_FROM_EXE/sample_satellite/components/telescope.ini

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
@@ -142,9 +142,10 @@ save_initialize_files = ENABLE
 // File name must not over 1024 characters (defined in initialize_file_access.hpp as kMaxCharLength)
 // If you want to add a spacecraft, create the corresponding spacecraft.ini, and specify it as spacecraft_file(1), spacecraft_file(2), ect.
 // If you want to add a ground station, create the corresponding ground_station.ini, and specify it as ground_station_file(1), ground_station_file(2), ect.
-number_of_simulated_spacecraft = 1
+number_of_simulated_spacecraft = 2
 number_of_simulated_ground_station = 1
 spacecraft_file(0)      = SETTINGS_DIR_FROM_EXE/sample_satellite/satellite.ini
+spacecraft_file(1)      = SETTINGS_DIR_FROM_EXE/sample_satellite/satellite_sub.ini
 ground_station_file(0)  = SETTINGS_DIR_FROM_EXE/sample_ground_station/ground_station.ini
 gnss_file               = SETTINGS_DIR_FROM_EXE/environment/sample_gnss.ini
 log_file_save_directory = ../../logs/

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,12 +82,73 @@ 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 =
+      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::kHcw: {
-      double reference_sat_orbit_radius = reference_sat_orbit->GetPosition_i_m().CalcNorm();
       stm_ = orbit::CalcHcwStm(reference_sat_orbit_radius, gravity_constant_m3_s2, elapsed_sec);
+      break;
+    }
+    case orbit::StmModel::kMelton: {
+      stm_ = orbit::CalcMeltonStm(reference_sat_orbit_radius, gravity_constant_m3_s2, elapsed_sec, &reference_oe);
+      break;
+    }
+    case orbit::StmModel::kSs: {
+      stm_ = orbit::CalcSsStm(reference_sat_orbit_radius, gravity_constant_m3_s2, elapsed_sec, &reference_oe);
+      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;
+    }
+    case orbit::StmModel::kYamakawaAnkersen: {
+      stm_ = relative_orbit_yamanaka_ankersen_.CalculateSTM(gravity_constant_m3_s2, elapsed_sec, f_ref_rad, &reference_oe);
+      break;
     }
     default: {
       // NOT REACHED
@@ -141,7 +204,7 @@ void RelativeOrbit::PropagateStm(double elapsed_sec) {
   math::Vector<6> current_state;
   CalculateStm(stm_model_type_, &(relative_information_->GetReferenceSatDynamics(reference_spacecraft_id_)->GetOrbit()), gravity_constant_m3_s2_,
                elapsed_sec);
-  current_state = stm_ * initial_state_;
+  current_state = stm_ * initial_state_ + correction_term_;
 
   relative_position_lvlh_m_[0] = current_state[0];
   relative_position_lvlh_m_[1] = current_state[1];