EkfOrbitJ2J3Update.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.
******************************************************************************/
/*
Simple PID header file
 
Author: Alex Reynolds
*/
 
#ifndef GNC_NAVIGATION_EKF_EKF_ORBIT_J2_J3_H
#define GNC_NAVIGATION_EKF_EKF_ORBIT_J2_J3_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 with J2, J3
     * 
     * 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 receives inertial position, velocity, and an associated covariance
     * matrix, and propagates them forward to the provided time 
     * using an internal RK4 integrator.
     * 
     * 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 EkfOrbitJ2J3Update : 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 position estimate for the object before EKF time update */
            SIGNAL(pos_prev,                CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The velocity estimate for the object before EKF time update */
            SIGNAL(vel_prev,                CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The covariance matrix for the object before EKF time update */
            SIGNAL(cov_prev,                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 position estimate for the object after EKF time update */
            SIGNAL(pos_update,              CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The velocity estimate for the object after EKF time update */
            SIGNAL(vel_update,              CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The covariance matrix for the object after EKF time update */
            SIGNAL(cov_update,              Matrix6D,               Matrix6D())
            /** The state transition matrix for the propagation from t_prev to t_update */
            SIGNAL(phi,                     Matrix6D,               Matrix6D())
        END_OUTPUTS
 
        // Model-specific implementations of startup and derivative
        EkfOrbitJ2J3Update();
        EkfOrbitJ2J3Update(clockwerk::Task &pnt, int schedule_slot=0, const std::string &m_name="ekf_j2_j3_update");
        EkfOrbitJ2J3Update(clockwerk::Executive &e, int schedule_slot=0, const std::string &m_name="ekf_j2_j3_update");
        virtual ~EkfOrbitJ2J3Update() {}
    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;
    };
 
}
 
#endif