EkfFocalAnglesMeasUpdate.h

/******************************************************************************
* 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.
******************************************************************************/
/*
Simple PID header file
 
Author: Alex Reynolds
*/
 
#ifndef GNC_NAVIGATION_EKF_EKF_FOCAL_ANGLES_MEAS_UPDATE_H
#define GNC_NAVIGATION_EKF_EKF_FOCAL_ANGLES_MEAS_UPDATE_H
 
#include "core/macros.h"
#include "architecture/Tasks.h"
#include "architecture/Time.h"
#include "core/CartesianVector.hpp"
#include "six_dof_dynamics/Quaternion.hpp"
 
#include "gnc/navigation/measurements/CalcFocalPlaneAngles.hpp"
#include "gnc/navigation/measurements/CalcFocalPlaneAnglesMatrix.hpp"
#include "gnc/navigation/ekf/EkfMeasurementUpdate.hpp"
 
namespace clockwerk {
    const std::array<double, 3> ZERO_VECTOR_OBS = {0.0, 0.0, 0.0};
 
    /**
     * @brief   EKF Measurement Update with focal plane angles
     * 
     * This function performs an EKF measurement update using provided
     * flocal plane (alpha/beta angles) information and an associated timestamp. The
     * task checks if the timestamp provided differs from previous
     * timestamps and only performs an update if the timestamp is 
     * different. 
     * 
     * This class comes with additional inputs to rotate/translate values in any frame
     * into the focal plane frame, and updates are performed there.
     * The focal frame is defined as:
     * Centered in the center of the reference image
     * X is "up" in the image
     * Y is "right" in the image
     * Z is "into" the image completing the right hand frame
     * 
     * |----------------------|
     * |         x^           |
     * |          |-->y       |
     * |                      |
     * |----------------------|
     * 
     * Output data includes the updated state and covariance as well 
     * as measurement residual and covariance information.
     * 
     * Author: Alex Reynolds <alex.reynolds@attx.tech>
    */
    class EkfFocalAnglesMeasUpdate : public clockwerk::Task {
    public:
        // Model params
        //         NAME                     TYPE                    DEFAULT VALUE
        START_PARAMS
            /** The 2D covariance matrix for the angles measurement [alpha, beta]*/
            SIGNAL(meas_covariance,         Matrix2D,               Matrix2D())
            /** The maximum alpha angle residual beyond which measurements will not be processed */
            SIGNAL(alpha_residual_filter,   double,                 1.0e10)
            /** The beta angle residual beyond which measurements will not be processed */
            SIGNAL(beta_residual_filter,    double,               1.0e10)
        END_PARAMS
 
        // Model inputs
        //         NAME                     TYPE                    DEFAULT VALUE
        START_INPUTS
            /** The time stamp associated with the range and rangerate update */
            SIGNAL(time_update,             clockwerk::Time,        clockwerk::Time())
            /** The range measurement which will be used in state update */
            SIGNAL(alpha,                   double,                 0.0)
            /** The range rate measurement which will be used in state update */
            SIGNAL(beta,                    double,                 0.0)
            /** The position estimate for the object before measurement update, expressed in the input frame */
            SIGNAL(pos_minus__i,            CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The velocity estimate for the object before measurement update, expressed in the input frame */
            SIGNAL(vel_minus__i,            CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The covariance matrix for the object before measurement update, expressed in the input frame */
            SIGNAL(cov_minus__i,            Matrix6D,               Matrix6D())
            /** The position of the observer relative to which measurement is taken, expressed in the input frame */
            SIGNAL(observer_pos__i,         CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The attitude of the focal frame wrt the observer frame expressed as a Quaternion */
            SIGNAL(observer_quat_f_i,       QuaternionD,            QuaternionD({1.0, 0.0, 0.0, 0.0}))
        END_INPUTS
 
        // Model outputs
        //         NAME                     TYPE                    DEFAULT VALUE
        START_OUTPUTS
            /** Flag to indicate whether measurement was processed (true) or not (false) */
            SIGNAL(meas_processed,          int,                    false)
            /** The position estimate for the object before measurement update, expressed in the input frame */
            SIGNAL(pos_plus__i,             CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The velocity estimate for the object before measurement update, expressed in the input frame */
            SIGNAL(vel_plus__i,             CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The covariance matrix for the object before measurement update, expressed in the input frame */
            SIGNAL(cov_plus__i,             Matrix6D,               Matrix6D())
            /** The residual in measurements as [range, range rate] prior to measurement update */
            SIGNAL(meas_pre_residual,       CartesianVector2D,      CartesianVector2D())
            /** The residual in measurements as [range, range rate] after measurement update */
            SIGNAL(meas_post_residual,      CartesianVector2D,      CartesianVector2D())
            /** The covariance of measurements prior to measurement update*/
            SIGNAL(meas_pre_covariance,     Matrix2D,               Matrix2D())
            /** The covariance of measurements after measurement update*/
            SIGNAL(meas_post_covariance,    Matrix2D,               Matrix2D())
        END_OUTPUTS
 
        // Model-specific implementations of startup and derivative
        EkfFocalAnglesMeasUpdate();
        EkfFocalAnglesMeasUpdate(clockwerk::Task &pnt, int schedule_slot=0, const std::string &m_name="ekf_focal_angles");
        EkfFocalAnglesMeasUpdate(clockwerk::Executive &e, int schedule_slot=0, const std::string &m_name="ekf_focal_angles");
        virtual ~EkfFocalAnglesMeasUpdate() {}
    protected:
        int start();
        int execute();
 
        // Measurement EKF functions and class
        CalcFocalPlaneAngles<double> _measurement_fun;
        CalcFocalPlaneAnglesMatrix<double> _H_meas_fun;
        EkfMeasurementUpdate<double, FOCAL_PLANE_MEAS_TARGET_STATES,
                             FOCAL_PLANE_MEASUREMENTS,
                             FOCAL_PLANE_MEAS_OBSERVER_STATES> _ekf_meas_update;
 
        // I/O to EKF
        std::array<double, FOCAL_PLANE_MEAS_TARGET_STATES> _state_input;
        std::array<double, FOCAL_PLANE_MEAS_TARGET_STATES> _state_output;
        std::array<double, FOCAL_PLANE_MEASUREMENTS> _measurement;
 
        // Measurement EKF variables
        Matrix<double, FOCAL_PLANE_MEASUREMENTS, FOCAL_PLANE_MEAS_TARGET_STATES> _H;
 
        // Variables to manage frame rotation from input into focal frame
        DCMD _dcm_f_i;
        DCMD _dcm_i_f;
        Matrix6D _full_state_rot_f_i;
        Matrix6D _full_state_rot_i_f;
        CartesianVector3D _pos__f;
        CartesianVector3D _vel__f;
        Matrix6D _cov_minus__f;
        Matrix6D _cov_plus__f;
 
        // Record of previous time
        clockwerk::Time _prev_time;
    };
}
 
#endif