First commit adding support for orbital maneuvers changing argument of periapsis

example_satellite_input_files/arg_of_periapsis_calibration_input.json

@@ -0,0 +1,10 @@
+{
+  "Inclination": 5,
+  "RAAN": 0,
+  "Argument of Periapsis": 20,
+  "Eccentricity": 0.1,
+  "Semimajor Axis": 15000,
+  "True Anomaly": 0,
+  "Mass": 100,
+  "Name": "Arg of Periapsis Calibration"
+}

example_satellite_input_files/arg_of_periapsis_test_input.json

@@ -0,0 +1,10 @@
+{
+  "Inclination": 5,
+  "RAAN": 0,
+  "Argument of Periapsis": 5,
+  "Eccentricity": 0.1,
+  "Semimajor Axis": 15000,
+  "True Anomaly": 0,
+  "Mass": 100,
+  "Name": "Arg of Periapsis Test"
+}

include/Satellite.h

@@ -23,6 +23,9 @@ class ThrustProfileLVLH {
  public:
   double t_start_ = {0};
   double t_end_ = {0};
+  bool arg_of_periapsis_change_thrust_profile = false;
+  double sign_of_delta_omega = {1};
+  double thrust_magnitude_ = {0}; // Only used for argument of periapsis change thrust profiles
   std::array<double, 3> LVLH_force_vec_ = {0, 0, 0};
   ThrustProfileLVLH(const double t_start, const double t_end,
                     const std::array<double, 3> LVLH_force_vec) {
@@ -41,7 +44,26 @@ class ThrustProfileLVLH {
           input_force_magnitude * LVLH_normalized_force_direction_vec.at(ind);
     }
   }
-
+  ThrustProfileLVLH(const double t_start, const double final_arg_of_periapsis,
+                  const  double thrust_magnitude, const double initial_arg_of_periapsis,
+                  const double satellite_eccentricity, const double satellite_a,
+                  const double satellite_mass) {
+    // Thrust profile for continuous-thrust argument of periapsis change
+    // Main ref: https://apps.dtic.mil/sti/tr/pdf/ADA384536.pdf
+    // 
+    // Used Eq. 67 in https://link.springer.com/article/10.1007/s10569-021-10033-9#Sec15 to determine burn angles
+    arg_of_periapsis_change_thrust_profile = true;
+    t_start_ = t_start;
+    const double alpha = M_PI/2.0; // continous thrust
+    (final_arg_of_periapsis > initial_arg_of_periapsis) ? sign_of_delta_omega = 1.0 : sign_of_delta_omega = -1.0;
+    thrust_magnitude_ = thrust_magnitude;
+    const double mu_Earth = G*mass_Earth;
+    const double delta_omega_mag = abs(final_arg_of_periapsis - initial_arg_of_periapsis); // = delta_omega / sgn(delta_omega)
+    const double delta_V = (2.0/3.0)*sqrt(mu_Earth/satellite_a)*satellite_eccentricity*delta_omega_mag/sqrt(1-satellite_eccentricity*satellite_eccentricity);
+    const double acceleration = thrust_magnitude/satellite_mass;
+    t_end_ = t_start + delta_V/acceleration;
+    std::cout << "Transfer time: " << t_end_ << "\n";
+  }
   bool operator==(const ThrustProfileLVLH& input_profile) {
     return ((t_start_ == input_profile.t_start_) &&
             (t_end_ == input_profile.t_end_) &&
@@ -332,7 +354,8 @@ class Satellite {
   void add_LVLH_thrust_profile(
       const std::array<double, 3> input_LVLH_thrust_vector,
       const double input_thrust_start_time, const double input_thrust_end_time);
-
+  void add_LVLH_thrust_profile(const double input_thrust_start_time, const double final_arg_of_periapsis,
+                                const double input_thrust_magnitude);
   void add_bodyframe_torque_profile(
       const std::array<double, 3> input_bodyframe_direction_unit_vec,
       const double input_bodyframe_torque_magnitude,
@@ -357,6 +380,7 @@ class Satellite {
                                                 const double yaw_angle);
   double get_attitude_val(const std::string input_attitude_val_name);
   double calculate_orbital_period();
+  int add_arg_of_periapsis_change_thrust();
 };
 
 #endif

include/utils.h

@@ -515,4 +515,6 @@ void sim_and_plot_gs_connectivity_gnuplot(PhasedArrayGroundStation input_ground_
 
 int add_lowthrust_orbit_transfer(Satellite& input_satellite_object, const double final_orbit_semimajor_axis_km, 
   const double thrust_magnitude, const double transfer_initiation_time = 0);
+
+double calibrate_mean_val(Satellite satellite_object, const SimParameters& input_sim_parameters, const std::string input_parameter_name);
 #endif

simulation_setup.cpp

@@ -134,9 +134,40 @@ int main() {
   sim_parameters.x_increment = pow(10,7);
   sim_parameters.y_increment = pow(10,7);
   sim_parameters.z_increment = 5*pow(10,6);
-  file_name = "Semimajor axis transfer";
-  sim_and_plot_orbital_elem_gnuplot(orbit_transfer_demo_vec, sim_parameters, "Semimajor Axis", file_name);
-
+  // file_name = "Semimajor axis transfer";
+  // sim_and_plot_orbital_elem_gnuplot(orbit_transfer_demo_vec, sim_parameters, "Semimajor Axis", file_name);
   sim_and_draw_orbit_gnuplot(orbit_transfer_demo_vec,sim_parameters);
-return 0;
+
+
+  // Now let's demonstrate changing the argument of periapsis
+  // Calibration strategy: there are some inherent oscillations in, e.g., arg of periapsis, even in absence of external forces or perturbations
+  // This oscillation magnitude can be different in initial and final orbits, so current strategy is to find the mean val of these oscillations
+  // at initial and final orbits, use those to get an offset, which can be applied to the final argument of periapsis to try to make the
+  // oscillations at the final orbit be close to the target value
+
+  Satellite arg_periapsis_change_sat("../example_satellite_input_files/arg_of_periapsis_test_input.json");
+  Satellite arg_periapsis_change_calibration_sat("../example_satellite_input_files/arg_of_periapsis_calibration_input.json");
+  sim_parameters.epsilon = pow(10,-12);
+  sim_parameters.total_sim_time = 250000; 
+  sim_parameters.perturbation_bool = false;
+  sim_parameters.drag_bool = false;
+  double set_initial_arg_of_periapsis = arg_periapsis_change_sat.get_orbital_parameter("Argument of Periapsis");
+  double mean_val_final = calibrate_mean_val(arg_periapsis_change_calibration_sat,sim_parameters,"Argument of Periapsis");
+  t_thrust_start = 25000;
+  double mean_val_initial = calibrate_mean_val(arg_periapsis_change_sat,sim_parameters,"Argument of Periapsis");
+  // NOTE: Make sure the below value matches the argument of periapsis of your calibration satellite.
+  double final_arg_of_periapsis_deg = 20;
+  if (final_arg_of_periapsis_deg != arg_periapsis_change_calibration_sat.get_orbital_parameter("Argument of Periapsis")) {
+    std::cout << "Warning: Entered final argument of periapsis differs from value in calibration satellite object. "
+    "This will skew the applied offset from the nominal value. Make sure these two argument of periapsis values are equal.\n";
+  }
+  double offset = (final_arg_of_periapsis_deg - mean_val_final) + (set_initial_arg_of_periapsis - mean_val_initial);
+  double final_arg_of_periapsis = final_arg_of_periapsis_deg * (M_PI/180.0); // rad
+  thrust_magnitude = 0.1; // N
+  arg_periapsis_change_sat.add_LVLH_thrust_profile(t_thrust_start,final_arg_of_periapsis+(offset*M_PI/180.0),thrust_magnitude);
+  file_name = "Arg of periapsis transfer";
+  std::vector<Satellite> arg_of_periapsis_transfer_vec = {arg_periapsis_change_sat};
+  sim_and_plot_orbital_elem_gnuplot(arg_of_periapsis_transfer_vec, sim_parameters, "Argument of Periapsis", file_name);
+
+  return 0;
 }

src/Satellite.cpp

@@ -25,8 +25,6 @@ double Satellite::calculate_orbital_period() {
 std::array<double, 3> Satellite::calculate_perifocal_position() {
   // Using approach from Fundamentals of Astrodynamics
   std::array<double, 3> calculated_perifocal_position;
-  double mu = G * mass_Earth;
-
   double p =
       a_ * (1 - eccentricity_) * (1 + eccentricity_);  // rearranging eq. 1-47
 
@@ -46,12 +44,11 @@ std::array<double, 3> Satellite::calculate_perifocal_position() {
 std::array<double, 3> Satellite::calculate_perifocal_velocity() {
   // Using approach from Fundamentals of Astrodynamics
   std::array<double, 3> calculated_perifocal_velocity;
-  double mu = G * mass_Earth;
   double p =
       a_ * (1 - eccentricity_) * (1 + eccentricity_);  // rearranging eq. 1-47
-
-  double v_perifocal_P = sqrt(mu / p) * (-sin(true_anomaly_));
-  double v_perifocal_Q = sqrt(mu / p) * (eccentricity_ + cos(true_anomaly_));
+  const double mu_Earth = G*mass_Earth;
+  double v_perifocal_P = sqrt(mu_Earth / p) * (-sin(true_anomaly_));
+  double v_perifocal_Q = sqrt(mu_Earth / p) * (eccentricity_ + cos(true_anomaly_));
 
   calculated_perifocal_velocity.at(0) = v_perifocal_P;
   calculated_perifocal_velocity.at(1) = v_perifocal_Q;
@@ -246,7 +243,7 @@ void Satellite::add_LVLH_thrust_profile(
   ThrustProfileLVLH new_thrust_profile(
       input_thrust_start_time, input_thrust_end_time, input_LVLH_thrust_vector);
   thrust_profile_list_.push_back(new_thrust_profile);
-  if (input_thrust_start_time == 0) {
+  if (input_thrust_start_time == t_) {
     list_of_LVLH_forces_at_this_time_.push_back(input_LVLH_thrust_vector);
     std::array<double, 3> ECI_thrust_vector = convert_LVLH_to_ECI_manual(
         input_LVLH_thrust_vector, ECI_position_, ECI_velocity_);
@@ -271,22 +268,29 @@ void Satellite::add_LVLH_thrust_profile(
                               input_LVLH_normalized_thrust_direction.at(ind);
   }
 
-  if (input_thrust_start_time == 0) {
+  if (input_thrust_start_time == t_) {
     list_of_LVLH_forces_at_this_time_.push_back(LVLH_thrust_vec);
     std::array<double, 3> ECI_thrust_vector = convert_LVLH_to_ECI_manual(
         LVLH_thrust_vec, ECI_position_, ECI_velocity_);
     list_of_ECI_forces_at_this_time_.push_back(ECI_thrust_vector);
   }
 }
 
+  // Now the version for argument of periapsis change maneuvers
+  void Satellite::add_LVLH_thrust_profile(const double input_thrust_start_time, const double final_arg_of_periapsis,
+    const double input_thrust_magnitude) {
+      ThrustProfileLVLH thrust_profile(input_thrust_start_time,final_arg_of_periapsis,input_thrust_magnitude,
+                                        arg_of_periapsis_, eccentricity_, a_, m_);
+      thrust_profile_list_.push_back(thrust_profile);
+    }
+
 int Satellite::update_orbital_elements_from_position_and_velocity() {
   // Anytime the orbit is changed via external forces, need to update the
   // orbital parameters of the satellite. True anomaly should change over time
   // even in absence of external forces Using approach from Fundamentals of
   // Astrodynamics
   int error_code = 0;  // 0 represents nominal operation
-  double mu = G * mass_Earth;
-
+  double mu_Earth = G*mass_Earth;
   Vector3d position_vector = {ECI_position_.at(0), ECI_position_.at(1),
                               ECI_position_.at(2)};
   Vector3d velocity_vector = {ECI_velocity_.at(0), ECI_velocity_.at(1),
@@ -301,14 +305,14 @@ int Satellite::update_orbital_elements_from_position_and_velocity() {
   double r_magnitude = get_radius();
 
   Vector3d e_vec_component_1 =
-      (1.0 / mu) * (v_magnitude * v_magnitude - (mu / r_magnitude)) *
+      (1.0 / mu_Earth) * (v_magnitude * v_magnitude - (mu_Earth / r_magnitude)) *
       position_vector;
   Vector3d e_vec_component_2 =
-      (1.0 / mu) * (position_vector.dot(velocity_vector)) * velocity_vector;
+      (1.0 / mu_Earth) * (position_vector.dot(velocity_vector)) * velocity_vector;
   Vector3d e_vec = e_vec_component_1 - e_vec_component_2;
   double calculated_eccentricity = e_vec.norm();
 
-  double calculated_p = h * h / mu;
+  double calculated_p = h * h / mu_Earth;
 
   double calculated_i = acos(h_vector(2) / h);
 
@@ -326,9 +330,20 @@ int Satellite::update_orbital_elements_from_position_and_velocity() {
     if (e_vec(2) < 0) {
       calculated_arg_of_periapsis = 2 * M_PI - calculated_arg_of_periapsis;
     }
-
     calculated_true_anomaly = acos(e_vec.dot(position_vector) /
-                                   (calculated_eccentricity * r_magnitude));
+    (calculated_eccentricity * r_magnitude));
+    if (std::isnan(calculated_true_anomaly)) {
+      // First try arg of the following acos call to a float to avoid bug
+      // where it ended up being represented by a value with magnitude 
+      // slightly larger than one, causing NaN in output of acos
+      float true_anomaly_acos_arg_float = e_vec.dot(position_vector) /
+      (calculated_eccentricity * r_magnitude);
+      calculated_true_anomaly = acos(true_anomaly_acos_arg_float);
+      if (std::isnan(calculated_true_anomaly)) {
+        std::cout << "NaN true anomaly encountered.\n";
+        error_code = 10;
+      }
+    }
     if (position_vector.dot(velocity_vector) < 0) {
       calculated_true_anomaly = 2 * M_PI - calculated_true_anomaly;
     }
@@ -398,6 +413,9 @@ std::pair<double, int> Satellite::evolve_RK45(
   double input_F_10 = drag_elements.first;
   double input_A_p = drag_elements.second;
 
+  // Add any thrusts from an argument of periapsis change maneuver, if applicable
+  int arg_of_periapsis_code = add_arg_of_periapsis_change_thrust();
+
   std::array<double, 13>
       combined_initial_position_velocity_quaternion_angular_velocity_array = {};
   std::pair<std::array<double, 3>, std::array<double, 3>>
@@ -506,6 +524,11 @@ std::pair<double, int> Satellite::evolve_RK45(
   evolve_RK45_output_pair.first = new_step_size;
   evolve_RK45_output_pair.second = orbit_elems_error_code;
 
+  // If you just added a temporally localized thrust profile stemming from an argument of periapsis change thrust profile,
+  // remove it now that it's no longer needed to avoid bloat of the thrust profile list
+  if (arg_of_periapsis_code == 1) {
+    thrust_profile_list_.pop_back();
+  }
   return evolve_RK45_output_pair;
 }
 
@@ -660,4 +683,20 @@ void Satellite::initialize_body_angular_velocity_vec_wrt_LVLH_in_body_frame() {
           initial_body_angular_velocity_in_LVLH_frame, roll_angle_, yaw_angle_,
           pitch_angle_);  // Because LVLH is always rotating around y axis to
                           // keep z pointed towards Earth
+}
+int Satellite::add_arg_of_periapsis_change_thrust() {
+  // Returning 0 means didn't find any arg of periapsis change thrust profiles
+  // Returning 1 means it did find one and added a temporally localized thrust at this time
+  for (ThrustProfileLVLH thrust_profile : thrust_profile_list_) {
+    if ((thrust_profile.arg_of_periapsis_change_thrust_profile) && (thrust_profile.t_start_ <= t_) && (thrust_profile.t_end_ >= t_)) {
+      std::array<double,3> thrust_direction_vec = {sin(true_anomaly_), 0, cos(true_anomaly_)};
+
+      add_LVLH_thrust_profile(thrust_direction_vec, thrust_profile.sign_of_delta_omega*thrust_profile.thrust_magnitude_,t_,thrust_profile.t_end_); 
+      // can add one with t_end stretching all the way to the t_end of the total arg of periapsis change maneuver because this temporary thrust profile will
+      // be deleted after this RK45 step. This end time is given so that whatever timestep is chosen for this step, all intermediate calculated steps will
+      // also see this thrust.
+      return 1;
+    }
+  }
+  return 0;
 }

src/utils.cpp

@@ -200,7 +200,8 @@ std::array<double, 3> calculate_orbital_acceleration(
   for (const ThrustProfileLVLH thrust_profile :
        input_list_of_thrust_profiles_LVLH) {
     if ((input_evaluation_time >= thrust_profile.t_start_) &&
-        (input_evaluation_time <= thrust_profile.t_end_)) {
+        (input_evaluation_time <= thrust_profile.t_end_) &&
+        (thrust_profile.arg_of_periapsis_change_thrust_profile == false)) {
       list_of_LVLH_forces_at_evaluation_time.push_back(
           thrust_profile.LVLH_force_vec_);
       std::array<double, 3> ECI_thrust_vector = convert_LVLH_to_ECI_manual(
@@ -226,9 +227,9 @@ std::array<double, 3> calculate_orbital_acceleration(
     // perturbation Ref:
     // https://vatankhahghadim.github.io/AER506/Notes/6%20-%20Orbital%20Perturbations.pdf
     double J2 = 1.083 * pow(10, -3);
-    double mu = G * mass_Earth;
+    const double mu_Earth = G*mass_Earth;
     double C =
-        3 * mu * J2 * radius_Earth * radius_Earth / (2 * pow(distance, 4));
+        3 * mu_Earth * J2 * radius_Earth * radius_Earth / (2 * pow(distance, 4));
     double x = input_r_vec.at(0);
     double y = input_r_vec.at(1);
     double rho = sqrt(pow(x, 2) + pow(y, 2));
@@ -660,6 +661,8 @@ void sim_and_plot_orbital_elem_gnuplot(
       double timestep_to_use = input_sim_parameters.initial_timestep_guess;
       current_satellite_time = current_satellite.get_instantaneous_time();
       while (current_satellite_time < input_sim_parameters.total_sim_time) {
+        // std::cout << "========================================================\n";
+        // std::cout << "Running an evolve step at satellite time " << current_satellite_time << "\n";
         std::pair<double, double> drag_elements = {input_sim_parameters.F_10, input_sim_parameters.A_p};
         std::pair<double, int> new_timestep_and_error_code =
             current_satellite.evolve_RK45(input_sim_parameters.epsilon, timestep_to_use,
@@ -1585,9 +1588,9 @@ int add_lowthrust_orbit_transfer(Satellite& input_satellite_object, const double
   double thrust_acceleration = input_thrust_magnitude/satellite_mass;
   double semimajor_axis_final = 1000 * final_orbit_semimajor_axis_km; // m
   double semimajor_axis_initial = input_satellite_object.get_orbital_parameter("Semimajor Axis");
-  double mu = G*mass_Earth;
-  double comp1 =sqrt(mu/semimajor_axis_initial);
-  double comp2 = sqrt(mu/semimajor_axis_final);
+  const double mu_Earth = G*mass_Earth;
+  double comp1 =sqrt(mu_Earth/semimajor_axis_initial);
+  double comp2 = sqrt(mu_Earth/semimajor_axis_final);
   double time_to_burn = (comp1-comp2)/thrust_acceleration;
 
   // Thrust is purely co-linear with velocity vector, so in the +- x direction of the LVLH frame
@@ -1609,4 +1612,48 @@ int add_lowthrust_orbit_transfer(Satellite& input_satellite_object, const double
       transfer_initiation_time, transfer_initiation_time + time_to_burn);
   }
   return error_code;
+}
+
+double calibrate_mean_val(Satellite satellite_object, const SimParameters& input_sim_parameters, const std::string input_parameter_name) {
+  // Objective: help calibrate simulations in context of inherent oscillations of parameters
+  // Here, the mean value of oscillations will be assumed to be constant (oscillations don't drift up or down over time)
+
+  // Let the simulation run without external applied forces, return mean value of parameter
+  // Not passing in satellite object by ref so that its internal clock doesn't get altered from its initial value before the actual
+  // simulations start
+
+  double val =
+  satellite_object.get_orbital_parameter(input_parameter_name);
+  double mean_val = val;
+  size_t num_datapoints = 1;
+  double current_satellite_time =
+    satellite_object.get_instantaneous_time();
+
+  double evolved_val = {0};
+
+  double timestep_to_use = input_sim_parameters.initial_timestep_guess;
+  current_satellite_time = satellite_object.get_instantaneous_time();
+  while (current_satellite_time < input_sim_parameters.total_sim_time) {
+  std::pair<double, double> drag_elements = {input_sim_parameters.F_10, input_sim_parameters.A_p};
+  std::pair<double, int> new_timestep_and_error_code =
+  satellite_object.evolve_RK45(input_sim_parameters.epsilon, timestep_to_use,
+        input_sim_parameters.perturbation_bool, 
+        input_sim_parameters.drag_bool, drag_elements);
+  double new_timestep = new_timestep_and_error_code.first;
+  int error_code = new_timestep_and_error_code.second;
+
+  if (error_code != 0) {
+    std::cout << "Error code " << error_code << " detected, halting simulation and returning 0\n";
+    return 0.0;
+  }
+  timestep_to_use = new_timestep;
+  evolved_val =
+  satellite_object.get_orbital_parameter(input_parameter_name);
+  mean_val += evolved_val;
+  num_datapoints+=1;
+
+  current_satellite_time = satellite_object.get_instantaneous_time();
+  }
+  mean_val /= num_datapoints;
+  return mean_val;
 }