InertialNavigationDynamics.h

Go to the documentation of this file.

/******************************************************************************
* 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 GNCUTILS_EKF_INERTIAL_NAVIGATION_DYNAMICS_HPP
#define GNCUTILS_EKF_INERTIAL_NAVIGATION_DYNAMICS_HPP
 
#include "gncutils/dynamics/Rates.hpp"
#include "dynamics/Quaternion.h"
#include "constants/planetdefaults.h"
#include "core/CartesianVector.hpp"
#include "core/mathmacros.h"
 
namespace warpos {
 
    // ***** Define the number of state vector elements, observer vector elements, and process noise elements ***** //
 
    // Postion of body wrt inertial resolved in inertial, Velocity of body wrt inertial resolved in inertial, Orientation quaternion of body wrt inertial, 
    // gyroscope bias of the body angular velocity wrt inertial angular velocity resolved in the body frame, and
    // accelerometer bias of the body acceleration (minus gravity) wrt inertial acceleration (minus gravity) resolved in the body frame
    const uint32 INERTIAL_NAVIGATION_STATE_VECTOR_ELEMENTS = 16;
 
    const uint32 INERTIAL_NAV_RATES_VECTOR_SIZE = 16 + 16*16;

    class InertialNavigationDynamics : public Rates<INERTIAL_NAV_RATES_VECTOR_SIZE>{
    public:
 
        int16 calculateRates(floating_point time, 
                             const std::array<floating_point, INERTIAL_NAV_RATES_VECTOR_SIZE> &state, 
                             std::array<floating_point, INERTIAL_NAV_RATES_VECTOR_SIZE> &out_rates) override;

        CartesianVector3 imu_meas_accel__body;
        CartesianVector3 imu_meas_ang_vel__body;

        CartesianVector3 gravity__pI;
    private:
        // Intermediate calculation variables
        clockwerk::Quaternion _quat_body_pI;        
        CartesianVector3 _v_dot;
        CartesianVector3 _unbiased_body_accel__body;
        CartesianVector4 _q_dot;
        CartesianVector3 _unbiased_body_ang_vel__body;
        Matrix16 _dynamics_jacobian = Matrix16();
        clockwerk::Matrix<3, 4> _dvdotdq;
        clockwerk::Matrix<4, 4> _dqdotdq;
        clockwerk::Matrix<4, 3> _dqdotdBw;
    };
 
}
 
#endif