 |
ModelSpace
Documentation for ModelSpace models and classes.
|
|
ModelSpace
Documentation for ModelSpace models and classes.
|
/******************************************************************************
* 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 "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/RK4Integrator.hpp"
#include "gncutils/EKF/EkfTimeUpdate.hpp"
#include "apps/gnc/InertialNavigationEKF/InertialNavigationDefinition.hpp"
namespace cfspp {
/**
* @brief GNC App for performing the propagation step of an EKF when an IMU is available to report the
* angular velocity and acceleration (without gravity). This app is an implementation if a
* strapdown IMU based dead-reckoning integration of an objects position, velocity, and attitude
* relative to some inertial frame (this frame can be pseudoinertial, use your best judgement).
* This form of propagation step is best used if there are significant external/internal forces
* other than gravity acting on the system, also forces that cannot be modeled easily as functions
* of the state vector.
*
* Author: James Tabony <james.tabony@attx.tech>
*/
class DeadReckon : public App {
public:
START_PARAMS
/** The position of the pseudo-inertial (pI) frame with respect to the true inertial (tI) frame
* as provided in pseudo-inertial (pI) frame coordinates. Defaults to zero because this is the
* usual case, e.g. using ECEF or ECI. The true inertial frame origin is defined to be where
* the primary bodies gravitational potential singularity exists. (meters) */
SIGNAL(pos_pI_tI__pI, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
/** The maximum time difference that is allowed between the input state time stamp (t_{k}) and
* the measurement time stamp as well as the maximum time difference between the output state
* time stamp and the measurement time stamp. This effectively limits the maximum difference
* between the input and output times to be twice the param value. The EKF will still propagate
* the state, but the error may be significant and a warning will be thrown. Defaults to 5 (seconds). */
SIGNAL(time_diff_warning_bound, clockwerk::Time, clockwerk::Time(5, 0))
// ***** PROCESS NOISE PARAMS ***** //
/** The velocity component of state vector process noise standard deviation. See the velocity
* component definition in (inputs/outputs) to understand the frame of reference and relative
* state. The process noise standard deviation is set on system start-up, but can be changed
* via a command during any part of operation. (sqrt(Hertz)) */
SIGNAL(velocity_standard_deviation, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
/** The attitude component of state vector process noise standard deviation. See the attitude
* component definition in (inputs/outputs) to understand the frame of reference and relative
* state. The process noise standard deviation is set on system start-up, but can be changed
* via a command during any part of operation. (sqrt(Hertz)) */
SIGNAL(attitude_standard_deviation, clockwerk::CartesianVector<4>, clockwerk::CartesianVector<4>({0.0, 0.0, 0.0, 0.0}))
/** The gyroscope measurement bias component of state vector process noise standard deviation.
* See the gyroscope measurement bias component definition in (inputs/outputs) to understand
* the frame of reference and relative state. The process noise standard deviation is set on
* system start-up, but can be changed via a command during any part of operation. (sqrt(Hertz)) */
SIGNAL(gyroBias_standard_deviation, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
/** The accelerometer measurement bias component of state vector process noise standard deviation.
* See the accelerometer measurement bias component definition in (inputs/outputs) to understand
* the frame of reference and relative state. The process noise standard deviation is set on
* system start-up, but can be changed via a command during any part of operation. (sqrt(Hertz)) */
SIGNAL(accelBias_standard_deviation,clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
// ***** IMU LOCATION PARAMS ***** //
/** The position of the IMU (IMU) frame with respect to the body (B) frame as provided in the
* body (B) frame coordinates. It should be noted that the body frame is assumed to have its
* origin coinciding with the object center of gravity. (meters) */
SIGNAL(pos_IMU_B__B, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
/** The attitude of the IMU (IMU) frame with respect to the body (B) frame as provided as a
* unit quaternion. Rotation from body frame to IMU frame. (unitless) */
SIGNAL(quat_IMU_B, clockwerk::Quaternion, clockwerk::Quaternion({1.0, 0.0, 0.0, 0.0}))
END_PARAMS
START_INPUTS
// ***** STATE INPUT DEFINITIONS ***** //
/** The position of the body (B) with respect to the pseudo-inertial (pI) frame as provided in
* pseudo-inertial (pI) frame coordinates. This value is part of the inertial navigation state
* vector and should somewhat align with the time of the incoming IMU measurement. (meters) */
SIGNAL(pos_B_pI__pI, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
/** The velocity of the body (B) with respect to the pseudo-inertial (pI) frame as provided in
* pseudo-inertial (pI) frame coordinates. This value is part of the inertial navigation state
* vector and should somewhat align with the time of the incoming IMU measurement. (meters/second) */
SIGNAL(vel_B_pI__pI, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
/** The attitude of the body (B) frame with respect to the pseudo-inertial (pI) frame as provided
* as a unit quaternion. This value is part of the inertial navigation state vector and should
* somewhat align with the time of the incoming IMU measurement. (unitless) */
SIGNAL(quat_B_pI, clockwerk::Quaternion, clockwerk::Quaternion({1.0, 0.0, 0.0, 0.0}))
/** The gyrocsope bias provided as an angular velocity of the body (B) frame with respect to
* the angular velocity of the pseudo-inertial (pI) frame as provided in pseudo-inertial (pI)
* frame coordinates. This value is part of the inertial navigation state vector and should
* somewhat align with the time of the incoming IMU measurement. (radians/second) */
SIGNAL(gyroBias_B_pI__pI, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
/** The accelerometer bias provided as an acceleration of the body (B) frame with respect to
* the acceleration of the pseudo-inertial (pI) frame as provided in pseudo-inertial (pI) frame
* coordinates. This value is part of the inertial navigation state vector that and should somewhat
* with the time of the incoming IMU measurement. (meters/seconds^2) */
SIGNAL(accelBias_B_pI__pI, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
/** The covariance matrix of the state vector. The covariances of the state random variable are defined
* diagonally in the order of [pos, vel, quat, gyroBias, accelBias] and are in the same units, frames,
* and relativistic states as the state vector definitions. */
SIGNAL(state_cov, inertial_nav::CovarianceMatrix, inertial_nav::CovarianceMatrix())
/** Time stamp of the input state. The time does not need to be measured relative to anything specific
* as long as all time stamps are measured relative to the same epoch, e.g. mission start, J2000, MJD.
* (seconds) */
SIGNAL(state_input_time_stamp, clockwerk::Time, clockwerk::Time(0, 0))
// ***** IMU MEASUREMENT INPUT DEFINITIONS ***** //
/** The biased angular velocity measurement provided as an angular velocity of the IMU (IMU) frame
* with respect to the true inertial (tI) frame as provided in IMU (IMU) frame coordinates. This
* value should be a direct output of the IMU gyroscope. (radians/second) */
SIGNAL(gyroMeas_IMU_tI__IMU, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
/** The biased acceleration measurement provided as an acceleration (minus gravity) experienced by
* the IMU (IMU) frame with respect to the true inertial (tI) frame as provided in IMU (IMU) frame
* coordinates. This value should be a direct output of the IMU accelerometer. (meters/second^2) */
SIGNAL(accelMeas_IMU_tI__IMU, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
/** Time stamp of the incoming measurement. The time does not need to be measured relative to anything
* specific as long as all time stamps are measured relative to the same epoch, e.g. mission start,
* J2000, MJD. (seconds) */
SIGNAL(IMU_meas_time_stamp, clockwerk::Time, clockwerk::Time(0, 0))
/** Boolean flag denoting the validity of the incoming IMU measurement, comes from A&R checks in the
* IMU SOP. If only one or both of the sensors report a faulty measurement, the boolean should be
* false (bad measurement). If both sensors report an accurate measurement, the boolean should be
* true (good measurement). */
SIGNAL(IMU_meas_validity_flag, bool, false)
// ***** ADDITIONAL INPUT DEFINITIONS ***** //
/** Acceleration of the body (B) frame due to gravitational potential energy as provided in pseudo-
* inertial (pI) frame coordinates. (meters/second^2) */
SIGNAL(accelGrav_body__pI, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
// ***** OUTPUT DEFINING INPUTS DEFINITIONS ***** //
/** Time stamp of the output state, this is the time that the state is to be propagated to. The time
* does not need to be measured relative to anything specific as long as all time stamps are measured
* relative to the same epoch, e.g. mission start, J2000, MJD. (seconds) */
SIGNAL(state_output_time_stamp, clockwerk::Time, clockwerk::Time(0, 0));
END_INPUTS
START_OUTPUTS
/** The position of the body (B) with respect to the pseudo-inertial (pI) frame as provided in
* pseudo-inertial (pI) frame coordinates. This value is part of the inertial navigation state
* vector and is aligned with the time of output - time_in. (meters) */
SIGNAL(pos_B_pI__pI, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
/** The velocity of the body (B) with respect to the pseudo-inertial (pI) frame as provided in
* pseudo-inertial (pI) frame coordinates. This value is part of the inertial navigation state
* vector and should somewhat align with the time of the incoming IMU measurement. (meters/second) */
SIGNAL(vel_B_pI__pI, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
/** The attitude of the body (B) frame with respect to the pseudo-inertial (pI) frame as provided
* as a unit quaternion. This value is part of the inertial navigation state vector and should
* somewhat align with the time of the incoming IMU measurement. (unitless) */
SIGNAL(quat_B_pI, clockwerk::Quaternion, clockwerk::Quaternion({1.0, 0.0, 0.0, 0.0}))
/** The gyrocsope bias provided as an angular velocity of the body (B) frame with respect to
* the angular velocity of the pseudo-inertial (pI) frame as provided in pseudo-inertial (pI)
* frame coordinates. This value is part of the inertial navigation state vector and should
* somewhat align with the time of the incoming IMU measurement - time_in. (radians/second) */
SIGNAL(gyroBias_B_pI__pI, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
/** The accelerometer bias provided as an acceleration of the body (B) frame with respect to
* the acceleration of the pseudo-inertial (pI) frame as provided in pseudo-inertial (pI) frame
* coordinates. This value is part of the inertial navigation state vector that and should somewhat
* with the time of the incoming IMU measurement. (meters/seconds^2) */
SIGNAL(accelBias_B_pI__pI, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
/** The covariance matrix of the state vector. The covariances of the state random variable are defined
* diagonally in the order of [pos, vel, quat, gyroBias, accelBias] and are in the same units, frames,
* and relativistic states as the state vector definitions. */
SIGNAL(state_cov, inertial_nav::CovarianceMatrix, inertial_nav::CovarianceMatrix())
END_OUTPUTS
DeadReckon(FlightExecutive &executive);
virtual ~DeadReckon() {};
/// @brief Activate the app. The app will step when active.
/// @return Flag indicating success/failure
int16 activate() override;
/// @brief Deactivate the app. The app will not step when deactivated
/// @return Flag indicating success/failure
int16 deactivate() override;
/// @brief Process commands issued to the app
/// @param apid The APID of the command sent
/// @param buffer Pointer to the location of the buffer holding the command
/// @param size The size of the command being sent
/// @return Flag indicating success/failure
int16 command(uint16 apid, uint8* buffer, uint16 size) override;
/// @brief Getter function to see the internally stored last valid measurement
/// @param[out] gyroMeas_IMU_tI__IMU Gyroscope measurement
/// @param[out] accelMeas_IMU_tI__IMU Accelerometer measurement
/// @param[out] time_stamp Time stamp of the measurement
/// @return Flag indicating success/failure
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;
/// Reference variable to the internal Dynamics
InertialNavigationDynamics _dynamics;
/// Reference variable to the internal Integrator
RK4Integrator<INERTIAL_NAVIGATION_STATE_VECTOR_ELEMENTS+INERTIAL_NAVIGATION_STATE_VECTOR_ELEMENTS*INERTIAL_NAVIGATION_STATE_VECTOR_ELEMENTS+INERTIAL_NAVIGATION_OBSERVER_VECTOR_ELEMENTS> _integrator;
/// Reference variable to the internal EkfTimeUpdate
EkfTimeUpdate<INERTIAL_NAVIGATION_STATE_VECTOR_ELEMENTS, INERTIAL_NAVIGATION_OBSERVER_VECTOR_ELEMENTS, INERTIAL_NAVIGATION_PROCESS_NOISE_ELEMENTS> _timeUpdate;
/// Dummy temporary array for swapping between std::array and clockwerk::CartesianVector
std::array<floating_point, 3> _dummy3; std::array<floating_point, 4> _dummy4; inertial_nav::PropagateObsVector _dummyObs; inertial_nav::CovarianceMatrix _dummyCov;
/// Internally stored last valid measurement
clockwerk::CartesianVector<3> _last_valid_gyro_meas, _last_valid_accel_meas; floating_point _last_valid_meas_time_stamp;
/// Boolean value to track if a valid measurement has been internally saved yet
bool _valid_measurement_saved = false;
/// Temporary variables for the array input/outputs passed into the EkfTimeUpdate object
inertial_nav::StateVector _state_input_array, _state_output_array; inertial_nav::PropagateObsVector _state_observer_array; inertial_nav::CovarianceMatrix _state_cov_output;
/// Internal variables for the transformed measurements
clockwerk::CartesianVector<3> _gyroMeas_B_pI__B, _accelMeas_B_pI__B;
/// Internally saved approximated angular acceleration
clockwerk::CartesianVector<3> _approximate_ang_accel = clockwerk::CartesianVector<3>({0.0, 0.0, 0.0});
/// Temporary variable for unitizing a quaternion
clockwerk::Quaternion _unit_quat;
/// Packet to hold state after propagation
tlm_gnc_inert_prop_state _tlm_a_priori_state;
};
}
#endif 
1.9.1