From dfa86190955911457917c804133c2019ba982dac Mon Sep 17 00:00:00 2001 From: kkrol27 <33558436+kkrol27@users.noreply.github.com> Date: Fri, 4 Dec 2020 14:37:18 -0600 Subject: [PATCH] GPS, Gyroscope, and Magnetometer Sensor Models (#278) --- config/parameters/sensors/base.txt | 24 +++++++++++++++++++++--- config/plots/sensors/gyroscope.yml | 2 +- include/psim/sensors/gps_no_attitude.yml | 6 ++++++ include/psim/sensors/gyroscope.yml | 16 +++++++++++++++- include/psim/sensors/magnetometer.yml | 6 ++++++ src/psim/sensors/gps_no_attitude.cpp | 4 ++-- src/psim/sensors/gyroscope.cpp | 12 ++++++++---- src/psim/sensors/magnetometer.cpp | 4 ++-- 8 files changed, 61 insertions(+), 13 deletions(-) diff --git a/config/parameters/sensors/base.txt b/config/parameters/sensors/base.txt index 6fec6638..48428aa6 100644 --- a/config/parameters/sensors/base.txt +++ b/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 diff --git a/config/plots/sensors/gyroscope.yml b/config/plots/sensors/gyroscope.yml index 100b6360..e3e68154 100644 --- a/config/plots/sensors/gyroscope.yml +++ b/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 diff --git a/include/psim/sensors/gps_no_attitude.yml b/include/psim/sensors/gps_no_attitude.yml index e045f163..38b7168e 100644 --- a/include/psim/sensors/gps_no_attitude.yml +++ b/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 diff --git a/include/psim/sensors/gyroscope.yml b/include/psim/sensors/gyroscope.yml index 94530e44..c8ad8aa9 100644 --- a/include/psim/sensors/gyroscope.yml +++ b/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 diff --git a/include/psim/sensors/magnetometer.yml b/include/psim/sensors/magnetometer.yml index be106480..ff0060e7 100644 --- a/include/psim/sensors/magnetometer.yml +++ b/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 diff --git a/src/psim/sensors/gps_no_attitude.cpp b/src/psim/sensors/gps_no_attitude.cpp index 1d62d27e..0bff3dba 100644 --- a/src/psim/sensors/gps_no_attitude.cpp +++ b/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(_randoms)); } Vector3 GpsNoAttitude::sensors_satellite_gps_r_error() const { diff --git a/src/psim/sensors/gyroscope.cpp b/src/psim/sensors/gyroscope.cpp index ab530ddd..e577c913 100644 --- a/src/psim/sensors/gyroscope.cpp +++ b/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(_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(_randoms)); } Vector3 Gyroscope::sensors_satellite_gyroscope_w_error() const { diff --git a/src/psim/sensors/magnetometer.cpp b/src/psim/sensors/magnetometer.cpp index 199b694f..1ed42662 100644 --- a/src/psim/sensors/magnetometer.cpp +++ b/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(_randoms)); } Vector3 Magnetometer::sensors_satellite_magnetometer_b_error() const {