Added attitude estimator simulation and fc model

BUILD.bazel

@@ -37,10 +37,10 @@ psim_cc_library(
     visibility = ["//visibility:public"],
 )
 
-# Builds the PSim sensor models
+# Builds the PSim utilities models
 psim_autocoded_cc_library(
-    name = "sensors",
-    deps = ["@lin//:lin", "//:gnc", "//:psim_core", "//:psim_utilities"],
+    name = "utilities",
+    deps = ["@lin//:lin", "//:gnc", "//:psim_core"],
     visibility = ["//visibility:public"],
 )
 
@@ -51,19 +51,26 @@ psim_autocoded_cc_library(
     visibility = ["//visibility:public"],
 )
 
-# Builds the PSim utilities models
+# Builds the PSim sensor models
 psim_autocoded_cc_library(
-    name = "utilities",
-    deps = ["@lin//:lin", "//:gnc", "//:psim_core"],
+    name = "sensors",
+    deps = ["@lin//:lin", "//:gnc", "//:psim_core", "//:psim_utilities"],
+    visibility = ["//visibility:public"],
+)
+
+# Builds the PSim flight computer models
+psim_autocoded_cc_library(
+    name = "fc",
+    deps = ["@lin//:lin", "//:gnc", "//:psim_core", "//:psim_utilities"],
     visibility = ["//visibility:public"],
 )
 
 # Builds all PSim models deemed "simulation" worthy.
 psim_cc_library(
     name = "simulations",
     deps = [
-        "//:psim_core", "//:psim_sensors", "//:psim_truth",
-        "//:psim_utilities"
+        "//:psim_core", "//:psim_utilities", "//:psim_truth", "//:psim_sensors",
+        "//:psim_fc"
     ],
     visibility = ["//visibility:public"],
 )

config/plots/fc/attitude.yml

@@ -0,0 +1,8 @@
+
+# Attitude estimator's state covariance norm over time.
+- x: truth.t.s
+  y: fc.leader.attitude.is_valid
+
+# Log of the attitude estimator's state covariance norm over time.
+- x: truth.t.s
+  y: fc.leader.attitude.log_P_fro

include/psim/fc/attitude_estimator.hpp

@@ -0,0 +1,64 @@
+//
+// MIT License
+//
+// Copyright (c) 2020 Pathfinder for Autonomous Navigation (PAN)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+/** @file psim/fc/attitude_estimator.hpp
+ *  @author Kyle Krol
+ */
+
+#ifndef PSIM_FC_ATTITUDE_ESTIMATOR_HPP_
+#define PSIM_FC_ATTITUDE_ESTIMATOR_HPP_
+
+#include <psim/fc/attitude_estimator.yml.hpp>
+
+#include <gnc/attitude_estimator.hpp>
+
+namespace psim {
+
+class AttitudeEstimator
+  : public AttitudeEstimatorInterface<AttitudeEstimator> {
+ private:
+  typedef AttitudeEstimatorInterface<AttitudeEstimator> Super;
+
+  gnc::AttitudeEstimatorState _attitude_state;
+  gnc::AttitudeEstimatorData _attitude_data;
+  gnc::AttitudeEstimate _attitude_estimate;
+
+  void _set_attitude_outputs();
+
+ public:
+  using Super::AttitudeEstimatorInterface;
+
+  AttitudeEstimator() = delete;
+  virtual ~AttitudeEstimator() = default;
+
+  virtual void add_fields(State &state) override;
+  virtual void step() override;
+
+  Vector3 fc_satellite_attitude_w_bias_sigma() const;
+  Real fc_satellite_attitude_P_fro() const;
+  Real fc_satellite_attitude_log_P_fro() const;
+};
+} // namespace psim
+
+#endif

include/psim/fc/attitude_estimator.yml

@@ -0,0 +1,51 @@
+
+name: AttitudeEstimatorInterface
+type: Model
+comment: >
+    Interface for how the flight computer's attitude estimator will interact
+    with the simulation in PSim standalone.
+
+args:
+    - satellite
+    - orbit
+
+adds:
+    - name: "fc.{satellite}.attitude.is_valid"
+      type: Integer
+      comment: >
+        Flag specifying whether or not the estimator is currently initialized.
+        Zero indicated the estimator isn't initialized while one indicates it
+        has been initialized.
+    - name: "fc.{satellite}.attitude.q.body_eci"
+      type: Vector4
+      comment: >
+        Current attitude estimate for the satellite. This value is represented
+        as a quaternion.
+    - name: "fc.{satellite}.attitude.w.bias"
+      type: Vector3
+      comment: >
+        Current angular rate bias estimate for the satellite.
+    - name: "fc.{satellite}.attitude.w.bias.sigma"
+      type: Lazy Vector3
+      comment: >
+        One sigma bounds on the estimate of the gyroscope bias.
+    - name: "fc.{satellite}.attitude.P_fro"
+      type: Lazy Real
+      comment: >
+        Frobenius norm of the estimate's state covariance.
+    - name: "fc.{satellite}.attitude.log_P_fro"
+      type: Lazy Real
+      comment: >
+        Log of the Frobenius norm of the estimate's state covariance.
+
+gets:
+    - name: "truth.t.s"
+      type: Real
+    - name: "{orbit}.r.ecef"
+      type: Vector3
+    - name: "sensors.{satellite}.gyroscope.w"
+      type: Vector3
+    - name: "sensors.{satellite}.sun_sensors.s"
+      type: Vector3
+    - name: "sensors.{satellite}.magnetometer.b"
+      type: Vector3

include/psim/simulations/attitude_estimator_test.hpp

