GPS, Gyroscope, and Magnetometer Sensor Models (#278)

config/parameters/sensors/base.txt

@@ -1,10 +1,28 @@
-# Basic configuration information shared across all sensor model configurations.
+# Basic configuration information shared across all sensor model initial
+# conditions.
+#
+# All configuration values for the gyroscope and magnetometer were
+# experimentally determined. See the following folder in the PAN team google
+# drive:
+#   System Level > Integration and Testing > Sensor Charactarization
 #
 
 # Leader spacecraft base sensor configuration
 
-sensors.leader.gyroscope.bias  0.02 0.01 -0.03
+sensors.leader.gps.r.sigma  0.0 0.0 0.0
+
+sensors.leader.gyroscope.w.bias        0.02    0.01    -0.03
+sensors.leader.gyroscope.w.bias.sigma  1.00e-6 1.00e-6  1.00e-6
+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
 
 # Follower spacecraft base sensor configuration
 
-sensors.follower.gyroscope.bias  -0.01 0.00 0.05
+sensors.follower.gps.r.sigma  0.0 0.0 0.0
+
+sensors.follower.gyroscope.w.bias        -0.01    0.01    0.04
+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

config/plots/sensors/gyroscope.yml

@@ -5,7 +5,7 @@
 
 # Gyroscope bias over time.
 - x: truth.t.s
-  y: [sensors.leader.gyroscope.bias.x, sensors.leader.gyroscope.bias.y, sensors.leader.gyroscope.bias.z]
+  y: [sensors.leader.gyroscope.w.bias.x, sensors.leader.gyroscope.w.bias.y, sensors.leader.gyroscope.w.bias.z]
 
 # Gyroscope measurement error over time.
 - x: truth.t.s

include/psim/sensors/gps_no_attitude.yml

@@ -8,6 +8,12 @@ comment: >
 args:
     - satellite
 
+params:
+    - name: "sensors.{satellite}.gps.r.sigma"
+      type: Vector3
+      comment: >
+        Standard deviation of the position reading from the GPS.
+
 adds:
     - name: "sensors.{satellite}.gps.r"
       type: Lazy Vector3

include/psim/sensors/gyroscope.yml

@@ -8,12 +8,24 @@ comment: >
 args:
     - satellite
 
+params:
+    - name: "sensors.{satellite}.gyroscope.w.sigma"
+      type: Vector3
+      comment: >
+        Standard deviation of the angular rate reading from the gyroscope. This
+        is white noise independent of the bias model.
+    - name: "sensors.{satellite}.gyroscope.w.bias.sigma"
+      type: Vector3
+      comment: >
+        Standard deviation of the noise integrated to simulate the gyroscope
+        bias' random walk over time.
+
 adds:
     - name: "sensors.{satellite}.gyroscope.w"
       type: Lazy Vector3
       comment: >
           Angular rate reported by the gyroscope in the body frame.
-    - name: "sensors.{satellite}.gyroscope.bias"
+    - name: "sensors.{satellite}.gyroscope.w.bias"
       type: Initialized Vector3
       comment: >
           Bias exhibited by the gyroscope readings in the body frame. This
@@ -25,5 +37,7 @@ adds:
           Error in the angular rate reported by the gyroscope in the body frame.
 
 gets:
+    - name: "truth.dt.s"
+      type: Real
     - name: "truth.{satellite}.attitude.w"
       type: Vector3

include/psim/sensors/magnetometer.yml

@@ -8,6 +8,12 @@ comment: >
 args:
     - satellite
 
+params:
+    - name: "sensors.{satellite}.magnetometer.b.sigma"
+      type: Vector3
+      comment: >
+        Standard deviation of the magnetic field reading from the magnetometer.
+
 adds:
     - name: "sensors.{satellite}.magnetometer.b"
       type: Lazy Vector3

src/psim/sensors/gps_no_attitude.cpp

@@ -32,9 +32,9 @@ namespace psim {
 
 Vector3 GpsNoAttitude::sensors_satellite_gps_r() const {
   auto const &truth_r_ecef = truth_satellite_orbit_r_ecef->get();
+  auto const &sigma = sensors_satellite_gps_r_sigma.get();
 
-  // TODO : Implement a configurable white noise model
-  return truth_r_ecef;
+  return truth_r_ecef + lin::multiply(sigma, lin::gaussians<Vector3>(_randoms));
 }
 
 Vector3 GpsNoAttitude::sensors_satellite_gps_r_error() const {

src/psim/sensors/gyroscope.cpp

@@ -33,15 +33,19 @@ namespace psim {
 void Gyroscope::step() {
   this->Super::step();
 
-  // TODO : Implement gyroscope bias drift
+  auto       &bias = sensors_satellite_gyroscope_w_bias.get();
+  auto const &bias_sigma = sensors_satellite_gyroscope_w_bias_sigma.get();
+  auto const &dt = truth_dt_s->get();
+
+  bias = bias + dt * lin::multiply(bias_sigma, lin::gaussians<Vector3>(_randoms));
 }
 
 Vector3 Gyroscope::sensors_satellite_gyroscope_w() const {
   auto const &truth_w = truth_satellite_attitude_w->get();
-  auto const &bias = sensors_satellite_gyroscope_bias.get();
+  auto const &bias = sensors_satellite_gyroscope_w_bias.get();
+  auto const &sigma = sensors_satellite_gyroscope_w_sigma.get();
 
-  // TODO : Implement a white noise model
-  return truth_w + bias;
+  return truth_w + bias + lin::multiply(sigma, lin::gaussians<Vector3>(_randoms));
 }
 
 Vector3 Gyroscope::sensors_satellite_gyroscope_w_error() const {

src/psim/sensors/magnetometer.cpp

@@ -32,9 +32,9 @@ namespace psim {
 
 Vector3 Magnetometer::sensors_satellite_magnetometer_b() const {
   auto const &truth_b = truth_satellite_environment_b_body->get();
+  auto const &sigma = sensors_satellite_magnetometer_b_sigma.get();
 
-  // TODO : Implement a configurable white noise model
-  return truth_b;
+  return truth_b + lin::multiply(sigma, lin::gaussians<Vector3>(_randoms));
 }
 
 Vector3 Magnetometer::sensors_satellite_magnetometer_b_error() const {