From d8324853d2dcf4ade93678d0df9b0bbdb471a242 Mon Sep 17 00:00:00 2001 From: kylekrol <33558436+kylekrol@users.noreply.github.com> Date: Thu, 14 Jan 2021 18:49:31 -0600 Subject: [PATCH] Improved Sun Sensors Model (#288) --- config/parameters/sensors/base.txt | 12 +++++ config/plots/sensors/sun_sensors.yml | 10 +++++ include/psim/sensors/sun_sensors.hpp | 1 + include/psim/sensors/sun_sensors.yml | 23 ++++++++++ src/psim/sensors/sun_sensors.cpp | 67 ++++++++++++++++++++++++++-- 5 files changed, 110 insertions(+), 3 deletions(-) create mode 100644 config/plots/sensors/sun_sensors.yml diff --git a/config/parameters/sensors/base.txt b/config/parameters/sensors/base.txt index 48428aa6..3ad238c3 100644 --- a/config/parameters/sensors/base.txt +++ b/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 diff --git a/config/plots/sensors/sun_sensors.yml b/config/plots/sensors/sun_sensors.yml new file mode 100644 index 00000000..c675a16b --- /dev/null +++ b/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] diff --git a/include/psim/sensors/sun_sensors.hpp b/include/psim/sensors/sun_sensors.hpp index 25931fe6..2f68b444 100644 --- a/include/psim/sensors/sun_sensors.hpp +++ b/include/psim/sensors/sun_sensors.hpp @@ -44,6 +44,7 @@ class SunSensors : public SunSensorsInterface { virtual ~SunSensors() = default; Vector3 sensors_satellite_sun_sensors_s() const; + Vector3 sensors_satellite_sun_sensors_s_error() const; }; } // namespace psim diff --git a/include/psim/sensors/sun_sensors.yml b/include/psim/sensors/sun_sensors.yml index cbedaa47..bcd1fade 100644 --- a/include/psim/sensors/sun_sensors.yml +++ b/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 diff --git a/src/psim/sensors/sun_sensors.cpp b/src/psim/sensors/sun_sensors.cpp index b8a6e8d7..e9ec37fc 100644 --- a/src/psim/sensors/sun_sensors.cpp +++ b/src/psim/sensors/sun_sensors.cpp @@ -28,12 +28,73 @@ #include +#include +#include + +#include +#include +#include + 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(); + } + } + + /* 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