diff --git a/BUILD.bazel b/BUILD.bazel index 2275147b..ce216f4a 100644 --- a/BUILD.bazel +++ b/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,10 +51,17 @@ 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"], ) @@ -62,8 +69,8 @@ psim_autocoded_cc_library( 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"], ) diff --git a/config/plots/fc/attitude.yml b/config/plots/fc/attitude.yml new file mode 100644 index 00000000..b7938a42 --- /dev/null +++ b/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 diff --git a/include/psim/fc/attitude_estimator.hpp b/include/psim/fc/attitude_estimator.hpp new file mode 100644 index 00000000..5080c3d9 --- /dev/null +++ b/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 + +#include + +namespace psim { + +class AttitudeEstimator + : public AttitudeEstimatorInterface { + private: + typedef AttitudeEstimatorInterface 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 diff --git a/include/psim/fc/attitude_estimator.yml b/include/psim/fc/attitude_estimator.yml new file mode 100644 index 00000000..ef03824d --- /dev/null +++ b/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 diff --git a/include/psim/simulations/attitude_estimator_test.hpp b/include/psim/simulations/attitude_estimator_test.hpp new file mode 100644 index 00000000..2d044cb6 --- /dev/null +++ b/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 +#include + +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 diff --git a/python/psim/_psim.cpp b/python/psim/_psim.cpp index 52f5a8a7..cef5f486 100644 --- a/python/psim/_psim.cpp +++ b/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); diff --git a/python/psim/sims.py b/python/psim/sims.py index 09533907..6e9eae6c 100644 --- a/python/psim/sims.py +++ b/python/psim/sims.py @@ -2,6 +2,7 @@ """ from _psim import ( + AttitudeEstimatorTestGnc, DualAttitudeOrbitGnc, DualOrbitGnc, SingleAttitudeOrbitGnc, diff --git a/src/psim/fc/attitude_estimator.cpp b/src/psim/fc/attitude_estimator.cpp new file mode 100644 index 00000000..5c7e1062 --- /dev/null +++ b/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 + +#include +#include + +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 diff --git a/src/psim/simulations/attitude_estimator_test.cpp b/src/psim/simulations/attitude_estimator_test.cpp new file mode 100644 index 00000000..26cd45af --- /dev/null +++ b/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 + +#include +#include + +namespace psim { + +AttitudeEstimatorTestGnc::AttitudeEstimatorTestGnc( + RandomsGenerator &randoms, Configuration const &config) + : ModelList(randoms) { + add(randoms, config); + add(randoms, config, "leader", "truth.leader.orbit"); +} +} // namespace psim