Update Initial Conditions and Fix Drag Related Bug

config/parameters/truth/deployment.txt

@@ -12,7 +12,7 @@ truth.leader.attitude.w           0.05 0.3 -0.1
 truth.leader.wheels.w             0.0  0.0  0.0
 
 truth.leader.orbit.r  6.8538e6 0.0      0.0
-truth.leader.orbit.v  0.0      5.3952e3 5.3952e3
+truth.leader.orbit.v  0.0      4.8954e3 5.3952e3
 
 # Follower spacecraft attitude and orbit initial conditions.
 #
@@ -24,4 +24,4 @@ truth.follower.attitude.w           0.2 -0.15 0.1
 truth.follower.wheels.w             0.0  0.0  0.0
 
 truth.follower.orbit.r  6.8538e6 0.0      0.0
-truth.follower.orbit.v  0.0      5.3954e3 5.3951e3
+truth.follower.orbit.v  0.0      4.8953e3 5.3951e3

config/parameters/truth/detumble.txt

@@ -1,6 +1,6 @@
 # Initial conditions for simulations starting during detumble. These initial
 # conditions were generated with the following command:
-#   python -m psim -s 10000 -c truth/base,truth/deployment --snapshot snapshot.txt DualAttitudeOrbitGnc
+#   python -m psim -s 10000 -c sensors/base,truth/base,truth/deployment --snapshot snapshot.txt DualAttitudeOrbitGnc
 #
 
 # Time model initial conditions.
@@ -13,14 +13,14 @@ truth.leader.attitude.q.body_eci -0.0629875 0.185791 -0.19747   0.960479
 truth.leader.attitude.w          -0.077216  0.294684  0.0952384
 truth.leader.wheels.w             0.0       0.0       0.0
 
-truth.leader.orbit.r  -2.16431e6  4.60304e6  4.5925e6
-truth.leader.orbit.v  -7236.19   -1695.2    -1708.76
+truth.leader.orbit.r -1.57865e6  4.83522e6  4.59249e6
+truth.leader.orbit.v -7037.69   -672.416   -1708.86
 
 # Follower spacecraft attitude and orbit initial conditions.
 
-truth.follower.orbit.r  -2.16425e6  4.60331e6  4.59251e6
-truth.follower.orbit.v  -7236.17   -1695.04   -1708.5
-
 truth.follower.attitude.q.body_eci -0.282557 -0.604902  -0.71832   -0.195632
 truth.follower.attitude.w          -0.247035  0.0574472  0.0659878
 truth.follower.wheels.w             0.0       0.0        0.0
+
+truth.follower.orbit.r -1.5788e6  4.83496e6  4.59222e6
+truth.follower.orbit.v -7037.81  -672.805   -1709.27

config/parameters/truth/standby.txt

@@ -1,7 +1,7 @@
 # Initial conditions for simulations starting during standby. These initial
 # conditions were loosely based on output form the following command(s):
-#   python -m psim -s 23000 -c truth/base,truth/deployment --snapshot snapshot.txt DetumblerTest
-#   python -m psim -s 23000 -c truth/base,truth/deployment --snapshot snapshot.txt DualAttitudeOrbitGnc
+#   python -m psim -s 23000 -c sensors/base,truth/base,truth/deployment --snapshot snapshot.txt DetumblerTest
+#   python -m psim -s 23000 -c sensors/base,truth/base,truth/deployment --snapshot snapshot.txt DualAttitudeOrbitGnc
 #
 
 # Time initial conditions.
@@ -14,14 +14,14 @@ truth.leader.attitude.q.body_eci  0.173301   0.971103 0.029768   0.161367
 truth.leader.attitude.w           0.00359105 0.137562 0.00386897
 truth.leader.wheels.w             0.0        0.0      0.0
 
-truth.leader.orbit.r  -2.41932e6 -4.53187e6 -4.53571e6
-truth.leader.orbit.v   7135.69   -1917.98   -1887.19
+truth.leader.orbit.r -3.59596e6 -3.66836e6 -4.53598e6
+truth.leader.orbit.v  6040.82   -3585.18   -1886.97
 
 # Follower spacecraft attitude and orbit initial conditions.
 
 truth.follower.attitude.q.body_eci   0.0492112  0.423932 0.765907  0.480881
 truth.follower.attitude.w           -0.134192  -0.002457 0.0027549
 truth.follower.wheels.w              0.0        0.0      0.0
 
-truth.follower.orbit.r  -2.42038e6 -4.53184e6 -4.53543e6
-truth.follower.orbit.v   7135.2    -1918.7    -1887.81
+truth.follower.orbit.r -3.59401e6 -3.66926e6 -4.53628e6
+truth.follower.orbit.v  6042.07   -3584.31   -1885.63

config/plots/truth/orbit.yml

@@ -25,8 +25,3 @@
 - type: Plot2D
   x: truth.t.s
   y: [truth.leader.orbit.v.ecef.x, truth.leader.orbit.v.ecef.y, truth.leader.orbit.v.ecef.z]
