Improved Sun Sensors Model (#288)

config/parameters/sensors/base.txt

@@ -6,6 +6,12 @@
 # drive:
 #   System Level > Integration and Testing > Sensor Charactarization
 #
+# The noise distribution for the sun sensors was chosen as two degrees, one
+# sigma. This matches the attitude filter's settings.
+#
+# Currently, we're not modelling eclipse with the sun sensors due to the
+# attitude estimators questionable performance. This will be updated later.
+#
 
 # Leader spacecraft base sensor configuration
 
@@ -17,6 +23,9 @@ sensors.leader.gyroscope.w.sigma       2.75e-4 2.75e-4  2.75e-4
 
 sensors.leader.magnetometer.b.sigma  5.00e-7 5.00e-7 5.00e-7
 
+sensors.leader.sun_sensors.model_eclipse  0
+sensors.leader.sun_sensors.s.sigma        0.0349066 0.0349066
+
 # Follower spacecraft base sensor configuration
 
 sensors.follower.gps.r.sigma  0.0 0.0 0.0
@@ -26,3 +35,6 @@ sensors.follower.gyroscope.w.bias.sigma   1.00e-6 1.00e-6 1.00e-6
 sensors.follower.gyroscope.w.sigma        2.75e-4 2.75e-4 2.75e-4
 
 sensors.follower.magnetometer.b.sigma  5.00e-7 5.00e-7 5.00e-7
+
+sensors.follower.sun_sensors.model_eclipse  0
+sensors.follower.sun_sensors.s.sigma        0.0349066 0.0349066

config/plots/sensors/sun_sensors.yml

@@ -0,0 +1,10 @@
+
+# Sun sensor's measurement over time.
+- type: Plot2D
+  x: truth.t.s
+  y: [sensors.leader.sun_sensors.s.x, sensors.leader.sun_sensors.s.y, sensors.leader.sun_sensors.s.z]
+
+# Sun sensor's measurement error over time.
+- type: Plot2D
+  x: truth.t.s
+  y: [sensors.leader.sun_sensors.s.error.x, sensors.leader.sun_sensors.s.error.y, sensors.leader.sun_sensors.s.error.z]

include/psim/sensors/sun_sensors.hpp

@@ -44,6 +44,7 @@ class SunSensors : public SunSensorsInterface<SunSensors> {
   virtual ~SunSensors() = default;
 
   Vector3 sensors_satellite_sun_sensors_s() const;
+  Vector3 sensors_satellite_sun_sensors_s_error() const;
 };
 }  // namespace psim
 

include/psim/sensors/sun_sensors.yml

@@ -8,13 +8,36 @@ comment: >
 args:
     - satellite
 
+params:
+    - name: "sensors.{satellite}.sun_sensors.s.sigma"
+      type: Vector2
+      comment: >
+          Noise in terms of a spherical coordinate representation of the sun
+          vector. The noise affects the phi and theta angles in the body frame
+          currently.
+    - name: "sensors.{satellite}.sun_sensors.model_eclipse"
+      type: Integer
+      comment: >
+          Toggle whether or not being in eclipse prevents a sun vector from
+          being modelled.
+
 adds:
     - name: "sensors.{satellite}.sun_sensors.s"
       type: Lazy Vector3
       comment: >
           Sun vector reported by the sun sensors in the body frame. This vector
           is set to NaN if a sun vector cannot be determined.
+    - name: "sensors.{satellite}.sun_sensors.s.error"
+      type: Lazy Vector3
+      comment: >
+          Error in the sun vector reported by the sun sensors. Note, that the
+          error in being represented as the difference between two unit vectors
+          and therefore has an extra degree of freedom.
 
 gets:
     - name: "truth.{satellite}.environment.s.body"
       type: Vector3
+    - name: "truth.{satellite}.environment.s.eci"
+      type: Vector3
+    - name: "truth.{satellite}.orbit.r.eci"
+      type: Vector3

src/psim/sensors/sun_sensors.cpp

@@ -28,12 +28,73 @@
 
 #include <psim/sensors/sun_sensors.hpp>
 
+#include <gnc/constants.hpp>
+#include <gnc/utilities.hpp>
+
+#include <lin/core.hpp>
+#include <lin/generators.hpp>
+#include <lin/math.hpp>
+
 namespace psim {
 
 Vector3 SunSensors::sensors_satellite_sun_sensors_s() const {
+  /* Don't generate a sun vector measurement if in eclipse and the model is
+   * enabled.
+   *
+   * We consider ourselves to be in eclipse if the dot product of our position
+   * vector and the sun vector, both in ECI, is negative.
+   */
+  auto const &model_eclipse = sensors_satellite_sun_sensors_model_eclipse.get();
+
+  if (model_eclipse) {
+    auto const &truth_r_eci = truth_satellite_orbit_r_eci->get();
+    auto const &truth_s_eci = truth_satellite_environment_s_eci->get();
+
+    if (lin::dot(truth_s_eci, truth_r_eci) < 0.0) {
+      return lin::nans<Vector3>();
+    }
+  }
+
+  /* Currently, we're using noise representation in spherical coordinates.
+   * This is alright for now but really we should update this model to simulate
+   * each photodiode and the sun vector calculation performed on the ADCS
+   * computer.
+   *
+   * TODO: Update the sun sensor model to include individual diodes.
+   */
+  auto const &truth_s_body = truth_satellite_environment_s_body->get();
+  auto const &sigma = sensors_satellite_sun_sensors_s_sigma.get();
+
+  /* 1. Generate a quaternion transform from the x axis to the true sun vector
+   *    in the body frame.
+   */
+  Vector4 q;
+  gnc::utl::vec_rot_to_quat(truth_s_body, Vector3({1.0, 0.0, 0.0}), q);
+
+  /* 2. Generate sensors noise values in spherical coordinates centered about
+   *    the x axis.
+   */
+  auto const phi = sigma(0) * _randoms.gaussian();
+  auto const theta = gnc::constant::pi / 2.0 + sigma(1) * _randoms.gaussian();
+
+  /* 3. Reconstruct the measured sun vector relative to the x axis (which is the
+   *    true sun vector given step 1).
+   */
+  Vector3 s;
+  s(0) = lin::sin(theta) * lin::cos(phi);
+  s(1) = lin::sin(theta) * lin::sin(phi);
+  s(2) = lin::cos(theta);
+
+  /* 4. Rotate the measured sun vector back into the body frame.
+   */
+  gnc::utl::rotate_frame(q, s);
+  return s;
+}
+
+Vector3 SunSensors::sensors_satellite_sun_sensors_s_error() const {
   auto const &truth_s = truth_satellite_environment_s_body->get();
+  auto const &s = Super::sensors_satellite_sun_sensors_s.get();
 
-  // TODO : Implement a configurable sun sensor model
-  return truth_s;
+  return s - truth_s;
 }
-}  // namespace psim
+} // namespace psim