InertialNavigationDefinition.hpp

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.
******************************************************************************/
 
#ifndef APPS_GNC_INERTIAL_NAVIGATION_EKF_DEFINITION_HPP
#define APPS_GNC_INERTIAL_NAVIGATION_EKF_DEFINITION_HPP
 
#include <array>
#include "core/Matrix.hpp"
#include "types.h"
 
namespace inertial_nav {

 
 
 
// ------------------------------------------------------------------------------------
// State Vector Indexing
// ------------------------------------------------------------------------------------
 
constexpr uint32 STATE_POS_IDX = 0;             // Index 0–2: position -- _B_pI__pI [m]
constexpr uint32 STATE_VEL_IDX = 3;             // Index 3–5: velocity -- _B_pI__pI [m/s]
constexpr uint32 STATE_QUAT_IDX = 6;            // Index 6–9: quaternion -- _B_pI [unitless]
constexpr uint32 STATE_GYRO_BIAS_IDX = 10;      // Index 10–12: gyro bias -- _B_pI__B [rad/s]
constexpr uint32 STATE_ACCEL_BIAS_IDX = 13;     // Index 13–15: accel bias -- _B_pI__B [m/s^2]
 
constexpr uint32 STATE_SIZE = 16;               // Total state vector elements
 
// Assert to ensure that state size does not change arbitrarily, as other elements
// of the software depend on this size (telemetry, etc.)
static_assert(STATE_SIZE == 16, "Changing state size must be associated with TLM size change.");
 
 
 
// ------------------------------------------------------------------------------------
// Covariance Indexing Notes
// ------------------------------------------------------------------------------------
// The covariance matrix P is STATE_SIZE x STATE_SIZE
// Covariances follow the same element order as the state
 
 
 
// ------------------------------------------------------------------------------------
// Propagation Observer Vector Definitions
// ------------------------------------------------------------------------------------
 
constexpr uint32 PROPAGATE_GRAV_ACCEL_IDX = 0;  // Index 0-2: acceleration -- _B_tI__pI [m/s^2]
constexpr uint32 PROPAGATE_GYRO_MEAS_IDX = 3;   // Index 3-5: gyro measurement -- _IMU_tI__IMU [rad/s]
constexpr uint32 PROPAGATE_ACCEL_MEAS_IDX = 6;  // Index 6-8: accel measurement -- _IMU_tI__IMU [m/s^2]
constexpr uint32 PROPAGATE_PI_POS_IDX = 9;      // Index 9-11: position -- _pI_tI__pI [m]
 
constexpr uint32 PROPAGATE_OBSERVER_SIZE = 12;  // Total propagation observer/param vector elements
 
 
 
// ------------------------------------------------------------------------------------
// Sun Sensor Measurement Update Observer Vector Definitions
// ------------------------------------------------------------------------------------
 
constexpr uint32 SUN_EXPECTED_SUN_VEC_IDX = 0;  // Index 0-2: sun pointing vector -- _B_tI__pI [unitless]
constexpr uint32 SUN_ALIGNMENT_QUAT_IDX = 3;    // Index 3-6: quaternion -- _SUN_B [unitless]
 
constexpr uint32 SUN_OBSERVER_SIZE = 7;         // Total sun sensor update observer/param vector elements
 
 
 
// ------------------------------------------------------------------------------------
// Magnetometer Measurement Update Observer Vector Definitions
// ------------------------------------------------------------------------------------
 
constexpr uint32 MAG_EXPECTED_MAG_VEC_IDX = 0;  // Index 0-2: mag field vector -- _B_tI__pI [nT]
constexpr uint32 MAG_ALIGNMENT_QUAT_IDX = 3;    // Index 3-6: quaternion -- _MAG_B [unitless]
 
constexpr uint32 MAG_OBSERVER_SIZE = 3;         // Total magnetometer update observer/param vector elements
 
 
 
// ------------------------------------------------------------------------------------
// GPS Measurement Update Observer Vector Definitions
// ------------------------------------------------------------------------------------
 
constexpr uint32 GPS_REFERENCE_FRAME_QUAT_IDX = 0;  // Index 0-3: quaternion -- _pI_REF [unitless]
 
constexpr uint32 GPS_OBSERVER_SIZE = 4;             // Total GPS update observer/param vector elements
 
 
 
// ------------------------------------------------------------------------------------
// Process Noise Vector Definitions
// ------------------------------------------------------------------------------------
 
constexpr uint32 PROCESS_NOISE_POS_IDX = 0;         // Index 0–2: position std -- _B_pI__pI [m*sqrt(Hz)]
constexpr uint32 PROCESS_NOISE_VEL_IDX = 3;         // Index 3–5: velocity std -- _B_pI__pI [m/s*sqrt(Hz)]
constexpr uint32 PROCESS_NOISE_QUAT_IDX = 6;        // Index 6–9: quaternion std -- _B_pI [sqrt(Hz)]
constexpr uint32 PROCESS_NOISE_GYRO_BIAS_IDX = 10;  // Index 10–12: gyro bias std -- _B_pI__B [rad/s*sqrt(Hz)]
constexpr uint32 PROCESS_NOISE_ACCEL_BIAS_IDX = 13; // Index 13–15: accel bias std -- _B_pI__B [m/s^2*sqrt(Hz)]
 
constexpr uint32 PROCESS_NOISE_SIZE = 16;           // Total process noise vector elements
 
 
 
// ------------------------------------------------------------------------------------
// Sun Sensor Measurement Noise Vector Definitions
// ------------------------------------------------------------------------------------
 
constexpr uint32 SUN_NOISE_SUN_VEC_IDX = 0;     // Index 0-2: sun pointing vector std -- _B_tI__SUN [unitless]
 
constexpr uint32 SUN_NOISE_SIZE = 3;            // Total sun sensor measurement noise vector elements
 
 
 
// ------------------------------------------------------------------------------------
// Magnetometer Measurement Noise Vector Definitions
// ------------------------------------------------------------------------------------
 
constexpr uint32 MAG_NOISE_MAG_VEC_IDX = 0;     // Index 0-2: mag field vector std -- _B_tI__MAG [nT]
 
constexpr uint32 MAG_NOISE_SIZE = 3;            // Total magnetometer measurement noise vector elements
 
 
 
// ------------------------------------------------------------------------------------
// GPS Measurement Noise Vector Definitions
// ------------------------------------------------------------------------------------
 
constexpr uint32 GPS_NOISE_POS_IDX = 0;         // Index 0-2: GPS position vector std -- _GPS_tI__REF [m]
 
constexpr uint32 GPS_NOISE_SIZE = 3;            // Total gps measurement noise vector elements
 
 
 
// ------------------------------------------------------------------------------------
// Sun Sensor Measurement Vector Definitions
// ------------------------------------------------------------------------------------
 
constexpr uint32 SUN_MEAS_SUN_VEC_IDX = 0;      // Index 0-2: sun pointing vector -- _B_tI__B [unitless]
 
constexpr uint32 SUN_MEAS_SIZE = 3;             // Total sun sensor measurement vector elements
 
 
 
// ------------------------------------------------------------------------------------
// Magnetometer Measurement Vector Definitions
// ------------------------------------------------------------------------------------
 
constexpr uint32 MAG_MEAS_MAG_VEC_IDX = 0;      // Index 0-2: mag field vector -- _B_tI__B [nT]
 
constexpr uint32 MAG_MEAS_SIZE = 3;             // Total magnetometer measurement vector elements
 
 
 
// ------------------------------------------------------------------------------------
// GPS Measurement Vector Definitions //todo
// ------------------------------------------------------------------------------------
 
constexpr uint32 GPS_MEAS_POS_IDX = 0;          // Index 0-2: Position vector -- _B_pI__pI [m]
 
constexpr uint32 GPS_MEAS_SIZE = 3;             // Total GPS measurement vector elements
 
 
 
// ------------------------------------------------------------------------------------
// Typedefs
// ------------------------------------------------------------------------------------
 
using StateVector = std::array<floating_point, STATE_SIZE>;
using CovarianceMatrix = clockwerk::Matrix<STATE_SIZE, STATE_SIZE>;
using PropagateObsVector = std::array<floating_point, PROPAGATE_OBSERVER_SIZE>;
using SunObsVector = std::array<floating_point, SUN_OBSERVER_SIZE>;
using SunMeasVector = std::array<floating_point, SUN_MEAS_SIZE>;
using MagObsVector = std::array<floating_point, MAG_OBSERVER_SIZE>;
using MagMeasVector = std::array<floating_point, MAG_MEAS_SIZE>;
using GpsObsVector = std::array<floating_point, GPS_OBSERVER_SIZE>;
using GpsMeasVector = std::array<floating_point, GPS_MEAS_SIZE>;
 
} // namespace inertial_nav
 
#endif