-
-# Plot orbital energies
-- type: Plot2D
-  x: truth.t.s
-  y: [truth.leader.orbit.T, truth.leader.orbit.U, truth.leader.orbit.E]

config/plots/truth/orbit_long_duration.yml

@@ -0,0 +1,20 @@
+
+# Plot altitude
+- type: Plot2D
+  x: truth.t.s
+  y: truth.leader.orbit.altitude
+
+# Plot magnitudes of orbital forces
+- type: Plot2DLog
+  x: truth.t.s
+  y: [truth.leader.orbit.a_gravity.norm, truth.leader.orbit.a_drag.norm, truth.leader.orbit.a_rot.norm]
+
+# Plot atmospheric density
+- type: Plot2D
+  x: truth.t.s
+  y: truth.leader.orbit.density
+
+# Plot orbital energies
+- type: Plot2D
+  x: truth.t.s
+  y: [truth.leader.orbit.T, truth.leader.orbit.U, truth.leader.orbit.E]

include/psim/truth/attitude_orbit.hpp

@@ -54,6 +54,11 @@ class AttitudeOrbitNoFuelEcef : public AttitudeOrbit<AttitudeOrbitNoFuelEcef> {
 
   virtual void step() override;
 
+  Real truth_satellite_orbit_altitude() const;
+  Vector3 truth_satellite_orbit_a_gravity() const;
+  Vector3 truth_satellite_orbit_a_drag() const;
+  Vector3 truth_satellite_orbit_a_rot() const;
+  Real truth_satellite_orbit_density() const;
   Real truth_satellite_orbit_T() const;
   Real truth_satellite_orbit_U() const;
   Real truth_satellite_orbit_E() const;

include/psim/truth/attitude_orbit.yml

@@ -31,7 +31,7 @@ params:
           radians per second.
 
 adds:
-    - name: "truth.{satellite}.orbit.S"
+    - name: "truth.{satellite}.S"
       type: Real
       comment: >
           Projected area of the satellite along the direction of travel in
@@ -53,6 +53,27 @@ 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.altitude"
+      type: Lazy Real
+      comment: >
+          Altitude of the satellite in meters.
+    - name: "truth.{satellite}.orbit.a_gravity"
+      type: Lazy Vector3
+      comment: >
+          Acceleration due to gravity acting on the satellite in ECEF.
+    - name: "truth.{satellite}.orbit.a_drag"
+      type: Lazy Vector3
+      comment: >
+          Acceleration due to drag acting on the satellite in ECEF.
+    - name: "truth.{satellite}.orbit.a_rot"
+      type: Lazy Vector3
+      comment: >
+          Acceleration due to the rotating frame acting on the satellite in
+          ECEF.
+    - name: "truth.{satellite}.orbit.density"
+      type: Lazy Real
+      comment: >
+          Density of Earth's atmosphere at satellite's location.
     - name: "truth.{satellite}.orbit.T"
       type: Lazy Real
       comment: >

include/psim/truth/orbit.hpp

@@ -53,6 +53,11 @@ class OrbitEcef : public Orbit<OrbitEcef> {
 
   virtual void step() override;
 
+  Real truth_satellite_orbit_altitude() const;
+  Vector3 truth_satellite_orbit_a_gravity() const;
+  Vector3 truth_satellite_orbit_a_drag() const;
+  Vector3 truth_satellite_orbit_a_rot() const;
+  Real truth_satellite_orbit_density() const;
   Real truth_satellite_orbit_T() const;
   Real truth_satellite_orbit_U() const;
   Real truth_satellite_orbit_E() const;

include/psim/truth/orbit.yml

@@ -39,6 +39,27 @@ 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.altitude"
+      type: Lazy Real
+      comment: >
+          Altitude of the satellite in meters.
+    - name: "truth.{satellite}.orbit.a_gravity"
+      type: Lazy Vector3
+      comment: >
+          Acceleration due to gravity acting on the satellite in ECEF.
+    - name: "truth.{satellite}.orbit.a_drag"
+      type: Lazy Vector3
+      comment: >
+          Acceleration due to drag acting on the satellite in ECEF.
+    - name: "truth.{satellite}.orbit.a_rot"
+      type: Lazy Vector3
+      comment: >
+          Acceleration due to the rotating frame acting on the satellite in
+          ECEF.
+    - name: "truth.{satellite}.orbit.density"
+      type: Lazy Real
+      comment: >
+          Density of Earth's atmosphere at satellite's location.
     - name: "truth.{satellite}.orbit.T"
       type: Lazy Real
       comment: >

src/psim/truth/attitude_orbit.cpp

@@ -69,7 +69,7 @@ void AttitudeOrbitNoFuelEcef::step() {
   auto const &wheels_t_body = truth_satellite_wheels_t.get();
   auto const &b_eci = truth_satellite_environment_b_eci->get();
 
-  auto &S = truth_satellite_orbit_S.get();
+  auto &S = truth_satellite_S.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();
@@ -187,6 +187,42 @@ void AttitudeOrbitNoFuelEcef::step() {
   wheels_w_body = lin::ref<Vector3>(x, 13, 0);
 }
 
+Real AttitudeOrbitNoFuelEcef::truth_satellite_orbit_altitude() const {
+  auto const &r_ecef = truth_satellite_orbit_r.get();
+
+  return lin::norm(r_ecef) - gnc::constant::r_earth;
+}
+
+Vector3 AttitudeOrbitNoFuelEcef::truth_satellite_orbit_a_gravity() const {
+  auto const &r_ecef = truth_satellite_orbit_r.get();
+
+  return orbit::gravity(r_ecef);
+}
+
+Vector3 AttitudeOrbitNoFuelEcef::truth_satellite_orbit_a_drag() const {
+  auto const &m = truth_satellite_m.get();
+  auto const &S = truth_satellite_S.get();
+  auto const &r_ecef = truth_satellite_orbit_r.get();
+  auto const &v_ecef = truth_satellite_orbit_v.get();
+
+  return orbit::drag(r_ecef, v_ecef, S, m);
+}
+
+Vector3 AttitudeOrbitNoFuelEcef::truth_satellite_orbit_a_rot() const {
+  auto const &earth_w = truth_earth_w->get();
+  auto const &earth_w_dot = truth_earth_w_dot->get();
+  auto const &r_ecef = truth_satellite_orbit_r.get();
+  auto const &v_ecef = truth_satellite_orbit_v.get();
+
+  return orbit::rotational(earth_w, earth_w_dot, r_ecef, v_ecef);
+}
+
+Real AttitudeOrbitNoFuelEcef::truth_satellite_orbit_density() const {
+  auto const &r_ecef = truth_satellite_orbit_r.get();
+
+  return orbit::density(r_ecef);
+};
+
 Real AttitudeOrbitNoFuelEcef::truth_satellite_orbit_T() const {
   static constexpr Real half = 0.5;
 
@@ -203,8 +239,7 @@ Real AttitudeOrbitNoFuelEcef::truth_satellite_orbit_U() const {
   auto const &m = truth_satellite_m.get();
 
   Real U;
-  Vector3 _;
-  orbit::gravity(r_ecef, _, U);
+  orbit::gravity(r_ecef, U);
 
   return m * U;
 }

src/psim/truth/orbit.cpp

@@ -29,6 +29,7 @@
 #include <psim/truth/orbit.hpp>
 
 #include <gnc/config.hpp>
+#include <gnc/constants.hpp>
 #include <gnc/utilities.hpp>
 
 #include <lin/core.hpp>
@@ -73,7 +74,7 @@ void OrbitEcef::step() {
   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};
+  IntegratorData data = {m, S, earth_w, earth_w_dot};
 
   // Simulate dynamics
   x = ode(Real(0.0), dt, x, &data,
@@ -103,6 +104,42 @@ void OrbitEcef::step() {
   v_ecef = lin::ref<Vector3>(x, 3, 0);
 }
 
+Real OrbitEcef::truth_satellite_orbit_altitude() const {
+  auto const &r_ecef = truth_satellite_orbit_r.get();
+
+  return lin::norm(r_ecef) - gnc::constant::r_earth;
+}
+
+Vector3 OrbitEcef::truth_satellite_orbit_a_gravity() const {
+  auto const &r_ecef = truth_satellite_orbit_r.get();
+
+  return orbit::gravity(r_ecef);
+}
+
+Vector3 OrbitEcef::truth_satellite_orbit_a_drag() const {
+  auto const &S = truth_satellite_S.get();
+  auto const &m = truth_satellite_m.get();
+  auto const &r_ecef = truth_satellite_orbit_r.get();
+  auto const &v_ecef = truth_satellite_orbit_v.get();
+
+  return orbit::drag(r_ecef, v_ecef, S, m);
+}
+
+Vector3 OrbitEcef::truth_satellite_orbit_a_rot() const {
+  auto const &earth_w = truth_earth_w->get();
+  auto const &earth_w_dot = truth_earth_w_dot->get();
+  auto const &r_ecef = truth_satellite_orbit_r.get();
+  auto const &v_ecef = truth_satellite_orbit_v.get();
+
+  return orbit::rotational(earth_w, earth_w_dot, r_ecef, v_ecef);
+}
+
+Real OrbitEcef::truth_satellite_orbit_density() const {
+  auto const &r_ecef = truth_satellite_orbit_r.get();
+
+  return orbit::density(r_ecef);
+};
+
 Real OrbitEcef::truth_satellite_orbit_T() const {
   static constexpr Real half = 0.5;
 
@@ -119,8 +156,7 @@ Real OrbitEcef::truth_satellite_orbit_U() const {
   auto const &m = truth_satellite_m.get();
 
   Real U;
-  Vector3 _;
-  orbit::gravity(r_ecef, _, U);
+  orbit::gravity(r_ecef, U);
 
   return m * U;
 }