dynamicsfunctions.hpp

/******************************************************************************
* 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.
******************************************************************************/
 
#ifndef DYNAMICS_FUNCTIONS_HPP
#define DYNAMICS_FUNCTIONS_HPP
 
#include "core/Matrix.hpp"
 
#define NUM_INPUT_STATES 6
 
namespace clockwerk {
 
    /// @brief Function to perform a simple approximate SNC update
    /// @param t_step The size of the timestep for which process noise is applied
    /// @param Qc The continuous time process noise matrix
    /// @param B The application matrix describing channels on which noise should be applied
    /// @param Qd Implicit return of the discrete time process noise matrix
    template <typename T, unsigned int N, unsigned int Qn>
    void calcSimpleSncMatrix(T t_step, const Matrix<T, Qn, Qn> &Qc,
                             const Matrix<T, N, Qn> &B, Matrix<T, N, N> &Qd) {
        // First generate our omega matrix from our time and B matrix
        Matrix<T, N, Qn> omega;
        multiply(t_step, B, omega);
 
        // Now generate our discrete time Q matrix
        Qd = omega*Qc*omega.transpose();
    }
 
    /// @brief Function to calculate A matrix for system with point mass, J2, J3 gravity
    /// @param state The state of the system, [x, y, z, xDot, yDot, zDot]
    /// @param mu Gravitational parameter of the system
    /// @param J2 J2 parameter of the system
    /// @param J3 J3 parameter of the system
    /// @param R Radius of the reference planet
    /// @param A Implicit return of A matrix for the system
    /// @note Overloaded to take multiple array sizes for ease of use
    template <typename T>
    int calcDfDzGravityJ2J3(const std::array<T, NUM_INPUT_STATES> &state,
                            T mu, T J2, T J3, T R, clockwerk::Matrix<T, NUM_INPUT_STATES, NUM_INPUT_STATES> &A) {
        // Zero out our matrix
        A.setToZeros();
 
        // Now pre-calculate some simple parameters
        T R2 = R*R;
        T R3 = R2*R;
        T x2 = state[0]*state[0];
        T y2 = state[1]*state[1];
        T z2 = state[2]*state[2];
        T z3 = z2*state[2];
        T z4 = z2*z2;
        T r = std::sqrt(x2 + y2 + z2);
        T r2 = r*r;
        T r4 = r2*r2;
        T r6 = r4*r2;
        T r8 = r4*r4;
        T r9 = r8*r;
        T r11 = r9*r2;
 
        // Define the most basic common terms
        T common_mu_r6_over_r17;
        int err = clockwerk::safeDivide(0.5*mu, r11, common_mu_r6_over_r17);
        if(err) {return err;}
        T J2R2 = J2*R2;
        T J3R3 = J3*R3;
        T tmp_1 = 105.0*r2*(J2R2*z2 - J3R3*state[2]);
 
        // Calculate each element of our matrix
        A[0][3] = 1.0;
        A[1][4] = 1.0;
        A[2][5] = 1.0;
        A[3][0] = -common_mu_r6_over_r17*(2.0*r8 + r6*(3.0*J2R2 - 6.0*x2) - 15.0*J2R2*r4*(x2 + z2) + J3R3*(15.0*r4*state[2] - 35.0*r2*z3 + 315.0*x2*z3) + tmp_1*x2);
        A[3][1] = A[4][0] = 3.0*common_mu_r6_over_r17*state[0]*state[1]*(2.0*r6 + 5.0*J2R2*r4 - 105.0*J3R3*z3 - 35.0*J2R2*r2*z2 + 35.0*J3R3*r2*state[2]);
        A[3][2] = A[5][0] = 3.0*common_mu_r6_over_r17*state[0]*(2.0*r6*state[2] - 5.0*J3R3*r4 - 105.0*J3R3*z4 + 15.0*J2R2*r4*state[2] - 35.0*J2R2*r2*z3 + 70.0*J3R3*r2*z2);
        A[4][1] = -common_mu_r6_over_r17*(2.0*r8 + r6*(3.0*J2R2 - 6.0*y2) - 15.0*J2R2*r4*(y2 + z2) + J3R3*(15.0*r4*state[2] - 35.0*r2*z3 + 315.0*y2*z3) + tmp_1*r2);
        A[4][2] = A[5][1] = 3.0*common_mu_r6_over_r17*state[1]*(2.0*r6*state[2] - 5.0*J3R3*r4 - 105.0*J3R3*z4 + 15.0*J2R2*r4*state[2] - 35.0*J2R2*r2*z3 + 70.0*J3R3*r2*z2);
        A[5][2] = -common_mu_r6_over_r17*(2.0*r8 - 6.0*r6*z2 + J3R3*state[2]*(315.0*z4 + 75.0*r4 - 350.0*r2*z2) + J2R2*(9.0*r6 + 105.0*r2*z4 - 90.0*r4*z2));
 
        return NO_ERROR;
    }
 