@@ -0,0 +1,50 @@
+//
+// MIT License
+//
+// Copyright (c) 2020 Pathfinder for Autonomous Navigation (PAN)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+/** @file psim/simulation/attitude_estimator_test.hpp
+ *  @author Kyle Krol
+ */
+
+#ifndef PSIM_SIMULATIONS_ATTITUDE_ESTIMATOR_TEST_HPP_
+#define PSIM_SIMULATIONS_ATTITUDE_ESTIMATOR_TEST_HPP_
+
+#include <psim/core/configuration.hpp>
+#include <psim/core/model_list.hpp>
+
+namespace psim {
+
+/** @brief Models attitude and orbital dynamics of a single spacecraft in order
+ *         to test the attitude estimator.
+ */
+class AttitudeEstimatorTestGnc : public ModelList {
+ public:
+  AttitudeEstimatorTestGnc() = delete;
+  virtual ~AttitudeEstimatorTestGnc() = default;
+
+  AttitudeEstimatorTestGnc(
+      RandomsGenerator &randoms, Configuration const &config);
+};
+} // namespace psim
+
+#endif

python/psim/_psim.cpp

@@ -184,6 +184,7 @@ static void py_assign(psim::StateFieldWritableBase &field, T const &value) {
       })
 
 void py_simulation(py::module &m) {
+  PY_SIMULATION(AttitudeEstimatorTestGnc);
   PY_SIMULATION(SingleAttitudeOrbitGnc);
   PY_SIMULATION(SingleOrbitGnc);
   PY_SIMULATION(DualAttitudeOrbitGnc);

python/psim/sims.py

@@ -2,6 +2,7 @@
 """
 
 from _psim import (
+    AttitudeEstimatorTestGnc,
     DualAttitudeOrbitGnc,
     DualOrbitGnc,
     SingleAttitudeOrbitGnc,

src/psim/fc/attitude_estimator.cpp

@@ -0,0 +1,97 @@
+//
+// MIT License
+//
+// Copyright (c) 2020 Pathfinder for Autonomous Navigation (PAN)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+/** @file psim/fc/attitude_estimator.cpp
+ *  @author Kyle Krol
+ */
+
+#include <psim/fc/attitude_estimator.hpp>
+
+#include <lin/math.hpp>
+#include <lin/queries.hpp>
+
+namespace psim {
+
+void AttitudeEstimator::_set_attitude_outputs() {
+  fc_satellite_attitude_is_valid.get() = _attitude_estimate.is_valid;
+  fc_satellite_attitude_q_body_eci.get() = _attitude_estimate.q_body_eci;
+  fc_satellite_attitude_w_bias.get() = _attitude_estimate.gyro_bias;
+}
+
+void AttitudeEstimator::add_fields(State &state) {
+  this->Super::add_fields(state);
+
+  // This ensures upon simulation construction the state fields hold proper
+  // values.
+  _set_attitude_outputs();
+}
+
+void AttitudeEstimator::step() {
+  this->Super::step();
+
+  auto const &t = truth_t_s->get();
+  auto const &r = orbit_r_ecef->get();
+  auto const &b = sensors_satellite_magnetometer_b->get();
+  auto const &s = sensors_satellite_sun_sensors_s->get();
+  auto const &w = sensors_satellite_gyroscope_w->get();
+
+  // If the current estimate is already valid, update it.
+  if (_attitude_state.is_valid) {
+    _attitude_data = gnc::AttitudeEstimatorData();
+    _attitude_data.t = t;
+    _attitude_data.r_ecef = r;
+    _attitude_data.b_body = b;
+    _attitude_data.s_body = s;
+    _attitude_data.w_body = w;
+
+    _attitude_estimate = gnc::AttitudeEstimate();
+    gnc::attitude_estimator_update(
+        _attitude_state, _attitude_data, _attitude_estimate);
+  }
+  // Attempt to reset the current estimate if it isn't valid.
+  else {
+    if (lin::all(lin::isfinite(t)) && lin::all(lin::isfinite(r)) &&
+        lin::all(lin::isfinite(b)) && lin::all(lin::isfinite(s)))
+      gnc::attitude_estimator_reset(_attitude_state, t, r, b, s);
+  }
+
+  _set_attitude_outputs();
+}
+
+Vector3 AttitudeEstimator::fc_satellite_attitude_w_bias_sigma() const {
+  auto const &P = _attitude_state.P;
+
+  return {lin::sqrt(P(3, 3)), lin::sqrt(P(4, 4)), lin::sqrt(P(5, 5))};
+}
+
+Real AttitudeEstimator::fc_satellite_attitude_P_fro() const {
+  return lin::fro(_attitude_state.P);
+}
+
+Real AttitudeEstimator::fc_satellite_attitude_log_P_fro() const {
+  auto const &P_fro = this->Super::fc_satellite_attitude_P_fro.get();
+
+  return lin::log10(P_fro);
+}
+} // namespace psim

src/psim/simulations/attitude_estimator_test.cpp

@@ -0,0 +1,42 @@
+//
+// MIT License
+//
+// Copyright (c) 2020 Pathfinder for Autonomous Navigation (PAN)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+/** @file psim/simulations/attitude_estimator_test.cpp
+ *  @author Kyle Krol
+ */
+
+#include <psim/simulations/attitude_estimator_test.hpp>
+
+#include <psim/fc/attitude_estimator.hpp>
+#include <psim/simulations/single_attitude_orbit.hpp>
+
+namespace psim {
+
+AttitudeEstimatorTestGnc::AttitudeEstimatorTestGnc(
+    RandomsGenerator &randoms, Configuration const &config)
+  : ModelList(randoms) {
+  add<SingleAttitudeOrbitGnc>(randoms, config);
+  add<AttitudeEstimator>(randoms, config, "leader", "truth.leader.orbit");
+}
+} // namespace psim