 |
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: GPS Measurement Update Header File
Author: James Tabony
*/
#ifndef APPS_GNC_INERTIAL_NAV_GPS_UPDATE_APP_H
#define APPS_GNC_INERTIAL_NAV_GPS_UPDATE_APP_H
#include "flight/App.h"
#include "flight/FlightExecutive.h"
#include "telemetry/tlm_InertialNavigationGPSMeasurementUpdate.h"
#include "command/cmd_InertialNavigationGPSMeasurementUpdate.h"
#include "gncutils/EKF/GPSMeasurements.hpp"
#include "gncutils/EKF/EkfMeasurementUpdate.hpp"
#include "apps/gnc/InertialNavigationEKF/InertialNavigationDefinition.hpp"
namespace cfspp{
/**
* @brief GNC App for performing the GPS measurement update step of an EKF when a GPS receiver is
* available to report the vehicle position in a known reference frame (e.g., ECEF, ECI, or NED).
* This app implements the EKF measurement correction step by comparing the measured GPS position
* (after necessary frame and lever-arm transformations) to the predicted position from the EKF state.
*
* This update step is primarily used to correct drift in position and velocity estimates that accumulate
* during IMU-only dead-reckoning propagation. By incorporating absolute GPS position fixes, the EKF can
* bound long-term error growth in navigation states.
*
* The method assumes that the GPS receiver outputs position in a global reference frame (commonly ECEF
* or ECI) and that the transformation between the GPS antenna phase center, the body frame, and the EKF
* state reference frame is known. Corrections account for lever-arm offsets between the GPS sensor and
* the body center of gravity as well as orientation differences between the reporting frame and the
* pseudo-inertial frame used by the EKF.
*
* @attention See InertialNavigationDefinition.hpp for an in-depth description of the state vector and
* frame conventions used in this measurement update.
*
* Author: James Tabony <james.tabony@attx.tech>
*/
class GPSUpdate : public App {
public:
START_PARAMS
/** The maximum time difference that is allowed between the input state time stamp (t_{k}) and
* the measurement time stamp. The EKF will not incorporate a measurement that is too stale.
* Defaults to 5 (seconds). */
SIGNAL(time_diff_stale_bound, clockwerk::Time, clockwerk::Time(1, 0))
/** The GPS measurement noise standard deviation. See the position 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. (meters) */
SIGNAL(pos_vec_standard_deviation, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
/** 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 position of the GPS receiver with resepct to the body (B) frame resolved in body frame coordinates.
* Defaults to zero which is a decent approximation for small vehicles but is not a negligible value. (meters) */
SIGNAL(pos_GPS_B__B, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({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 MAG 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 MAG 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 MAG 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 repsect to
* the angular velocity of the pseudo-inertial (pI) frame as provided in pseudo-inertial (pI)
* frame coordiantes. This value is part of the inertial navigation state vector and should
* somewhat align with the time of the incomming MAG 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 MAG 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))
// ***** GPS MEASUREMENT INPUT DEFINITIONS ***** //
/** The biased GPS measurement provided as a position of the GPS receiver with respect to some reference frame
* resolved in that reference (REF) frame. This value should be a direct output of the GPS. (meters) */
SIGNAL(GPS_meas_vec__REF, 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(GPS_meas_time_stamp, clockwerk::Time, clockwerk::Time(0, 0))
/** Boolean flag denoting the validity of the incoming GPS measurement, comes from A&R checks in the
* GPS SOP. If the sensor reports a faulty measurement, the boolean should be false (bad measurement).
* If sensor reports an accurate measurement, the boolean should be true (good measurement). */
SIGNAL(GPS_meas_validity_flag, bool, false)
// ***** ADDITIONAL INPUT DEFINITIONS ***** //
/** The attitude of the pseudo-inertial (pI) frame with respect to the GPS measurement (REF) frame
* as provided as a unit quaternion. Rotation from measurement frame to pseudo-inertial frame. (unitless) */
SIGNAL(quat_pI_REF, clockwerk::Quaternion, clockwerk::Quaternion({1.0, 0.0, 0.0, 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 post measurement incorporation. (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 is post measurement incorporation. (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 is post
* measurement incorporation. (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 repsect to
* the angular velocity of the pseudo-inertial (pI) frame as provided in pseudo-inertial (pI)
* frame coordiantes. This value is part of the inertial navigation state vector and is post
* measurement incorporation. (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 is post
* measurement incorporation. (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. This value is post measurement incorporation. */
SIGNAL(state_cov, inertial_nav::CovarianceMatrix, inertial_nav::CovarianceMatrix())
END_OUTPUTS
GPSUpdate(FlightExecutive &executive);
virtual ~GPSUpdate() {};
/// @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 grab the internal residual
/// @return Measurement residual
inertial_nav::MagMeasVector getResidual() {return _residual;}
protected:
int16 start() override;
int16 execute() override;
/// Reference variable to the internal Measurements
GPSMeasurements<inertial_nav::STATE_SIZE> _measurements;
/// Reference variable to the internal EkfMeasurementsUpdate
EkfMeasurementUpdate<inertial_nav::STATE_SIZE, GPS_MEASUREMENT_VECTOR_ELEMENTS,
GPS_MEASUREMENT_OBSERVER_VECTOR_ELEMENTS, GPS_MEASUREMENT_MEASUREMENT_NOISE_ELEMENTS> _measUpdate;
/// 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::MagObsVector _dummyObs; inertial_nav::CovarianceMatrix _dummyCov;
/// Temporary variables for the array input/outputs passed into the EkfMeasurementUpdate object
inertial_nav::StateVector _state_input_array, _state_output_array;
/// Temporary variables for the array input/outputs passed into the EkfMeasurementUpdate object
inertial_nav::GpsObsVector _state_observer_array;
/// Temporary variables for the array input/outputs passed into the EkfMeasurementUpdate object
inertial_nav::CovarianceMatrix _state_cov_output;
/// Temporary variables for the array input/outputs passed into the EkfMeasurementUpdate object
inertial_nav::GpsMeasVector _measurement_array, _residual;
/// Internal variables for the transformed measurements
clockwerk::CartesianVector<3> _gps_meas_vec_B_pI__pI;
/// Temporary variable for unitizing a quaternion
clockwerk::Quaternion _unit_quat;
/// Packet to hold state after measurement incorporation
tlm_gnc_gps_update_state _tlm_a_posteriori_state;
};
}
#endif
1.9.1