Updated Orbit Propagator

config/parameters/truth/base.txt

@@ -10,6 +10,7 @@ truth.dt.ns  170000000
 
 # Leader spacecraft attitude and orbit shared initial conditions.
 
+truth.leader.S  0.03
 truth.leader.m  5.0
 truth.leader.J  0.03798 0.03957 0.00688
 
@@ -18,6 +19,7 @@ truth.leader.wheels.w_max  677.0
 
 # Follower spacecraft attitude and orbit shared initial conditions.
 
+truth.follower.S  0.03
 truth.follower.m  5.0
 truth.follower.J  0.03798 0.03957 0.00688
 

include/gnc/constants.hpp

@@ -36,6 +36,10 @@ GNC_TRACKED_CONSTANT(constexpr static double, mu_earth, 3.986004418e14);
 
 GNC_TRACKED_CONSTANT(constexpr static float, mu_earth_f, mu_earth);
 
+GNC_TRACKED_CONSTANT(constexpr static double, r_earth, 6.3781e6);
+
+GNC_TRACKED_CONSTANT(constexpr static float, r_earth_f, r_earth);
+
 GNC_TRACKED_CONSTANT(constexpr static double, nan, std::numeric_limits<double>::quiet_NaN());
 
 GNC_TRACKED_CONSTANT(constexpr static float, nan_f, std::numeric_limits<float>::quiet_NaN());

include/psim/truth/orbit.hpp

