Feature/910 state class

src/fswAlgorithms/_GeneralModuleFiles/dynamicModels.cpp

@@ -0,0 +1,89 @@
+/*
+ ISC License
+
+ Copyright (c) 2024, University of Colorado at Boulder
+
+ Permission to use, copy, modify, and/or distribute this software for any
+ purpose with or without fee is hereby granted, provided that the above
+ copyright notice and this permission notice appear in all copies.
+
+ THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+
+ */
+
+#include "dynamicModels.h"
+
+DynamicsModel::DynamicsModel() = default;
+
+DynamicsModel::~DynamicsModel() = default;
+
+/*! Call the propagation function for the dynamics
+   @param StateVector inputState
+   @param StateVector propagatedState
+*/
+StateVector DynamicsModel::propagate(const std::array<double, 2> interval, const StateVector& state, const double dt) const {
+    double t_0 = interval[0];
+    double t_f = interval[1];
+    double t = t_0;
+    StateVector X(state);
+
+    /*! Propagate to t_final with an RK4 integrator */
+    double N = ceil((t_f-t_0)/dt);
+    for (int c=0; c < N; c++) {
+        double step = std::min(dt,t_f-t);
+        X = this->rk4(this->propagator, X, t, step);
+        t = t + step;
+    }
+
+    return X;
+}
+
+/*! Set the propagation function for the dynamics
+   @param std::function<const StateVector(const StateVector&)>& dynamicsPropagator
+*/
+void DynamicsModel::setPropagator(const std::function<const StateVector(const double, const StateVector&)>&
+        dynamicsPropagator){
+    this->propagator = dynamicsPropagator;
+}
+
+/*! Call the dynamics matrix containing the partials of the dynamics with respect to the state
+   @param std::function<const Eigen::MatrixXd(const StateVector&)>& dynamicsMatrixCalculator
+*/
+Eigen::MatrixXd DynamicsModel::computeDynamicsMatrix(const double time, const StateVector& state) const {
+    return this->dynamicsMatrix(time, state);
+}
+
+/*! Set the dynamics matrix containing the partials of the dynamics with respect to the state
+   @param std::function<const Eigen::MatrixXd(const StateVector&)>& dynamicsMatrixCalculator
+*/
+void DynamicsModel::setDynamicsMatrix(const std::function<const Eigen::MatrixXd(const double, const StateVector&)>&
+        dynamicsMatrixCalculator){
+    this->dynamicsMatrix = dynamicsMatrixCalculator;
+}
+
+/*! Runge-Kutta 4 (RK4) function for state propagation
+    @param ODEfunction function handle that includes the equations of motion
+    @param X0 initial state
+    @param t0 initial time
+    @param dt time step
+    @return Eigen::VectorXd
+*/
+    StateVector DynamicsModel::rk4(const std::function<const StateVector(const double, const StateVector&)>& ODEfunction,
+            const StateVector& X0,
+            double t0,
+            double dt) const{
+    double h = dt;
+
+    StateVector k1 = ODEfunction(t0, X0);
+    StateVector k2 = ODEfunction(t0 + h/2., X0.add(k1.scale(h/2.)));
+    StateVector k3 = ODEfunction(t0 + h/2., X0.add(k2.scale(h/2.)));
+    StateVector k4 = ODEfunction(t0 + h, X0.add(k3.scale(h)));
+
+    return k1.scale(h/6.).add(k2.scale(h/3.)).add(k3.scale(h/3.)).add(k4.scale(h/6.));
+}

src/fswAlgorithms/_GeneralModuleFiles/dynamicModels.h

@@ -0,0 +1,48 @@
+/*
+ ISC License
+
+ Copyright (c) 2024, University of Colorado at Boulder
+
+ Permission to use, copy, modify, and/or distribute this software for any
+ purpose with or without fee is hereby granted, provided that the above
+ copyright notice and this permission notice appear in all copies.
+
+ THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+
+ */
+
+#ifndef FILTER_DYN_MODELS_H
+#define FILTER_DYN_MODELS_H
+
+#include <Eigen/Core>
+#include "fswAlgorithms/_GeneralModuleFiles/stateModels.h"
+
+/*! @brief Measurement models used to map a state vector to a measurement */
+class DynamicsModel{
+private:
+    std::function<const StateVector(const double, const StateVector&)> propagator; //!< [-] state propagator using dynamics
+    std::function<const Eigen::MatrixXd(const double, const StateVector&)> dynamicsMatrix; //!< [-] partial of dynamics wrt state
+
+    StateVector rk4(const std::function<const StateVector(const double, const StateVector&)>& ODEfunction,
+            const StateVector& X0,
+            double t0,
+            double dt) const;
+
+public:
+    DynamicsModel();
+    ~DynamicsModel();
+
+    StateVector propagate(const std::array<double, 2>, const StateVector& state, const double dt) const;
+    void setPropagator(const std::function<const StateVector(const double, const StateVector&)>& dynamicsPropagator);
+
+    Eigen::MatrixXd computeDynamicsMatrix(const double time, const StateVector& state) const;
+    void setDynamicsMatrix(const std::function<const Eigen::MatrixXd(const double, const StateVector&)>& dynamicsMatrixCalculator);
+};
+
+#endif