    /// @brief Function to calculate A matrix for system with point mass, J2, J3 gravity
    /// @param state The state of the system, [x, y, z, xDot, yDot, zDot] -- all others ignored
    /// @param mu Gravitational parameter of the system
    /// @param J2 J2 parameter of the system
    /// @param J3 J3 parameter of the system
    /// @param R Radius of the reference planet
    /// @param A Implicit return of A matrix for the system
    /// @note Overloaded to take multiple array sizes for ease of use
    template <typename T>
    int calcDfDzGravityJ2J3(const std::array<T, NUM_INPUT_STATES + NUM_INPUT_STATES*NUM_INPUT_STATES> &state,
                            T mu, T J2, T J3, T R, clockwerk::Matrix<T, NUM_INPUT_STATES, NUM_INPUT_STATES> &A) {
        // Zero out our matrix
        A.setToZeros();
 
        // Now pre-calculate some simple parameters
        T R2 = R*R;
        T R3 = R2*R;
        T x2 = state[0]*state[0];
        T y2 = state[1]*state[1];
        T z2 = state[2]*state[2];
        T z3 = z2*state[2];
        T z4 = z2*z2;
        T r = std::sqrt(x2 + y2 + z2);
        T r2 = r*r;
        T r4 = r2*r2;
        T r6 = r4*r2;
        T r8 = r4*r4;
        T r9 = r8*r;
        T r11 = r9*r2;
 
        // Define the most basic common terms
        T common_mu_r6_over_r17;
        int err = clockwerk::safeDivide(0.5*mu, r11, common_mu_r6_over_r17);
        if(err) {return err;}
        T J2R2 = J2*R2;
        T J3R3 = J3*R3;
        T tmp_1 = 105.0*r2*(J2R2*z2 - J3R3*state[2]);
 
        // Calculate each element of our matrix
        A[0][3] = 1.0;
        A[1][4] = 1.0;
        A[2][5] = 1.0;
        A[3][0] = -common_mu_r6_over_r17*(2.0*r8 + r6*(3.0*J2R2 - 6.0*x2) - 15.0*J2R2*r4*(x2 + z2) + J3R3*(15.0*r4*state[2] - 35.0*r2*z3 + 315.0*x2*z3) + tmp_1*x2);
        A[3][1] = A[4][0] = 3.0*common_mu_r6_over_r17*state[0]*state[1]*(2.0*r6 + 5.0*J2R2*r4 - 105.0*J3R3*z3 - 35.0*J2R2*r2*z2 + 35.0*J3R3*r2*state[2]);
        A[3][2] = A[5][0] = 3.0*common_mu_r6_over_r17*state[0]*(2.0*r6*state[2] - 5.0*J3R3*r4 - 105.0*J3R3*z4 + 15.0*J2R2*r4*state[2] - 35.0*J2R2*r2*z3 + 70.0*J3R3*r2*z2);
        A[4][1] = -common_mu_r6_over_r17*(2.0*r8 + r6*(3.0*J2R2 - 6.0*y2) - 15.0*J2R2*r4*(y2 + z2) + J3R3*(15.0*r4*state[2] - 35.0*r2*z3 + 315.0*y2*z3) + tmp_1*r2);
        A[4][2] = A[5][1] = 3.0*common_mu_r6_over_r17*state[1]*(2.0*r6*state[2] - 5.0*J3R3*r4 - 105.0*J3R3*z4 + 15.0*J2R2*r4*state[2] - 35.0*J2R2*r2*z3 + 70.0*J3R3*r2*z2);
        A[5][2] = -common_mu_r6_over_r17*(2.0*r8 - 6.0*r6*z2 + J3R3*state[2]*(315.0*z4 + 75.0*r4 - 350.0*r2*z2) + J2R2*(9.0*r6 + 105.0*r2*z4 - 90.0*r4*z2));
 
        return NO_ERROR;
    }
 
}
 
#endif