 |
WarpTwin
Documentation for WarpTwin models and classes.
|
|
WarpTwin
Documentation for WarpTwin 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: Magnetometer Measurement Update Header File
Author: James Tabony
*/
#ifndef APPS_GNC_INERTIAL_NAV_MAG_UPDATE_APP_H
#define APPS_GNC_INERTIAL_NAV_MAG_UPDATE_APP_H
#include "core/mathmacros.h"
#include "flight/App.h"
#include "flight/FlightExecutive.h"
#include "telemetry/tlm_InertialNavigationMagnetometerMeasurementUpdate.h"
#include "command/cmd_InertialNavigationMagnetometerMeasurementUpdate.h"
#include "gncutils/EKF/PointingVectorMeasurements.hpp"
#include "gncutils/EKF/EkfMeasurementUpdate.hpp"
#include "apps/gnc/InertialNavigationEKF/InertialNavigationDefinition.hpp"
namespace warpos{
/**
* @brief GNC App for performing the magnetometer measurement update step of an EKF when a 3-axis
* magnetometer is available to report the local magnetic field vector. This app implements the
* EKF measurement correction step by comparing the measured magnetic field in the magnetometer frame
* to the expected magnetic field derived from an internal model (e.g., WMM, IGRF, lookup table) and
* the current state estimate.
*
* This update step is primarily used to improve the estimation of vehicle attitude, particularly in
* scenarios where gyroscope bias and drift can degrade the accuracy of dead-reckoning propagation.
* The method assumes that the magnetic field vector at the vehicle's location is known or can be
* reasonably approximated in a reference frame (e.g., ECEF or NED), and that the transformation
* between the magnetometer frame and the body frame is known.
*
* @attention See InertialNavigationDefinition.hpp for in depth description
*
* TODO: Need to add params for magnetometer shapping ellipse or magnetometer bias
*
* Author: James Tabony <james.tabony@attx.tech>
*/
class MagUpdate : 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(5, 0))
/** The magnetometer measurement noise standard deviation. See the mag field vector 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. (nT) */
SIGNAL(mag_meas_noise, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
/** The attitude of the MAG (MAG) frame with respect to the body (B) frame as provided as a
* unit quaternion. Rotation from body frame to MAG frame. (unitless) */
SIGNAL(quat_MAG_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 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, Matrix16, Matrix16())
/** 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))
// ***** MAG MEASUREMENT INPUT DEFINITIONS ***** //
/** The biased magnetic field measurement provided as a magnetic field vector experienced at the MAG (MAG) frame
* as provided in MAG (MAG) frame coordinates. This value should be a direct output of the magnetometer. (nT) */
SIGNAL(mag_meas_vec__MAG, 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(MAG_meas_time_stamp, clockwerk::Time, clockwerk::Time(0, 0))
/** Boolean flag denoting the validity of the incoming MAG measurement, comes from A&R checks in the
* MAG 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(MAG_meas_validity_flag, bool, false)
// ***** ADDITIONAL INPUT DEFINITIONS ***** //
/** The expected magnetic field measurement provided as a magnetic field vector experienced at the MAG (MAG) frame
* as provided in reference (REF) frame coordinates. This value shoud be determined externally in a model. (nT) */
SIGNAL(expected_mag_vec__REF, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
/** The attitude of the pseudo-inertial (pI) frame with respect to the reference frame of the external magnetic field
* model as provided by a unit quaternion. (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, Matrix16, Matrix16())
/** Time stamp of the output 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_output_time_stamp, clockwerk::Time, clockwerk::Time(0, 0))
/** The pre update measurement residual */
SIGNAL(pre_residual, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
/** The post update measurement residual */
SIGNAL(post_residual, clockwerk::CartesianVector<3>, clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
END_OUTPUTS
MagUpdate(FlightExecutive &executive);
virtual ~MagUpdate() {};
/// @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
PointingVectorMeasurements<inertial_nav::STATE_SIZE> _measurements;
/// Reference variable to the internal EkfMeasurementsUpdate
EkfMeasurementUpdate<inertial_nav::STATE_SIZE, POINTING_MEASUREMENT_MEASUREMENT_VECTOR_ELEMENTS,
POINTING_MEASUREMENT_OBSERVER_VECTOR_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;
/// 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::MagObsVector _state_observer_array;
/// Temporary variables for the array input/outputs passed into the EkfMeasurementUpdate object
inertial_nav::MagMeasVector _measurement_array, _residual;
/// Internal variables for the transformed measurements
clockwerk::CartesianVector<3> _expected_mag_vec__pI, _mag_meas_vec__B;
/// Temporary variable for unitizing a quaternion
clockwerk::Quaternion _unit_quat;
/// Packet to hold state after measurement incorporation
tlm_gnc_mag_update_state _tlm_a_posteriori_state;
/// Variables to verify we are not processing stale measurements
clockwerk::Time _repeat_check_time;
CartesianVector3 _repeat_check_meas;
};
}
#endif
1.16.1