src/fswAlgorithms/_GeneralModuleFiles/stateModels.cpp

@@ -0,0 +1,169 @@
+/*
+ ISC License
+
+ Copyright (c) 2024, University of Colorado at Boulder
+
+ Permission to use, copy, modify, and/or distribute this software for any
+ purpose with or without fee is hereby granted, provided that the above
+ copyright notice and this permission notice appear in all copies.
+
+ THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+
+ */
+
+#include "stateModels.h"
+
+State::State() = default;
+
+State::~State() = default;
+
+/*! Set the values of a given state
+   @param Eigen::VectorXd
+*/
+void State::setValues(const Eigen::VectorXd &componentValues){
+    this->values = componentValues;
+}
+
+/*! Get the values of a given state
+   @return Eigen::VectorXd
+*/
+Eigen::VectorXd State::getValues() const {
+    return this->values;
+}
+
+StateVector::StateVector() = default;
+
+StateVector::~StateVector() = default;
+
+/*! Set the positional components of your state (cartesian position, attitude, etc)
+   @param Eigen::VectorXd positionComponents
+*/
+size_t StateVector::size() const{
+    size_t totalSize = 0;
+    totalSize += this->position.getValues().size();
+    totalSize += this->velocity.getValues().size();
+    totalSize += this->acceleration.getValues().size();
+    totalSize += this->bias.getValues().size();
+    totalSize += this->considerParameters.getValues().size();
+    return totalSize;
+}
+
+/*! Add two states together
+   @param StateVector vector
+   @return StateVector sumVector
+*/
+StateVector StateVector::add(const StateVector &vector) const {
+    StateVector sum;
+    sum.setPositionStates(this->position.getValues() + vector.getPositionStates());
+    sum.setVelocityStates(this->velocity.getValues() + vector.getVelocityStates());
+    sum.setAccelerationStates(this->acceleration.getValues() + vector.getAccelerationStates());
+    sum.setBiasStates(this->bias.getValues() + vector.getBiasStates());
+    sum.setConsiderStates(this->considerParameters.getValues() + vector.getConsiderStates());
+    sum.attachSTM(this->stm + vector.detatchSTM());
+    return sum;
+}
+
+/*! Scale a state vector by a constant
+   @param double scalar
+   @return StateVector scaledVector
+*/
+StateVector StateVector::scale(const double scalar) const {
+    StateVector scaledVector;
+    scaledVector.setPositionStates(this->position.getValues()*scalar);
+    scaledVector.setVelocityStates(this->velocity.getValues()*scalar);
+    scaledVector.setAccelerationStates(this->acceleration.getValues()*scalar);
+    scaledVector.setBiasStates(this->bias.getValues()*scalar);
+    scaledVector.setConsiderStates(this->considerParameters.getValues()*scalar);
+    scaledVector.attachSTM(this->stm*scalar);
+    return scaledVector;
+}
+
+/*! Set the positional components of your state (cartesian position, attitude, etc)
+   @param Eigen::VectorXd positionComponents
+*/
+void StateVector::setPositionStates(const Eigen::VectorXd &positionComponents){
+    this->position.setValues(positionComponents);
+}
+
+/*! Get the positional components of your state (cartesian position, attitude, etc)
+   @return Eigen::VectorXd
+*/
+Eigen::VectorXd StateVector::getPositionStates() const {
+    return this->position.getValues();
+}
+
+/*! Set the velocity components of your state (cartesian velocity, angular rate, etc)
+   @param Eigen::VectorXd setVelocityStates
+*/
+void StateVector::setVelocityStates(const Eigen::VectorXd &velocityComponents){
+    this->velocity.setValues(velocityComponents);
+}
+
+/*! Get the velocity components of your state (cartesian velocity, angular rate, etc)
+   @return Eigen::VectorXd
+*/
+Eigen::VectorXd StateVector::getVelocityStates() const {
+    return this->velocity.getValues();
+}
+
+/*! Set the acceleration components of your state (cartesian acceleration, angular acceleration, etc)
+   @param Eigen::VectorXd accelerationComponents
+*/
+void StateVector::setAccelerationStates(const Eigen::VectorXd &accelerationComponents){
+    this->acceleration.setValues(accelerationComponents);
+}
+
+/*! Get the acceleration components of your state (cartesian acceleration, angular acceleration, etc)
+   @return Eigen::VectorXd
+*/
+Eigen::VectorXd StateVector::getAccelerationStates() const {
+    return this->acceleration.getValues();
+}
+
+/*! Set the bias parameters of your state (gyro bias, scale factors, etc)
+   @param Eigen::VectorXd biasComponents
+*/
+void StateVector::setBiasStates(const Eigen::VectorXd &biasComponents){
+    this->bias.setValues(biasComponents);
+}
+
+/*! Get the bias parameters of your state (gyro bias, scale factors, etc)
+   @return Eigen::VectorXd
+*/
+Eigen::VectorXd StateVector::getBiasStates() const {
+    return this->bias.getValues();
+}
+
+/*! Set the consider parameters of your state (gravitational parameter, etc)
+   @param Eigen::VectorXd considerParameters
+*/
+void StateVector::setConsiderStates(const Eigen::VectorXd &considerComponents){
+    this->considerParameters.setValues(considerComponents);
+}
+
+/*! Get the consider parameters of your state (gravitational parameter, etc)
+   @return Eigen::VectorXd
+*/
+Eigen::VectorXd StateVector::getConsiderStates() const {
+    return this->considerParameters.getValues();
+}
+
+/*! Attach the state transition matrix of your state for simultaneous propagation
+   @param Eigen::MatrixXd stm
+*/
+void StateVector::attachSTM(const Eigen::MatrixXd& stm){
+    this->stm = stm;
+}
+
+/*! Detatch the state transition matrix of your state for simultaneous propagation
+   @return Eigen::MatrixXd stm
+*/
+Eigen::MatrixXd StateVector::detatchSTM() const{
+    return this->stm;
+}

