DeadReckon.h

Go to the documentation of this file.

/******************************************************************************
* Copyright (c) ATTX INC 2025. All Rights Reserved. 
*
* This software and associated documentation (the "Software") are the 
* proprietary and confidential information of ATTX, INC. 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, INC.
******************************************************************************/
/*
Inertial Navigation EKF Dead Reckoning Propagation Step Header File 
 
Author: James Tabony
*/
#ifndef APPS_INERTIAL_NAVIGATION_DEAD_RECKON_H
#define APPS_INERTIAL_NAVIGATION_DEAD_RECKON_H
 
#include "core/mathmacros.h"
 
#include "flight/App.h"
#include "flight/FlightExecutive.h"
 
#include "telemetry/tlm_InertialNavigationDeadReckon.h"
#include "command/cmd_InertialNavigationDeadReckon.h"
 
#include "gncutils/EKF/InertialNavigationDynamics.h"
#include "gncutils/integrator/ForwardEulerIntegrator.hpp"
#include "gncutils/EKF/EkfTimeUpdate.hpp"
 
#include "apps/gnc/InertialNavigationEKF/InertialNavigationDefinition.hpp"
 
namespace warpos {
    
    class DeadReckon : public App {
    public:
        START_PARAMS
            SIGNAL(pos_pI_tI__pI,               clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(time_diff_warning_bound,     clockwerk::Time,                clockwerk::Time(5, 0))
 
            // ***** PROCESS NOISE PARAMS ***** //

            SIGNAL(velocity_standard_deviation, clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(attitude_standard_deviation, clockwerk::CartesianVector<4>,  clockwerk::CartesianVector<4>({0.0, 0.0, 0.0, 0.0}))
            SIGNAL(gyroBias_standard_deviation, clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(accelBias_standard_deviation,clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
 
            // ***** IMU LOCATION PARAMS ***** //

            SIGNAL(pos_IMU_B__B,                clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(quat_IMU_B,                  clockwerk::Quaternion,          clockwerk::Quaternion({1.0, 0.0, 0.0, 0.0}))
        END_PARAMS
 
        START_INPUTS
            // ***** STATE INPUT DEFINITIONS ***** //

            SIGNAL(pos_B_pI__pI,                clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(vel_B_pI__pI,                clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(quat_B_pI,                   clockwerk::Quaternion,          clockwerk::Quaternion({1.0, 0.0, 0.0, 0.0}))
            SIGNAL(gyroBias_B_pI__pI,           clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(accelBias_B_pI__pI,          clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(state_cov,                   Matrix16,                       Matrix16())
            SIGNAL(state_input_time_stamp,      clockwerk::Time,                clockwerk::Time(0, 0))
 
            // ***** IMU MEASUREMENT INPUT DEFINITIONS ***** //
 
            
            SIGNAL(gyroMeas_IMU_tI__IMU,        clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(accelMeas_IMU_tI__IMU,       clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(IMU_meas_time_stamp,         clockwerk::Time,                clockwerk::Time(0, 0))
            SIGNAL(IMU_meas_validity_flag,      bool,                           false)
 
            // ***** ADDITIONAL INPUT DEFINITIONS ***** //
 
            
            SIGNAL(accelGrav_body__pI,          clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
 
            // ***** OUTPUT DEFINING INPUTS DEFINITIONS ***** //

            SIGNAL(state_output_time_stamp,     clockwerk::Time,                clockwerk::Time(0, 0));
 
        END_INPUTS
 
        START_OUTPUTS
            SIGNAL(pos_B_pI__pI,                clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(vel_B_pI__pI,                clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(quat_B_pI,                   clockwerk::Quaternion,          clockwerk::Quaternion({1.0, 0.0, 0.0, 0.0}))
            SIGNAL(gyroBias_B_pI__pI,           clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(accelBias_B_pI__pI,          clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(state_cov,                  Matrix16,                        Matrix16())
        END_OUTPUTS
 
        DeadReckon(FlightExecutive &executive);
 
        virtual ~DeadReckon() {};
    
        int16 activate() override;

        int16 deactivate() override;

        int16 command(uint16 apid, uint8* buffer, uint16 size) override;

        int16 getLastValidMeasurement(clockwerk::CartesianVector<3> &gyroMeas_IMU_tI__IMU, clockwerk::CartesianVector<3> &accelMeas_IMU_tI__IMU, floating_point &time_stamp);
 
    protected:
        int16 start() override;
        int16 execute() override;

        InertialNavigationDynamics _dynamics;
        ForwardEulerIntegrator<INERTIAL_NAVIGATION_STATE_VECTOR_ELEMENTS+INERTIAL_NAVIGATION_STATE_VECTOR_ELEMENTS*INERTIAL_NAVIGATION_STATE_VECTOR_ELEMENTS> _integrator;
        EkfTimeUpdate<INERTIAL_NAVIGATION_STATE_VECTOR_ELEMENTS> _time_update;

        std::array<floating_point, 3> _dummy3; 
        std::array<floating_point, 4> _dummy4; 
        inertial_nav::PropagateObsVector _dummyObs; 

        clockwerk::CartesianVector<3> _last_valid_gyro_meas, _last_valid_accel_meas; 
        floating_point _last_valid_meas_time_stamp;
        
        bool _valid_measurement_saved = false;

        inertial_nav::StateVector _state_input_array, _state_output_array; 
        inertial_nav::PropagateObsVector _state_observer_array;

        clockwerk::CartesianVector<3> _gyroMeas_B_pI__B, _accelMeas_B_pI__B;

        clockwerk::CartesianVector<3> _approximate_ang_accel = clockwerk::CartesianVector<3>({0.0, 0.0, 0.0});

        clockwerk::Quaternion _unit_quat;

        tlm_gnc_inert_prop_state _tlm_a_priori_state;
    };
}
 
#endif