diff --git a/config/plots/fc/relative_orbit.yml b/config/plots/fc/relative_orbit.yml new file mode 100644 index 00000000..dcb1b087 --- /dev/null +++ b/config/plots/fc/relative_orbit.yml @@ -0,0 +1,42 @@ + +# Relative orbit estimator's validity over time. +- type: Plot2D + x: truth.t.s + y: fc.leader.relative_orbit.is_valid + +# Relative orbit estimator's relative position estimate and error in ECEF. +- type: Plot2D + x: truth.t.s + y: [fc.leader.relative_orbit.dr.x, fc.leader.relative_orbit.dr.y, fc.leader.relative_orbit.dr.z] +- type: Plot2D + x: truth.t.s + y: [fc.leader.relative_orbit.dr.error.x, fc.leader.relative_orbit.dr.error.y, fc.leader.relative_orbit.dr.error.z] + +# Relative orbit estimator's relative velocity estimate and error in ECEF. +- type: Plot2D + x: truth.t.s + y: [fc.leader.relative_orbit.dv.x, fc.leader.relative_orbit.dv.y, fc.leader.relative_orbit.dv.z] +- type: Plot2D + x: truth.t.s + y: [fc.leader.relative_orbit.dv.error.x, fc.leader.relative_orbit.dv.error.y, fc.leader.relative_orbit.dv.error.z] + +# Relative orbit estimator performance in terms of hill frame position and +# velocity measurements for the other spacecraft. +- type: PlotEstimate + x: truth.t.s + y: fc.leader.relative_orbit.r.hill.x +- type: PlotEstimate + x: truth.t.s + y: fc.leader.relative_orbit.r.hill.y +- type: PlotEstimate + x: truth.t.s + y: fc.leader.relative_orbit.r.hill.z +- type: PlotEstimate + x: truth.t.s + y: fc.leader.relative_orbit.v.hill.x +- type: PlotEstimate + x: truth.t.s + y: fc.leader.relative_orbit.v.hill.y +- type: PlotEstimate + x: truth.t.s + y: fc.leader.relative_orbit.v.hill.z diff --git a/include/gnc/relative_orbit_estimate.hpp b/include/gnc/relative_orbit_estimate.hpp new file mode 100644 index 00000000..e5b0892b --- /dev/null +++ b/include/gnc/relative_orbit_estimate.hpp @@ -0,0 +1,313 @@ +// +// 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 gnc/relative_orbit_estimate.hpp + * @author Kyle Krol + */ + +#ifndef GNC_RELATIVE_ORBIT_ESTIMATE_HPP_ +#define GNC_RELATIVE_ORBIT_ESTIMATE_HPP_ + +#include +#include +#include + +#include + +namespace gnc { + +/** @brief Estimate of the "other" spacecraft relative to "this" one. + * + * This class implements a square root Kalman filter using the following state + * space representation: + * + * x = [ r_hill ] + * [ v_hill ] + * + * where r_hill and v_hill are three dimensional vector representing the + * position and velocity of the other spacecraft in this spacecraft's HILL + * frame. + * + * As such, the state covariance is represented as an upper triangular matrix + * S defined by: + * + * P = transpose(S) S + * + * making S the Cholesky factorization of the state covariance matrix P. + * Throughout this class, all Cholesky factors (also called matrix square + * roots) are assumed to be upper triangular. + * + * Lastly, the nomenclature "this" spacecraft refers to the craft physically + * running the estimator - which is at the center of it's own HILL frame. The + * "other" spacecraft refers to the spacecraft whose relative position and + * velocity is being estimated. + * + * Reference(s): + * - https://space.stackexchange.com/questions/32860/diagram-of-hayabusa-2-in-hill-coordinate-system-what-is-that-exactly-how-to + * - https://en.wikipedia.org/wiki/Cholesky_decomposition + */ +class RelativeOrbitEstimate { + public: + /** @brief Type representing real scalars within the class. + */ + typedef double Real; + + /** @brief Type representing time in nanoseconds within the class. + */ + typedef int32_t Time; + + /** @brief Convenience template for defining lin matrix types within the + * class. + */ + template + using Matrix = lin::Matrix; + + /** @brief Convenience template for defining lin row vector types within the + * class. + */ + template + using RowVector = lin::RowVector; + + /** @brief Convenience template for defining lin vector types within the + * class. + */ + template + using Vector = lin::Vector; + + private: + /** @internal + * + * @brief True is the relative orbit estimate is valid and false otherwise. + */ + bool _valid = false; + + /** @internal + * + * @brief State space representation of the relative orbit estimate. + */ + Vector<6> _x = lin::nans>(); + + /** @internal + * + * @brief Cholesky factorization of the estimate's covariance. + */ + Matrix<6, 6> _sqrtP = lin::nans>(); + + /** @internal + * + * @brief Relative position estimate in ECEF. + */ + Vector<3> _dr_ecef = lin::nans>(); + + /** @internal + * + * @brief Relative velocity estimate in ECEF. + */ + Vector<3> _dv_ecef = lin::nans>(); + + /** @internal + * + * @brief Temporary variabels used during update steps. + */ + Vector<3> _r_ecef, _v_ecef, _r_ecef0, _v_ecef0, _w_hill_ecef0, _w_earth_ecef; + + /** @internal + * + * @brief Another temporary variables used during update steps. + */ + Matrix<3, 3> _Q_hill_ecef0; + + /** @internal + * + * @brief Calculates the state transition matrix from the Clohessy Wiltshire + * equations. + * + * @param dt_ns Timestep (ns). + * @param n Mean motion for this spacecraft (rad/s). + * + * @return State transition matrix. + */ + static Matrix<6, 6> _state_transition_matrix(Time dt_ns, Real n); + + /** @internal + * + * @brief Populates the temporary variables used during update steps. + * + * @param w_earth_ecef Earth's angular rate in ECEF (rad/s). + * @param r_ecef Position of this spacecraft in ECEF (m). + * @param v_ecef Velocity of this spacecraft in ECEF (m). + */ + void _inputs(Vector<3> const &w_earth_ecef, Vector<3> const &r_ecef, + Vector<3> const &v_ecef); + + /** @internal + * + * @brief Square root Kalman filter prediction step. + * + * @param dt_ns Timestep (ns) + * @param n Mean motion for this spacecraft (rad/s). + * @param sqrtQ Cholesky factorization of the process noise. + * + * Steps the member variables `_x` and `_sqrtP` forward in time. + */ + void _predict(Time dt_ns, Real n, Matrix<6, 6> const &sqrtQ); + + /** @internal + * + * @brief Square root Kalman filter update step. + * + * @param dr_hill Relative position measurement in the HILL frame (m). + * @param sqrtR Cholesky factorization of the sensor noise. + */ + void _update(Vector<3> const &dr_hill, Matrix<3, 3> const &sqrtR); + + /** @internal + * + * @brief Sets filter outputs. + * + * The temporary variables, `_x`, and `_sqrtP` must all be set. + */ + void _outputs(); + + /** @internal + * + * @brief Tests whether the current estimate is valid. + * + * If it's not valid, all outputs will be set to `nan` and the `_valid` flag + * to false. + */ + void _check_validity(); + + public: + RelativeOrbitEstimate() = default; + RelativeOrbitEstimate(RelativeOrbitEstimate const &) = default; + RelativeOrbitEstimate(RelativeOrbitEstimate &&) = default; + RelativeOrbitEstimate &operator=(RelativeOrbitEstimate const &) = default; + RelativeOrbitEstimate &operator=(RelativeOrbitEstimate &&) = default; + + /** @brief Constructs a new relative orbit estimate and checks validity. + * + * @param w_earth_ecef Earth's angular rate in ECEF (rad/s). + * @param r_ecef Position of this spacecraft in ECEF (m). + * @param v_ecef Velocity of this spacecraft in ECEF (m/s). + * @param dr_ecef Other spacecraft's relative position in ECEF (m). + * @param dv_ecef Other spacecraft's relative velocity in ECEF (m/s). + * @param S Cholesky factorization of the initial state + * covariance. + * + * The initial estimate is constructed using the relative position and + * velocity of the other spacecraft along with the initial state covariance. + * Validity is then checked. + */ + RelativeOrbitEstimate(Vector<3> const &w_earth_ecef, Vector<3> const &r_ecef, + Vector<3> const &v_ecef, Vector<3> const &dr_ecef, + Vector<3> const &dv_ecef, Matrix<6, 6> const &S); + + /** @return True if the estimate is valid and false otherwise. + */ + inline bool valid() const { + return _valid; + } + + /** @return Relative position estimate of the other spacecraft in ECEF (m). + * + * Will be set to NaNs if the estimate isn't valid. + */ + inline Vector<3> dr_ecef() const { + return _dr_ecef; + } + + /** @return Position estimate of the other spacecraft in the HILL frame (m). + * + * Will be set to NaNs if the estimate isn't valid. + */ + inline Vector<3> r_hill() const { + return lin::ref>(_x, 0, 0); + } + + /** @return Relative velocity estimate of the other spacecraft in ECEF (m/s). + * + * Will be set to NaNs if the estimate isn't valid. + */ + inline Vector<3> dv_ecef() const { + return _dv_ecef; + } + + /** @return Velocity estimate of the other spacecraft in the HILL frame (m/s). + * + * Will be set to NaNs if the estimate isn't valid. + */ + inline Vector<3> v_hill() const { + return lin::ref>(_x, 3, 0); + } + + /** @return Cholesky factorization of the state covariance. + * + * This matrix is upper triangular and will be set to NaNs if the estimate + * isn't valid. + */ + inline Matrix<6, 6> S() const { + return _sqrtP; + } + + /** @brief Prediction only step updating the estimate. + * + * @param dt_ns Timestep (ns). + * @param w_earth_ecef Earth's angular rate in ECEF (rad/s). + * @param r_ecef This satellite's position in ECEF (m). + * @param v_ecef This satellite's velocity in ECEF (m/s). + * @param sqrtQ Cholesky factorization of the process noise. + * + * Intended to be called when no sensor measurement is available. Validity is + * checked after the step is performed. + */ + void update(Time dt_ns, Vector<3> const &w_earth_ecef, + Vector<3> const &r_ecef, Vector<3> const &v_ecef, + Matrix<6, 6> const &sqrtQ); + + /** @brief Prediction and update step updating the estimate. + * + * @param dt_ns Timestep (ns). + * @param w_earth_ecef Earth's angular rate in ECEF (rad/s). + * @param r_ecef This satellite's position in ECEF (m). + * @param v_ecef This satellite's velocity in ECEF (m/s). + * @param dr_ecef Other spacecraft's relative position measurement in + * ECEF (m). + * @param sqrtQ Cholesky factorization of the process noise. + * @param sqrtR Cholesky factorization of the sensor noise. + * + * Intended to be called when a sensor measurement is available. Validity is + * checked after the step is performed. + * + * WARNING: The relative position read in here is antiparallel to the reading + * CDGPS will provide through Piski. + */ + void update(Time dt_ns, Vector<3> const &w_earth_ecef, + Vector<3> const &r_ecef, Vector<3> const &v_ecef, + Vector<3> const &dr_ecef, Matrix<6, 6> const &sqrtQ, + Matrix<3, 3> const &sqrtR); +}; +} // namespace gnc + +#endif diff --git a/include/psim/fc/relative_orbit_estimator.hpp b/include/psim/fc/relative_orbit_estimator.hpp new file mode 100644 index 00000000..a3e68b0d --- /dev/null +++ b/include/psim/fc/relative_orbit_estimator.hpp @@ -0,0 +1,66 @@ +// +// 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_RELATIVE_ORBIT_ESTIMATOR_HPP_ +#define PSIM_FC_RELATIVE_ORBIT_ESTIMATOR_HPP_ + +#include + +#include + +#include +#include + +namespace psim { + +class RelativeOrbitEstimator : public RelativeOrbitEstimatorInterface { + private: + typedef RelativeOrbitEstimatorInterface Super; + + Vector3 previous_dr = lin::nans(); + gnc::RelativeOrbitEstimate estimate; + + void _set_relative_orbit_outputs(); + + public: + using Super::RelativeOrbitEstimatorInterface; + + RelativeOrbitEstimator() = delete; + virtual ~RelativeOrbitEstimator() = default; + + virtual void add_fields(State &state) override; + virtual void step() override; + + Vector3 fc_satellite_relative_orbit_dr_error() const; + Vector3 fc_satellite_relative_orbit_r_hill_error() const; + Vector3 fc_satellite_relative_orbit_dv_error() const; + Vector3 fc_satellite_relative_orbit_v_hill_error() const; +}; +} // namespace psim + +#endif diff --git a/include/psim/fc/relative_orbit_estimator.yml b/include/psim/fc/relative_orbit_estimator.yml new file mode 100644 index 00000000..9a00b837 --- /dev/null +++ b/include/psim/fc/relative_orbit_estimator.yml @@ -0,0 +1,84 @@ + +name: RelativeOrbitEstimatorInterface +type: Model +comment: > + Interface for how the flight computer's relative orbit estimator will + interact with the simulation in PSim standalone. + +args: + - satellite + - other + +adds: + - name: "fc.{satellite}.relative_orbit.is_valid" + type: Integer + comment: > + Flag specifying whether or not the estimator is currently initialized. + - name: "fc.{satellite}.relative_orbit.dr" + type: Vector3 + comment: > + Estimate for the relative position of the other satellite in ECEF. + - name: "fc.{satellite}.relative_orbit.dr.error" + type: Lazy Vector3 + comment: > + Estimate error for the relative position of the other satellite in + ECEF. + - name: "fc.{satellite}.relative_orbit.r.hill" + type: Vector3 + comment: > + Estimate for the relative position of the other satellite in the HILL + frame. + - name: "fc.{satellite}.relative_orbit.r.hill.error" + type: Lazy Vector3 + comment: > + Estimate error for the relative position of the other satellite in the + HILL frame. + - name: "fc.{satellite}.relative_orbit.r.hill.sigma" + type: Vector3 + comment: > + Estimate error one sigma bounds for the relative position of the other + satellite in the HILL frame. + - name: "fc.{satellite}.relative_orbit.dv" + type: Vector3 + comment: > + Estimate for the relative position of the other satellite in ECEF. + - name: "fc.{satellite}.relative_orbit.dv.error" + type: Lazy Vector3 + comment: > + Estimate error for the relative position of the other satellite in + ECEF. + - name: "fc.{satellite}.relative_orbit.v.hill" + type: Vector3 + comment: > + Estimate for the relative position of the other satellite in the HILL + frame. + - name: "fc.{satellite}.relative_orbit.v.hill.error" + type: Lazy Vector3 + comment: > + Estimate error for the relative position of the other satellite in the + HILL frame. + - name: "fc.{satellite}.relative_orbit.v.hill.sigma" + type: Vector3 + comment: > + Estimate error one sigma bounds for the relative position of the other + satellite in the HILL frame. + +gets: + - name: "truth.dt.ns" + type: Integer + - name: "truth.earth.w" + type: Vector3 + - name: "truth.{satellite}.orbit.r.ecef" + type: Vector3 + - name: "truth.{satellite}.orbit.v.ecef" + type: Vector3 + - name: "truth.{satellite}.hill.dr" + type: Vector3 + - name: "truth.{satellite}.hill.dv" + type: Vector3 + - name: "truth.{other}.orbit.r.ecef" + type: Vector3 + - name: "truth.{other}.orbit.v.ecef" + type: Vector3 + - name: "sensors.{satellite}.cdgps.dr" + type: Vector3 diff --git a/include/psim/simulations/relative_orbit_estimator_test.hpp b/include/psim/simulations/relative_orbit_estimator_test.hpp new file mode 100644 index 00000000..53292b52 --- /dev/null +++ b/include/psim/simulations/relative_orbit_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/relative_orbit_estimator_test.hpp + * @author Kyle Krol + */ + +#ifndef PSIM_SIMULATIONS_RELATIVE_ORBIT_ESTIMATOR_TEST_HPP_ +#define PSIM_SIMULATIONS_RELATIVE_ORBIT_ESTIMATOR_TEST_HPP_ + +#include +#include + +namespace psim { + +/** @brief Models orbital dynamics for two spacecraft in order to test the + * relative orbit estimator. + */ +class RelativeOrbitEstimatorTest : public ModelList { + public: + RelativeOrbitEstimatorTest() = delete; + virtual ~RelativeOrbitEstimatorTest() = default; + + RelativeOrbitEstimatorTest( + RandomsGenerator &randoms, Configuration const &config); +}; +} // namespace psim + +#endif diff --git a/python/psim/_psim.cpp b/python/psim/_psim.cpp index 0251a986..3553b83f 100644 --- a/python/psim/_psim.cpp +++ b/python/psim/_psim.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include @@ -190,6 +191,7 @@ void py_simulation(py::module &m) { PY_SIMULATION(SingleAttitudeOrbitGnc); PY_SIMULATION(SingleOrbitGnc); PY_SIMULATION(OrbOrbitEstimatorTest); + PY_SIMULATION(RelativeOrbitEstimatorTest); PY_SIMULATION(DualAttitudeOrbitGnc); PY_SIMULATION(DualOrbitGnc); } diff --git a/python/psim/sims.py b/python/psim/sims.py index da933f78..697b5d21 100644 --- a/python/psim/sims.py +++ b/python/psim/sims.py @@ -6,6 +6,7 @@ DualAttitudeOrbitGnc, DualOrbitGnc, OrbOrbitEstimatorTest, + RelativeOrbitEstimatorTest, SingleAttitudeOrbitGnc, SingleOrbitGnc, ) diff --git a/src/gnc/relative_orbit_estimate.cpp b/src/gnc/relative_orbit_estimate.cpp new file mode 100644 index 00000000..e3f37fae --- /dev/null +++ b/src/gnc/relative_orbit_estimate.cpp @@ -0,0 +1,230 @@ +// +// 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 gnc_relative_orbit_estimator.cpp + * @author Kyle Krol + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace gnc { + +RelativeOrbitEstimate::Matrix<6, 6> +RelativeOrbitEstimate::_state_transition_matrix(Time dt_ns, Real n) { + static constexpr Real zero = 0.0; + static constexpr Real one = 1.0; + static constexpr Real two = 2.0; + static constexpr Real three = 3.0; + static constexpr Real four = 4.0; + static constexpr Real six = 6.0; + + /* State transition matrix using the Clohessy Wiltshire equations: + * + * F = [ Phi_rr(dt), Phi_rv(dt) ] + * [ Phi_vr(dt), Phi_vv(dt) ]. + * + * Reference(s): + * - https://en.wikipedia.org/wiki/Clohessy%E2%80%93Wiltshire_equations + */ + Real const dt = static_cast(dt_ns) * Real(1.0e-9); + Real const nt = n * dt; + Real const snt = lin::sin(nt); + Real const cnt = lin::cos(nt); + + return { + four - three * cnt, zero, zero, snt / n, two * (one - cnt) / n, zero, + six * (snt - nt), one, zero, two * (cnt - one) / n, (four * snt - three * nt) / n, zero, + zero, zero, cnt, zero, zero, snt / n, + three * n * snt, zero, zero, cnt, two * snt, zero, + six * n * (cnt - one), zero, zero, -two * snt, four * cnt - three, zero, + zero, zero, -n * snt, zero, zero, cnt + }; +} + +void RelativeOrbitEstimate::_inputs(Vector<3> const &w_earth_ecef, + Vector<3> const &r_ecef, Vector<3> const &v_ecef) { + // This satellites position and velocity in ecef. + _r_ecef = r_ecef; + _v_ecef = v_ecef; + + // This satellites position and velocity in ecef0. + _r_ecef0 = r_ecef; + _v_ecef0 = v_ecef + lin::cross(w_earth_ecef, r_ecef); + + // Angular rate of this satellite's hill frame. + _w_hill_ecef0 = lin::cross(_r_ecef0, _v_ecef0) / lin::fro(_r_ecef0); + + // Transformation from ecef0 to this satellite's hill frame. + utl::dcm(_Q_hill_ecef0, _r_ecef0, _v_ecef0); + + // Angular rate of the Earth + _w_earth_ecef = w_earth_ecef; +} + +void RelativeOrbitEstimate::_predict( + Time dt_ns, Real n, Matrix<6, 6> const &sqrtQ) { + Matrix<6, 6> const F = _state_transition_matrix(dt_ns, n); + + // State prediction step. + _x = (F * _x).eval(); + + /* Covariance prediction step. + * + * This leverages the square root formulation of the EKF covariance + * prediction step: + * + * qr([ S_k|k transpose(F_k) ]) = _ S_k+1|k + * ([ sqrt(Q_k) ]) + */ + Matrix<12, 6> A, _; + lin::ref>(A, 0, 0) = _sqrtP * lin::transpose(F); + lin::ref>(A, 6, 0) = sqrtQ; + lin::qr(A, _, _sqrtP); +} + +void RelativeOrbitEstimate::_update( + Vector<3> const &dr_hill, Matrix<3, 3> const &sqrtR) { + /* This again leverages the square root formulation of the EKF. The + * update step is given by: + * + * qr([ sqrt(R) 0 ]) = _ [ transpose(C) D ] + * ([ S_k+1|k transpose(H) S_k+1|k ]) [ 0 S_k+1|k+1 ] + * qr(B) = _ A. + * + * Note, that in this case here the measurement matrix has the following form: + * + * H = [ I 0 ] + * + * which implies the multiplication involving it's transpose can be done + * symbolically: + * + * S H' = [ S11 S12 ] [ I ] = [ S11 ] + * [ S21 S22 ] [ 0 ] [ S21 ]. + */ + Matrix<9, 9> A; + { + Matrix<9, 9> B, _; + lin::ref>(B, 0, 0) = sqrtR; + lin::ref>(B, 0, 3) = lin::zeros>(); + lin::ref>(B, 3, 0) = lin::ref>(_sqrtP, 0, 0); + lin::ref>(B, 3, 3) = _sqrtP; + lin::qr(B, _, A); + } + + /* Kalman gain calculation from the square root formulation: + * + * K = transpose(D) inverse(C) + * K C = transpose(D) + * transpose(C) tranpose(K) = D. + * + * where transpose(C) is already upper triangular. + */ + Matrix<6, 3> K; + { + Matrix<3, 6> L; + lin::backward_sub(lin::ref>(A, 0, 0), L, lin::ref>(A, 0, 3)); + K = lin::transpose(L); + } + + // Apply the Kalman gain + _x = _x + K * (dr_hill - lin::ref>(_x, 0, 0)).eval(); + _sqrtP = lin::ref>(A, 3, 3); +} + +void RelativeOrbitEstimate::_outputs() { + auto const r_hill = lin::ref>(_x, 0, 0); + auto const v_hill = lin::ref>(_x, 3, 0); + + /* Invert the equations presented in the constructor to go from r_hill, v_hill + * to dr_ecef, dv_ecef. + */ + _dr_ecef = lin::transpose(_Q_hill_ecef0) * r_hill; + _dv_ecef = lin::transpose(_Q_hill_ecef0) * v_hill - lin::cross(_w_earth_ecef - _w_hill_ecef0, _dr_ecef); +} + +void RelativeOrbitEstimate::_check_validity() { + if (lin::any(!lin::isfinite(_x)) || lin::any(!lin::isfinite(_dr_ecef)) || + lin::any(!lin::isfinite(_dv_ecef)) || lin::any(!lin::isfinite(_sqrtP))) { + _valid = false; + _x = lin::nans>(); + _dr_ecef = lin::nans>(); + _dv_ecef = lin::nans>(); + _sqrtP = lin::nans>(); + } else { + _valid = true; + } +} + +RelativeOrbitEstimate::RelativeOrbitEstimate(Vector<3> const &w_earth_ecef, + Vector<3> const &r_ecef, Vector<3> const &v_ecef, Vector<3> const &dr_ecef, + Vector<3> const &dv_ecef, Matrix<6, 6> const &S) { + _inputs(w_earth_ecef, r_ecef, v_ecef); + + auto r_hill = lin::ref>(_x, 0, 0); + auto v_hill = lin::ref>(_x, 3, 0); + /* r_hill = Q_hill_ecef * dr_ecef + * + * v_hill = Q_hill_ecef * (dv_ecef0 - w_hill_ecef0 x dr_ecef0) + * = Q_hill_ecef * (dv_ecef + w_earth_ecef x dr_ecef - w_hill_ecef0 x dr_ecef0) + * = Q_hill_ecef * (dv_ecef + (w_earth_ecef - w_hill_ecef) x dr_ecef) + */ + r_hill = _Q_hill_ecef0 * (dr_ecef); + v_hill = _Q_hill_ecef0 * (dv_ecef + lin::cross(_w_earth_ecef - _w_hill_ecef0, dr_ecef)); + _sqrtP = S; + + _outputs(); + _check_validity(); +} + +void RelativeOrbitEstimate::update(Time dt_ns, Vector<3> const &w_earth_ecef, + Vector<3> const &r_ecef, Vector<3> const &v_ecef, + Matrix<6, 6> const &sqrtQ) { + _inputs(w_earth_ecef, r_ecef, v_ecef); + _predict(dt_ns, lin::norm(_w_hill_ecef0), sqrtQ); + _outputs(); + _check_validity(); +} + +void RelativeOrbitEstimate::update(Time dt_ns, Vector<3> const &w_earth_ecef, + Vector<3> const &r_ecef, Vector<3> const &v_ecef, Vector<3> const &dr_ecef, + Matrix<6, 6> const &sqrtQ, Matrix<3, 3> const &sqrtR) { + _inputs(w_earth_ecef, r_ecef, v_ecef); + _predict(dt_ns, lin::norm(_w_hill_ecef0), sqrtQ); + _update(_Q_hill_ecef0 * dr_ecef, sqrtR); + _outputs(); + _check_validity(); +} +} // namespace gnc diff --git a/src/psim/fc/relative_orbit_estimator.cpp b/src/psim/fc/relative_orbit_estimator.cpp new file mode 100644 index 00000000..87d0eae6 --- /dev/null +++ b/src/psim/fc/relative_orbit_estimator.cpp @@ -0,0 +1,143 @@ +// +// 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/relative_orbit_estimator.cpp + * @author Kyle Krol + */ + +#include + +#include +#include +#include +#include +#include + +namespace psim { + +void RelativeOrbitEstimator::_set_relative_orbit_outputs() { + fc_satellite_relative_orbit_is_valid.get() = estimate.valid(); + fc_satellite_relative_orbit_dr.get() = estimate.dr_ecef(); + fc_satellite_relative_orbit_r_hill.get() = estimate.r_hill(); + fc_satellite_relative_orbit_r_hill_sigma.get() = + lin::ref(lin::diag(estimate.S()), 0, 0); + fc_satellite_relative_orbit_dv.get() = estimate.dv_ecef(); + fc_satellite_relative_orbit_v_hill.get() = estimate.v_hill(); + fc_satellite_relative_orbit_v_hill_sigma.get() = + lin::ref(lin::diag(estimate.S()), 3, 0); +} + +void RelativeOrbitEstimator::add_fields(State &state) { + this->Super::add_fields(state); + + // This ensures upon simulation construction the state fields hold proper + // values. + _set_relative_orbit_outputs(); +} + +void RelativeOrbitEstimator::step() { + this->Super::step(); + + // Process noise + static constexpr auto sqrtQ = + lin::diag(Vector<6>({1.0e-8, 1.0e-8, 1.0e-8, 1.0e-4, 1.0e-4, 1.0e-2})) + .eval(); + + // Sensor noise + static constexpr auto sqrtR = + lin::diag(lin::consts>(1.0e-1)).eval(); + + auto const &dt = truth_dt_ns->get(); + auto const &w_earth = truth_earth_w->get(); + auto const &r_ecef = truth_satellite_orbit_r_ecef->get(); + auto const &v_ecef = truth_satellite_orbit_v_ecef->get(); + auto const &cdgps_dr = sensors_satellite_cdgps_dr->get(); + + // Handle when the estimate is already valid + if (estimate.valid()) { + // Predict and update + if (lin::all(lin::isfinite(cdgps_dr))) { + estimate.update(dt, w_earth, r_ecef, v_ecef, -cdgps_dr, sqrtQ, sqrtR); + } + // No measurement so we just predict + else { + estimate.update(dt, w_earth, r_ecef, v_ecef, sqrtQ); + } + } + // Attempt to initialize the estimate + else { + // Initialize the velocity with a finite difference + if (lin::all(lin::isfinite(previous_dr))) { + Vector3 cdgps_dv = 1.0e9 * (cdgps_dr - previous_dr) / Real(dt); + + Matrix<6, 6> S; + lin::ref>(S, 0, 0) = sqrtR; + lin::ref>(S, 3, 3) = lin::sqrt(2.0e9 / Real(dt)) * sqrtR; + + estimate = gnc::RelativeOrbitEstimate( + w_earth, r_ecef, v_ecef, -cdgps_dr, -cdgps_dv, S); + + previous_dr = lin::nans(); + } + // Cache a position reading + else { + previous_dr = cdgps_dr; + } + } + + _set_relative_orbit_outputs(); +} + +Vector3 RelativeOrbitEstimator::fc_satellite_relative_orbit_dr_error() const { + auto const &fc_dr = fc_satellite_relative_orbit_dr.get(); + auto const &truth_r_ecef = truth_satellite_orbit_r_ecef->get(); + auto const &truth_other_r_ecef = truth_other_orbit_r_ecef->get(); + + return fc_dr - (truth_other_r_ecef - truth_r_ecef); +} + +Vector3 +RelativeOrbitEstimator::fc_satellite_relative_orbit_r_hill_error() const { + auto const &fc_r_hill = fc_satellite_relative_orbit_r_hill.get(); + auto const &truth_r_hill = truth_satellite_hill_dr->get(); + + return fc_r_hill - truth_r_hill; +} + +Vector3 RelativeOrbitEstimator::fc_satellite_relative_orbit_dv_error() const { + auto const &fc_dv = fc_satellite_relative_orbit_dv.get(); + auto const &truth_v_ecef = truth_satellite_orbit_v_ecef->get(); + auto const &truth_other_v_ecef = truth_other_orbit_v_ecef->get(); + + return fc_dv - (truth_other_v_ecef - truth_v_ecef); +} + +Vector3 +RelativeOrbitEstimator::fc_satellite_relative_orbit_v_hill_error() const { + auto const &fc_v_hill = fc_satellite_relative_orbit_v_hill.get(); + auto const &truth_v_hill = truth_satellite_hill_dv->get(); + + return fc_v_hill - truth_v_hill; +} +} // namespace psim diff --git a/src/psim/simulations/relative_orbit_estimator_test.cpp b/src/psim/simulations/relative_orbit_estimator_test.cpp new file mode 100644 index 00000000..5a43ebdb --- /dev/null +++ b/src/psim/simulations/relative_orbit_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/relative_orbit_estimator_test.cpp + * @author Kyle Krol + */ + +#include + +#include +#include + +namespace psim { + +RelativeOrbitEstimatorTest::RelativeOrbitEstimatorTest( + RandomsGenerator &randoms, Configuration const &config) + : ModelList(randoms) { + add(randoms, config); + add(randoms, config, "leader", "follower"); +} +} // namespace psim