src/fswAlgorithms/_GeneralModuleFiles/stateModels.h

@@ -0,0 +1,77 @@
+/*
+ ISC License
+
+ Copyright (c) 2024, University of Colorado at Boulder
+
+ Permission to use, copy, modify, and/or distribute this software for any
+ purpose with or without fee is hereby granted, provided that the above
+ copyright notice and this permission notice appear in all copies.
+
+ THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+
+ */
+
+
+#ifndef FILTER_STATE_MODELS_H
+#define FILTER_STATE_MODELS_H
+
+#include <Eigen/Core>
+
+/*! @brief State class */
+class State{
+private:
+    Eigen::VectorXd values;
+public:
+    State();
+    ~State();
+    void setValues(const Eigen::VectorXd& componentValues);
+    Eigen::VectorXd getValues() const;
+};
+
+class PositionState : public State{};
+class VelocityState : public State{};
+class AccelerationState : public State{};
+class BiasState : public State{};
+class ConsiderState : public State{};
+
+
+/*! @brief State models used to map a state vector to a measurement */
+class StateVector{
+private:
+    PositionState position;
+    VelocityState velocity;
+    AccelerationState acceleration;
+    BiasState bias;
+    ConsiderState considerParameters;
+    Eigen::MatrixXd stm;
+
+public:
+    StateVector();
+    ~StateVector();
+
+    size_t size() const;
+    StateVector add(const StateVector &vector) const;
+    StateVector scale(const double scalar) const;
+
+    void setPositionStates(const Eigen::VectorXd& positionComponents);
+    Eigen::VectorXd getPositionStates() const;
+    void setVelocityStates(const Eigen::VectorXd& velocityComponents);
+    Eigen::VectorXd getVelocityStates() const;
+    void setAccelerationStates(const Eigen::VectorXd& accelerationComponents);
+    Eigen::VectorXd getAccelerationStates() const;
+    void setBiasStates(const Eigen::VectorXd& biasComponents);
+    Eigen::VectorXd getBiasStates() const;
+    void setConsiderStates(const Eigen::VectorXd& considerComponents);
+    Eigen::VectorXd getConsiderStates() const;
+
+    void attachSTM(const Eigen::MatrixXd& stm);
+    Eigen::MatrixXd detatchSTM() const;
+};
+
+#endif