@@ -35,23 +35,27 @@
 
 namespace psim {
 
-/** @brief Orbit propegator in ECI implemented using the GNC gravity model.
+/** @brief Orbit propagator in ECEF.
  */
-class OrbitGncEci : public Orbit<OrbitGncEci> {
+class OrbitEcef : public Orbit<OrbitEcef> {
  private:
-  typedef Orbit<OrbitGncEci> Super;
+  typedef Orbit<OrbitEcef> Super;
   gnc::Ode4<Real, 6> ode;
 
  public:
-  OrbitGncEci() = delete;
-  virtual ~OrbitGncEci() = default;
+  OrbitEcef() = delete;
+  virtual ~OrbitEcef() = default;
 
-  /** @brief Set the frame argument to ECI.
+  /** @brief Set the frame argument to ECEF.
    */
-  OrbitGncEci(RandomsGenerator &randoms, Configuration const &config,
+  OrbitEcef(RandomsGenerator &randoms, Configuration const &config,
       std::string const &satellite);
 
   virtual void step() override;
+
+  Real truth_satellite_orbit_T() const;
+  Real truth_satellite_orbit_U() const;
+  Real truth_satellite_orbit_E() const;
 };
 } // namespace psim
 

include/psim/truth/orbit.yml

@@ -15,6 +15,11 @@ params:
       type: Real
       comment: >
           Mass of the satellite in units of kilograms.
+    - name: "truth.{satellite}.S"
+      type: Real
+      comment: >
+          Projected area of the satellite along the direction of travel in
+          meters squared. This value is used for the drag calculation.
 
 adds:
     - name: "truth.{satellite}.orbit.r"
@@ -34,13 +39,24 @@ adds:
           kilogram meters per second squared. The coordinate system is
           implementation dependant. Note that this field is zeroed out on each
           simulation step to avoid applying a continuous input.
+    - name: "truth.{satellite}.orbit.T"
+      type: Lazy Real
+      comment: >
+          Satellite's orbital kinetic energy.
+    - name: "truth.{satellite}.orbit.U"
+      type: Lazy Real
+      comment: >
+          Satellite's orbital potential energy.
+    - name: "truth.{satellite}.orbit.E"
+      type: Lazy Real
+      comment: >
+          Satellite's orbital total energy. This is essentially the difference
+          of the kinetic and potential energies.
 
 gets:
-    - name: "truth.t.s"
-      type: Real
-      comment: >
-          Time sense the PAN epoch in seconds.
+    - name: "truth.earth.w"
+      type: Vector3
+    - name: "truth.earth.w_dot"
+      type: Vector3
     - name: "truth.dt.s"
       type: Real
-      comment: >
-          Timestep of the simulation in seconds.

src/psim/truth/orbit.cpp

@@ -28,63 +28,107 @@
 
 #include <psim/truth/orbit.hpp>
 
-#include <gnc/environment.hpp>
+#include <gnc/config.hpp>
 #include <gnc/utilities.hpp>
 
 #include <lin/core.hpp>
-#include <lin/generators/constants.hpp>
+#include <lin/generators.hpp>
 #include <lin/math.hpp>
 #include <lin/references.hpp>
 
+#include <psim/truth/orbit_utilities.hpp>
+
 namespace psim {
 
-OrbitGncEci::OrbitGncEci(RandomsGenerator &randoms, Configuration const &config,
+OrbitEcef::OrbitEcef(RandomsGenerator &randoms, Configuration const &config,
     std::string const &satellite)
-  : Super(randoms, config, satellite, "eci") { }
+  : Super(randoms, config, satellite, "ecef") {}
 
-void OrbitGncEci::step() {
+void OrbitEcef::step() {
   this->Super::step();
 
-  // References to the current time and timestep
+  struct IntegratorData {
+    Real const &m;
+    Real const &S;
+    Vector3 const &earth_w;
+    Vector3 const &earth_w_dot;
+  };
+
   auto const &dt = truth_dt_s->get();
-  auto const &t = truth_t_s->get();
-
-  // References to position and velocity
-  auto &r = truth_satellite_orbit_r.get();
-  auto &v = truth_satellite_orbit_v.get();
-
-  // Treat our thruster firings as purely impulses
-  v = v + truth_satellite_orbit_J_frame.get() / truth_satellite_m.get();
-  truth_satellite_orbit_J_frame.get() = lin::zeros<Vector3>();
-
-  // Simulate our dynamics
-  auto const xf = ode(t, dt, {r(0), r(1), r(2), v(0), v(1), v(2)}, nullptr,
-      [](Real t, Vector<6> const &x, void *) -> Vector<6> {
-    // References to our position and velocity in ECI
-    auto const r = lin::ref<Vector3>(x, 0, 0);
-    auto const v = lin::ref<Vector3>(x, 3, 0);
-
-    // Calculate the Earth's current attitude
-    Vector4 q;
-    gnc::env::earth_attitude(t, q);  // q = q_ecef_eci
-
-    // Determine our gravitation acceleration in ECI
-    Vector3 g;
-    {
-      Vector3 r_ecef;
-      gnc::utl::rotate_frame(q, r.eval(), r_ecef);
-
-      Real _;
-      gnc::env::gravity(r_ecef, g, _);  // g = g_ecef
-      gnc::utl::quat_conj(q);           // q = q_eci_ecef
-      gnc::utl::rotate_frame(q, g);     // g = g_eci
-    }
-
-    return {v(0), v(1), v(2), g(0), g(1), g(2)};
-  });
+  auto const &earth_w = truth_earth_w->get();
+  auto const &earth_w_dot = truth_earth_w_dot->get();
+  auto const &S = truth_satellite_S.get();
+  auto const &m = truth_satellite_m.get();
+
+  auto &r_ecef = truth_satellite_orbit_r.get();
+  auto &v_ecef = truth_satellite_orbit_v.get();
+  auto &J_ecef = truth_satellite_orbit_J_frame.get();
+
+  // Thruster firings are modelled here as instantaneous impulses. This removes
+  // thruster dependance from the state dot function in the integrator.
+  v_ecef = v_ecef + J_ecef / m;
+  J_ecef = lin::zeros<Vector3>();
+
+  // Prepare integrator inputs
+  Vector<6> x;
+  lin::ref<Vector3>(x, 0, 0) = r_ecef;
+  lin::ref<Vector3>(x, 3, 0) = v_ecef;
+  IntegratorData data = {S, m, earth_w, earth_w_dot};
+
+  // Simulate dynamics
+  x = ode(Real(0.0), dt, x, &data,
+      [](Real t, Vector<6> const &x, void *ptr) -> Vector<6> {
+        auto const *data = static_cast<IntegratorData *>(ptr);
+
+        auto const &m = data->m;
+        auto const &S = data->S;
+        auto const earth_w = (data->earth_w + t * data->earth_w_dot).eval();
+        auto const &earth_w_dot = data->earth_w_dot;
+
+        auto const r_ecef = lin::ref<Vector3>(x, 0, 0);
+        auto const v_ecef = lin::ref<Vector3>(x, 3, 0);
+
+        Vector3 const a_ecef = orbit::acceleration(
+            earth_w, earth_w_dot, r_ecef.eval(), v_ecef.eval(), S, m);
+
+        Vector<6> dx;
+        lin::ref<Vector3>(dx, 0, 0) = v_ecef;
+        lin::ref<Vector3>(dx, 3, 0) = a_ecef;
+
+        return dx;
+      });
 
   // Write back to our state fields
-  r = lin::ref<Vector3>(xf, 0, 0);
-  v = lin::ref<Vector3>(xf, 3, 0);
+  r_ecef = lin::ref<Vector3>(x, 0, 0);
+  v_ecef = lin::ref<Vector3>(x, 3, 0);
+}
+
+Real OrbitEcef::truth_satellite_orbit_T() const {
+  static constexpr Real half = 0.5;
+
+  auto const &earth_w = truth_earth_w->get();
+  auto const &r_ecef = truth_satellite_orbit_r.get();
+  auto const &v_ecef = truth_satellite_orbit_v.get();
+  auto const &m = truth_satellite_m.get();
+
+  return half * m * lin::fro(v_ecef + lin::cross(earth_w, r_ecef));
+}
+
+Real OrbitEcef::truth_satellite_orbit_U() const {
+  auto const &r_ecef = truth_satellite_orbit_r.get();
+  auto const &m = truth_satellite_m.get();
+
+  Real U;
+  Vector3 _;
+  orbit::gravity(r_ecef, _, U);
+
+  return m * U;
+}
+
+Real OrbitEcef::truth_satellite_orbit_E() const {
+  auto const &T = this->Super::truth_satellite_orbit_T.get();
+  auto const &U = this->Super::truth_satellite_orbit_U.get();
+
+  return T - U;
 }
-}  // namespace psim
+} // namespace psim

src/psim/truth/orbit_utilities.cpp

@@ -0,0 +1,138 @@
+//
+// MIT License
+//
+// Copyright (c) 2020 Pathfinder for Autonomous Navigation (PAN)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+/** @file psim/truth/orbit_utilities.cpp
+ *  @author Kyle Krol
+ */
+
+#include <psim/truth/orbit_utilities.hpp>
+
+#include <gnc/constants.hpp>
+
+#include <lin/core.hpp>
+#include <lin/math.hpp>
+
+#include <GGM05S.hpp>
+#include <geograv.hpp>
+
+namespace psim {
+namespace orbit {
+
+void gravity(Vector3 const &r_ecef, Vector3 &g_ecef) {
+  Real _;
+  gravity(r_ecef, g_ecef, _);
+}
+
+void gravity(Vector3 const &r_ecef, Vector3 &g_ecef, Real &U) {
+  static constexpr auto order = 11;
+  static constexpr auto grav = static_cast<geograv::Coeff<order>>(GGM05S);
+
+  geograv::Vector g, r = {r_ecef(0), r_ecef(1), r_ecef(2)};
+  U = geograv::GeoGrav(r, g, grav, true);
+  g_ecef = {g.x, g.y, g.z};
+}
+
+Real density(Vector3 const &r_ecef) {
+  /* Atmospheric density model is pulled from section 11.2.1 of "Fundamentals of
+   * Spacecraft Attitude Determination and Control" by Markley and Crassidis.
+   */
+  static constexpr lin::size_t I = 36;
+  static constexpr Vector<I> h0 = {0.0, 25.0e3, 30.0e3, 35.0e3, 40.0e3, 45.0e3,
+      50.0e3, 55.0e3, 60.0e3, 65.0e3, 70.0e3, 75.0e3, 80.0e3, 85.0e3, 90.0e3,
+      95.0e3, 100.0e3, 110.0e3, 120.0e3, 130.0e3, 140.0e3, 150.0e3, 160.0e3,
+      180.0e3, 200.0e3, 250.0e3, 300.0e3, 350.0e3, 400.0e3, 450.0e3, 500.0e3,
+      600.0e3, 700.0e3, 800.0e3, 900.0e3, 1000.0e3};
+  static constexpr Vector<I> p0 = {1.225, 3.899e-2, 1.774e-2, 8.279e-3,
+      3.972e-3, 1.995e-3, 1.057e-3, 5.821e-4, 3.206e-4, 1.718e-4, 8.770e-5,
+      4.178e-5, 1.905e-5, 8.337e-6, 3.396e-6, 1.343e-6, 5.297e-7, 9.661e-8,
+      2.438e-8, 8.484e-9, 3.845e-9, 2.070e-9, 1.224e-9, 5.464e-10, 2.789e-10,
+      7.248e-11, 2.418e-11, 9.158e-12, 3.725e-12, 1.585e-12, 6.967e-13,
+      1.454e-13, 3.614e-14, 1.170e-14, 5.245e-15, 3.019e-15};
+  static constexpr Vector<I> H = {8.44e3, 6.49e3, 6.75e3, 7.07e3, 7.47e3,
+      7.83e3, 7.95e3, 7.73e3, 7.29e3, 6.81e3, 6.33e3, 6.00e3, 5.70e3, 5.41e3,
+      5.38e3, 5.74e3, 6.15e3, 8.06e3, 11.6e3, 16.1e3, 20.6e3, 24.6e3, 26.3e3,
+      33.2e3, 38.5e3, 46.9e3, 52.5e3, 56.4e3, 59.4e3, 62.2e3, 65.8e3, 79.0e3,
+      109.0e3, 164.0e3, 225.0e3, 268.0e3};
+
+  // Calculate our altitude
+  auto const h = lin::norm(r_ecef) - gnc::constant::r_earth;
+
+  // Determine the appropriate index
+  lin::size_t i = h0.size() - 1;
+  while (h < h0(i) && i > 0) i--;
+
+  // Atmospheric density calculation
+  return p0(i) * lin::exp((h0(i) - h) / H(i));
+}
+
+Vector3 drag(Vector3 const &r_ecef, Vector3 const &v_ecef, Real S, Real m) {
+  static constexpr Real half = 0.5;
+
+  /* Drag coefficient pulled from "An Evaluation of CubeSat Orbital Decay" by
+   * Oltrogge and Leveque.
+   *
+   * https://digitalcommons.usu.edu/cgi/viewcontent.cgi?article=1144&context=smallsat
+   */
+  static constexpr Real Cd = 2.2;
+
+  /* Section 10.3.3 of "Fundamental of Spacecraft Attitude Determination and
+   * Control" by Markley and Crassidis gives force due to atmospheric drag as:
+   *
+   *  F_drag = -1/2 rho Cd S |v| v
+   *
+   * where F_drag is the force of drag, rho is atmospheric density, Cd is a
+   * dimensionaless drag coefficient, S is the object area projected along the
+   * direction of travel, and v is the velocity relative to the atmosphere (i.e.
+   * in ECEF because the atmosphere rotates).
+   */
+  auto const rho = density(r_ecef);
+  return v_ecef * (-half * Cd * S * rho * lin::norm(v_ecef) / m);
+}
+
+Vector3 acceleration(Vector3 const &earth_w, Vector3 const &earth_w_dot,
+    Vector3 const &r_ecef, Vector3 const &v_ecef, Real S, Real m) {
+  static constexpr Real two = 2.0;
+
+  /* Numerically, starting with the smaller forces first like fake forces and
+   * drag will reduce rounding errors. Therefore, we included the force of
+   * gravity last here.
+   */
+
+  /* Acceleration due to the rotating frame.
+   *
+   * https://en.wikipedia.org/wiki/Rotating_reference_frame#Relation_between_accelerations_in_the_two_frames
+   */
+  Vector3 const a_ecef = -(two * lin::cross(earth_w, v_ecef) +
+                           lin::cross(earth_w, lin::cross(earth_w, r_ecef)) +
+                           lin::cross(earth_w_dot, r_ecef));
+
+  Vector3 const a_drag_ecef = drag(r_ecef, v_ecef, S, m);
+
+  Vector3 g_ecef;
+  gravity(r_ecef, g_ecef);
+
+  return (a_ecef + a_drag_ecef) + g_ecef;
+}
+} // namespace orbit
+} // namespace psim