Add Kepler and Encke orbit propagation method

CMakeLists.txt

@@ -79,6 +79,7 @@ add_subdirectory(src/Library/sgp4)
 add_subdirectory(src/Library/utils)
 add_subdirectory(src/Library/optics)
 add_subdirectory(src/Library/RelativeOrbit)
+add_subdirectory(src/Library/Orbit)
 
 set(SOURCE_FILES
   src/S2E.cpp
@@ -136,7 +137,7 @@ endif()
 
 ## Linking libraries
 set(S2E_LIBRARIES
-  IGRF WRAPPER_NRLMSISE00 INIH MATH SGP4 UTIL OPTICS RELATIVE_ORBIT_MODELS
+  IGRF WRAPPER_NRLMSISE00 INIH SGP4 UTIL OPTICS RELATIVE_ORBIT_MODELS ORBIT_MODELS MATH
 )
 # Initialize link
 target_link_libraries(COMPONENT INTERFACE INI_IN DYNAMICS GLOBAL_ENVIRONMENT LOCAL_ENVIRONMENT SC_IO RELATIVE_INFO ${S2E_LIBRARIES})

data/SampleSat/ini/SampleSat.ini

@@ -51,17 +51,21 @@ pointing_sub_t_b(2) = 1.0
 calculation = ENABLE
 logging = ENABLE
 
+// World Geodetic System
+wgs = 2 // 0: wgs72old, 1: wgs72, 2: wgs84
+
 // Orbit propagation mode
-// 0: RK4 with initial position and velocity
-// 1: SGP4 with TLE
-// 2: Relative dynamics (for formation flying simulation)
-propagate_mode = 1
+// 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 = RK4
 
 // TLE definition for SGP4 ///////////////////////////////////////////////
 // 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
-wgs = 2 // 0: wgs72old, 1: wgs72, 2: wgs84
 //////////////////////////////////////////////////////////////////////////
 
 // Initial value definition for RK4 //////////////////////////////////////
@@ -101,6 +105,27 @@ reference_sat_id = 1
 ///////////////////////////////////////////////////////////////////////////////
 
 
+// Information used for orbital propagation by the Kepler Motion ///////////
+// initialize mode for kepler motion
+// INIT_POSVEL : initialize with position and velocity defined for RK4
+// INIT_OE : initialize with the following orbital elements
+init_mode_kepler = INIT_POSVEL
+// Orbital Elements for INIT_OE
+semi_major_axis_m = 6794500.0
+eccentricity = 0.0015
+inclination_rad = 0.9012
+raan_rad = 0.1411
+arg_perigee_rad = 1.7952
+epoch_jday = 2.458940966402607e6
+///////////////////////////////////////////////////////////////////////////////
+
+
+// Information used for orbital propagation by the Encke Formulation ///////////
+error_tolerance = 0.0001
+// initialize position and vector are same with RK4 setting
+///////////////////////////////////////////////////////////////////////////////
+
+
 [Thermal]
 IsCalcEnabled=0
 debug=0

src/Dynamics/CMakeLists.txt

@@ -5,6 +5,8 @@ add_library(${PROJECT_NAME} STATIC
   Orbit/EarthCenteredOrbit.cpp
   Orbit/SimpleCircularOrbit.cpp
   Orbit/RelativeOrbit.cpp
+  Orbit/KeplerOrbitPropagation.cpp
+  Orbit/EnckeOrbitPropagation.cpp
   Thermal/Node.cpp
   Thermal/Temperature.cpp
   Attitude/AttitudeRK4.cpp

src/Dynamics/Orbit/EnckeOrbitPropagation.cpp

@@ -0,0 +1,144 @@
+#include "EnckeOrbitPropagation.h"
+
+#include "../../Library/Orbit/OrbitalElements.h"
+
+EnckeOrbitPropagation::EnckeOrbitPropagation(const double mu_m3_s2, const double prop_step_s, const double current_jd,
+                                             const Vector<3> init_position_i_m, const Vector<3> init_velocity_i_m_s, const double error_tolerance,
+                                             const int wgs)
+    : mu_m3_s2_(mu_m3_s2), error_tolerance_(error_tolerance), prop_step_s_(prop_step_s), libra::ODE<6>(prop_step_s) {
+  // TODO whichconst周りを整理する
+  if (wgs == 0) {
+    whichconst = wgs72old;
+  } else if (wgs == 1) {
+    whichconst = wgs72;
+  } else if (wgs == 2) {
+    whichconst = wgs84;
+  }
+
+  prop_time_s_ = 0.0;
+  Initialize(current_jd, init_position_i_m, init_velocity_i_m_s);
+}
+
+EnckeOrbitPropagation::~EnckeOrbitPropagation() {}
+
+// Functions for Orbit
+void EnckeOrbitPropagation::Propagate(double endtime, double current_jd) {
+  if (!IsCalcEnabled) return;
+
+  // Rectification
+  double norm_sat_position_m = norm(sat_position_i_);
+  double norm_diff_position_m = norm(diff_position_i_m_);
+  if (norm_diff_position_m / norm_sat_position_m > error_tolerance_) {
+    Initialize(current_jd, sat_position_i_, sat_velocity_i_);
+  }
+
+  // Update reference orbit
+  ref_kepler_orbit.CalcPosVel(current_jd);
+  ref_position_i_m_ = ref_kepler_orbit.GetPosition_i_m();
+  ref_velocity_i_m_s_ = ref_kepler_orbit.GetVelocity_i_m_s();
+
+  // Propagate difference orbit
+  setStepWidth(prop_step_s_);  // Re-set propagation Δt
+  while (endtime - prop_time_s_ - prop_step_s_ > 1.0e-6) {
+    Update();  // Propagation methods of the ODE class
+    prop_time_s_ += prop_step_s_;
+  }
+  setStepWidth(endtime - prop_time_s_);  // Adjust the last propagation Δt
+  Update();
+  prop_time_s_ = endtime;
+
+  acc_i_ *= 0.0;  // Reset disturbance acceleration
+  diff_position_i_m_[0] = state()[0];
+  diff_position_i_m_[1] = state()[1];
+  diff_position_i_m_[2] = state()[2];
+  diff_velocity_i_m_s_[0] = state()[3];
+  diff_velocity_i_m_s_[1] = state()[4];
+  diff_velocity_i_m_s_[2] = state()[5];
+
+  UpdateSatOrbit(current_jd);
+}
+
+std::string EnckeOrbitPropagation::GetLogHeader() const {
+  std::string str_tmp = "";
+
+  str_tmp += WriteVector("sat_position", "i", "m", 3);
+  str_tmp += WriteVector("sat_velocity", "i", "m/s", 3);
+  str_tmp += WriteVector("sat_acc", "i", "m/s^2", 3);
+
+  return str_tmp;
+}
+
+std::string EnckeOrbitPropagation::GetLogValue() const {
+  std::string str_tmp = "";
+
+  str_tmp += WriteVector(sat_position_i_, 16);
+  str_tmp += WriteVector(sat_velocity_i_, 10);
+  str_tmp += WriteVector(acc_i_, 10);
+
+  return str_tmp;
+}
+
+// Functions for ODE
+void EnckeOrbitPropagation::RHS(double t, const Vector<6>& state, Vector<6>& rhs) {
+  Vector<3> diff_pos_i_m, diff_acc_i_m_s2;
+  for (int i = 0; i < 3; i++) {
+    diff_pos_i_m[i] = state[i];
+  }
+
+  double q_func = CalcQFunction(diff_pos_i_m);
+  double r_m = norm(ref_position_i_m_);
+  double r_m3 = pow(r_m, 3.0);
+
+  diff_acc_i_m_s2 = -(mu_m3_s2_ / r_m3) * (q_func * sat_position_i_ + diff_pos_i_m) + acc_i_;
+
+  rhs[0] = state[3];
+  rhs[1] = state[4];
+  rhs[2] = state[5];
+  rhs[3] = diff_acc_i_m_s2[0];
+  rhs[4] = diff_acc_i_m_s2[1];
+  rhs[5] = diff_acc_i_m_s2[2];
+}
+
+// Private Functions
+void EnckeOrbitPropagation::Initialize(double current_jd, Vector<3> init_ref_position_i_m, Vector<3> init_ref_velocity_i_m_s) {
+  // General
+  fill_up(acc_i_, 0.0);
+
+  // reference orbit
+  ref_position_i_m_ = init_ref_position_i_m;
+  ref_velocity_i_m_s_ = init_ref_velocity_i_m_s;
+  OrbitalElements oe_ref(mu_m3_s2_, current_jd, init_ref_position_i_m, init_ref_velocity_i_m_s);
+  ref_kepler_orbit = KeplerOrbit(mu_m3_s2_, current_jd, oe_ref);
+
+  // difference orbit
+  fill_up(diff_position_i_m_, 0.0);
+  fill_up(diff_velocity_i_m_s_, 0.0);
+
+  Vector<6> zero(0.0f);
+  setup(0.0, zero);
+
+  UpdateSatOrbit(current_jd);
+}
+
+void EnckeOrbitPropagation::UpdateSatOrbit(double current_jd) {
+  sat_position_i_ = ref_position_i_m_ + diff_position_i_m_;
+  sat_velocity_i_ = ref_velocity_i_m_s_ + diff_velocity_i_m_s_;
+
+  // ECI->ECEF
+  TransECIToGeo(current_jd);
+  TransECIToECEF(current_jd);
+}
+
+double EnckeOrbitPropagation::CalcQFunction(Vector<3> diff_pos_i) {
+  double r2;
+  r2 = inner_product(sat_position_i_, sat_position_i_);
+
+  Vector<3> dr_2r;
+  dr_2r = diff_pos_i - 2.0 * sat_position_i_;
+
+  double q = inner_product(diff_pos_i, dr_2r) / r2;
+
+  double q_func = q * (q * q + 3.0 * q + 3.0) / (pow(1.0 + q, 1.5) + 1.0);
+
+  return q_func;
+}

src/Dynamics/Orbit/EnckeOrbitPropagation.h

@@ -0,0 +1,40 @@
+#pragma once
+#include "../../Library/Orbit/KeplerOrbit.h"
+#include "../../Library/math/ODE.hpp"
+#include "Orbit.h"
+
+class EnckeOrbitPropagation : public Orbit, public libra::ODE<6> {
+ public:
+  EnckeOrbitPropagation(const double mu_m3_s2, const double prop_step_s, const double current_jd, const Vector<3> init_position_i_m,
+                        const Vector<3> init_velocity_i_m_s, const double error_tolerance, const int wgs);
+  ~EnckeOrbitPropagation();
+
+  // Orbit class
+  virtual void Propagate(double endtime, double current_jd);
+  virtual std::string GetLogHeader() const;
+  virtual std::string GetLogValue() const;
+
+  // ODE class
+  virtual void RHS(double t, const Vector<6>& state, Vector<6>& rhs);
+
+ private:
+  // General
+  const double mu_m3_s2_;
+  const double error_tolerance_;  // error ratio
+  double prop_step_s_;            // Δt for RK4
+  double prop_time_s_;            // Simulation current time for numerical integration by RK4
+
+  // reference orbit
+  Vector<3> ref_position_i_m_;
+  Vector<3> ref_velocity_i_m_s_;
+  KeplerOrbit ref_kepler_orbit;
+
+  // difference orbit
+  Vector<3> diff_position_i_m_;
+  Vector<3> diff_velocity_i_m_s_;
+
+  // functions
+  void Initialize(double current_jd, Vector<3> init_ref_position_i_m, Vector<3> init_ref_velocity_i_m_s);
+  void UpdateSatOrbit(double current_jd);
+  double CalcQFunction(Vector<3> diff_pos_i);
+};

src/Dynamics/Orbit/KeplerOrbitPropagation.cpp

@@ -0,0 +1,61 @@
+#include "KeplerOrbitPropagation.h"
+
+#include "../../Library/math/s2e_math.hpp"
+
+KeplerOrbitPropagation::KeplerOrbitPropagation(const double current_jd, KeplerOrbit kepler_orbit, const int wgs) : KeplerOrbit(kepler_orbit) {
+  // TODO whichconst周りを整理する
+  if (wgs == 0) {
+    whichconst = wgs72old;
+  } else if (wgs == 1) {
+    whichconst = wgs72;
+  } else if (wgs == 2) {
+    whichconst = wgs84;
+  }
+
+  UpdateState(current_jd);
+}
+
+KeplerOrbitPropagation::~KeplerOrbitPropagation() {}
+
+void KeplerOrbitPropagation::Propagate(double endtime, double current_jd) {
+  if (!IsCalcEnabled) return;
+
+  UpdateState(current_jd);
+}
+
+std::string KeplerOrbitPropagation::GetLogHeader() const {
+  std::string str_tmp = "";
+
+  str_tmp += WriteVector("sat_position", "i", "m", 3);
+  str_tmp += WriteVector("sat_velocity", "i", "m/s", 3);
+  str_tmp += WriteVector("sat_velocity", "b", "m/s", 3);
+  str_tmp += WriteVector("sat_acc_i", "i", "m/s^2", 3);
+  str_tmp += WriteScalar("lat", "rad");
+  str_tmp += WriteScalar("lon", "rad");
+  str_tmp += WriteScalar("alt", "m");
+
+  return str_tmp;
+}
+
+std::string KeplerOrbitPropagation::GetLogValue() const {
+  std::string str_tmp = "";
+
+  str_tmp += WriteVector(sat_position_i_, 16);
+  str_tmp += WriteVector(sat_velocity_i_, 10);
+  str_tmp += WriteVector(sat_velocity_b_);
+  str_tmp += WriteVector(acc_i_, 10);
+  str_tmp += WriteScalar(lat_rad_);
+  str_tmp += WriteScalar(lon_rad_);
+  str_tmp += WriteScalar(alt_m_);
+
+  return str_tmp;
+}
+
+// Private Function
+void KeplerOrbitPropagation::UpdateState(const double current_jd) {
+  CalcPosVel(current_jd);
+  sat_position_i_ = position_i_m_;
+  sat_velocity_i_ = velocity_i_m_s_;
+  TransECIToGeo(current_jd);
+  TransECIToECEF(current_jd);
+}

src/Dynamics/Orbit/KeplerOrbitPropagation.h

@@ -0,0 +1,18 @@
+#pragma once
+#include "../../Library/Orbit/KeplerOrbit.h"
+#include "Orbit.h"
+
+class KeplerOrbitPropagation : public Orbit, public KeplerOrbit {
+ public:
+  // Initialize with orbital elements
+  KeplerOrbitPropagation(const double current_jd, KeplerOrbit kepler_orbit, const int wgs);
+  ~KeplerOrbitPropagation();
+
+  // Orbit class
+  virtual void Propagate(double endtime, double current_jd);
+  virtual std::string GetLogHeader() const;
+  virtual std::string GetLogValue() const;
+
+ private:
+  void UpdateState(const double current_jd);
+};

src/Dynamics/Orbit/Orbit.h

@@ -12,6 +12,7 @@ using libra::Quaternion;
 using libra::Vector;
 
 #include <Interface/LogOutput/ILoggable.h>
+#include <math.h>
 
 #include <Environment/Global/PhysicalConstants.hpp>
 
@@ -22,9 +23,9 @@ using libra::Vector;
 
 class Orbit : public ILoggable {
  public:
-  virtual ~Orbit() {}
+  enum class PROPAGATE_MODE { RK4 = 0, SGP4, RELATIVE_ORBIT, KEPLER, ENCKE };
 
-  enum class PROPAGATE_MODE { RK4 = 0, SGP4, RELATIVE_ORBIT };
+  virtual ~Orbit() {}
 
   inline PROPAGATE_MODE GetPropagateMode() const { return propagate_mode_; }
   inline Vector<3> GetSatPosition_i() const { return sat_position_i_; }