EkfOrbitStateRelState.h

/******************************************************************************
* Copyright (c) ATTX LLC 2024. All Rights Reserved.
*
* This software and associated documentation (the "Software") are the 
* proprietary and confidential information of ATTX, LLC. The Software is 
* furnished under a license agreement between ATTX and the user organization 
* and may be used or copied only in accordance with the terms of the agreement.
* Refer to 'license/attx_license.adoc' for standard license terms.
*
* EXPORT CONTROL NOTICE: THIS SOFTWARE MAY INCLUDE CONTENT CONTROLLED UNDER THE
* INTERNATIONAL TRAFFIC IN ARMS REGULATIONS (ITAR) OR THE EXPORT ADMINISTRATION 
* REGULATIONS (EAR99). No part of the Software may be used, reproduced, or 
* transmitted in any form or by any means, for any purpose, without the express 
* written permission of ATTX, LLC.
******************************************************************************/
/*
Orbit EKF with state and relative state file
 
Author: Alex Reynolds
*/
 
#ifndef GNC_NAVIGATION_EKF_EKF_ORBIT_STATE_REL_STATE_H
#define GNC_NAVIGATION_EKF_EKF_ORBIT_STATE_REL_STATE_H
 
#include "core/macros.h"
#include "architecture/Tasks.h"
#include "architecture/Time.h"
#include "core/CartesianVector.hpp"
 
#include "gnc/navigation/dynamics/CalcXdotPhiDotGravityJ2J3.hpp"
#include "utils/RK4Integrator.hpp"
#include "gnc/navigation/ekf/EkfTimeUpdate.hpp"
 
namespace clockwerk {
    /**
     * @brief   EKF Time Update step for Orbit dynamics spacecraft and relative state
     * 
     * This function performs an EKF time update using orbit dynamics
     * including J2 and J3 effects. It does NOT include noising models 
     * such as state noise compensation, which must be added later. 
     * 
     * It propagates forward in time a spacecraft state and spacecraft relative
     * state in the same frame using orbit dynamics accounting for J2 and J3 
     * effects. It also propagates forward covariance. The inputs to the function
     * include a spacecraft inertial position and velocity, as well as the 
     * associated covariance. Inputs also include a relative inertial position
     * and velocity in the same frame, as well as covariance.
     * 
     * This EKF assumes that the provided position and velocity are inertial
     * and that the x-y plane of the inertial frame is aligned to the x-y
     * plane of the planet rotating frame (thus allowing the spacecraft dynamics
     * to be calculated in an inertial frame without the need to rotate into
     * a planet fixed frame)
     * 
     * Author: Alex Reynolds <alex.reynolds@attx.tech>
    */
    class EkfOrbitStateRelState : public clockwerk::Task {
    public:
        // Model params
        //         NAME                     TYPE                    DEFAULT VALUE
        START_PARAMS
            /** This is the gravitational parameter of our parent planet. Defaults to
             * Earth's gravitational parameter for ease of use */
            SIGNAL(mu,                      double,                 3.986004418e+14)
            /** This is the J2 parameter of our parent planet. Defaults to
             * Earth's J2 parameter for ease of use */
            SIGNAL(J2,                      double,                 1.082635854e-3)
            /** This is the J3 parameter of our parent planet. Defaults to
             * Earth's J3 parameter for ease of use */
            SIGNAL(J3,                      double,                 -2.532435346e-6)
            /** The reference radius for the planet. Must be set prior to calculation. */
            SIGNAL(R,                       double,                 6378140.0)
        END_PARAMS
 
        // Model inputs
        //         NAME                     TYPE                    DEFAULT VALUE
        START_INPUTS
            /** The input time stamp representing the currently known spacecraft state
             *  Can be configured to loopback to outputs/time_state */
            SIGNAL(time_prev,               clockwerk::Time,        clockwerk::Time(0))
            /** The input time stamp representing the end of the spacecraft state integration.
             *  Will be written to time_state */
            SIGNAL(time_update,             clockwerk::Time,        clockwerk::Time(0))
            /** The inertial position estimate for the spacecraft before EKF time update */
            SIGNAL(pos_prev_sc__i,          CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The inertial velocity estimate for the spacecraft before EKF time update */
            SIGNAL(vel_prev_sc__i,          CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The covariance matrix for the object before EKF time update */
            SIGNAL(cov_prev_sc__i,          Matrix6D,               Matrix6D())
            /** The relative position estimate in the inertial frame for the object before EKF time update */
            SIGNAL(pos_prev_sc_rel__i,      CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The relative velocity estimate in the inertial frame for the object before EKF time update */
            SIGNAL(vel_prev_sc_rel__i,      CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The covariance matrix for the relative state before EKF time update */
            SIGNAL(cov_prev_sc_rel__i,      Matrix6D,               Matrix6D())
        END_INPUTS
 
        // Model outputs
        //         NAME                     TYPE                    DEFAULT VALUE
        START_OUTPUTS
            /** The time stamp associated with the post snc covariance matrix */
            SIGNAL(time_state,              clockwerk::Time,        clockwerk::Time(0))
            /** The inertial position estimate for the spacecraft after EKF time update */
            SIGNAL(pos_update_sc__i,        CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The inertial velocity estimate for the spacecraft after EKF time update */
            SIGNAL(vel_update_sc__i,        CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The covariance matrix for the object after EKF time update */
            SIGNAL(cov_update_sc__i,        Matrix6D,               Matrix6D())
            /** The relative position estimate in the inertial frame for the object after EKF time update */
            SIGNAL(pos_update_sc_rel__i,    CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The relative velocity estimate in the inertial frame for the object after EKF time update */
            SIGNAL(vel_update_sc_rel__i,    CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The covariance matrix for the relative state after EKF time update */
            SIGNAL(cov_update_sc_rel__i,    Matrix6D,               Matrix6D())
            /** The state transition matrix for the spacecraft propagation from t_prev to t_update */
            SIGNAL(phi_sc__i,               Matrix6D,               Matrix6D())
            /** The state transition matrix for the spacecraft relative propagation from t_prev to t_update */
            SIGNAL(phi_sc_rel__i,           Matrix6D,               Matrix6D())
        END_OUTPUTS
 
        // Model-specific implementations of startup and derivative
        EkfOrbitStateRelState();
        EkfOrbitStateRelState(clockwerk::Task &pnt, int schedule_slot=0, const std::string &m_name="ekf_state_relstate_update");
        EkfOrbitStateRelState(clockwerk::Executive &e, int schedule_slot=0, const std::string &m_name="ekf_state_relstate_update");
        virtual ~EkfOrbitStateRelState() {}
    protected:
        int start();
        int execute();
 
        // Kalman filtering variables
        CalcXdotPhiDotGravityJ2J3<double> _dynamics;
        RK4Integrator<double, 42> _integrator;
        EkfTimeUpdate<double, 6> _ekf_time_update;
 
        // I/O to EKF
        std::array<double, 6> _state_input;
        std::array<double, 6> _state_output;
        std::array<double, 6> _state_input_rel;
        std::array<double, 6> _state_output_rel;
    };
 
}
 
#endif