EkfRangeRangerateMeasUpdate.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_RANGE_RANGERATE_MEAS_UPDATE_H
#define GNC_NAVIGATION_EKF_EKF_RANGE_RANGERATE_MEAS_UPDATE_H
 
#include "core/macros.h"
#include "architecture/Tasks.h"
#include "architecture/Time.h"
#include "core/CartesianVector.hpp"
 
#include "gnc/navigation/measurements/CalcRangeRangeRate.hpp"
#include "gnc/navigation/measurements/CalcRangeRangeRateMatrix.hpp"
#include "utils/RK4Integrator.hpp"
#include "gnc/navigation/ekf/EkfMeasurementUpdate.hpp"
 
namespace clockwerk {
    /**
     * @brief   EKF Measurement Update with Range and Range Rate
     * 
     * This function performs an EKF measurement update using provided
     * range and range rate 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. 
     * 
     * Output data includes the updated state and covariance as well 
     * as measurement residual and covariance information.
     * 
     * Author: Alex Reynolds <alex.reynolds@attx.tech>
    */
    class EkfRangeRangerateMeasUpdate : public clockwerk::Task {
    public:
        // Model params
        //         NAME                     TYPE                    DEFAULT VALUE
        START_PARAMS
            /** The covariance in the range measurement which will be used to construct the R matrix*/
            SIGNAL(range_covariance,        double,                 1.0e-10)
            /** The covariance in the range rate measurement which will be used to construct the R matrix*/
            SIGNAL(range_rate_covariance,   double,                 1.0e-10)
            /** The maximum range residual beyond which measurements will not be processed */
            SIGNAL(range_residual_filter,   double,                 1.0e10)
            /** The maximum range rate residual beyond which measurements will not be processed */
            SIGNAL(range_rate_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(range,                   double,                 0.0)
            /** The range rate measurement which will be used in state update */
            SIGNAL(range_rate,              double,                 0.0)
            /** The position estimate for the object before measurement update */
            SIGNAL(pos_minus,               CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The velocity estimate for the object before measurement update */
            SIGNAL(vel_minus,               CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The covariance matrix for the object before measurement update */
            SIGNAL(cov_minus,               Matrix6D,               Matrix6D())
            /** The position of the observer relative to which measurement is taken */
            SIGNAL(observer_pos,            CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The velocity of the observer relative to which measurement is taken */
            SIGNAL(observer_vel,            CartesianVector3D,      CartesianVector3D({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 */
            SIGNAL(pos_plus,                CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The velocity estimate for the object before measurement update */
            SIGNAL(vel_plus,                CartesianVector3D,      CartesianVector3D({0.0, 0.0, 0.0}))
            /** The covariance matrix for the object before measurement update */
            SIGNAL(cov_plus,                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
        EkfRangeRangerateMeasUpdate();
        EkfRangeRangerateMeasUpdate(clockwerk::Task &pnt, int schedule_slot=0, const std::string &m_name="ekf_range_rangerate");
        EkfRangeRangerateMeasUpdate(clockwerk::Executive &e, int schedule_slot=0, const std::string &m_name="ekf_range_rangerate");
        virtual ~EkfRangeRangerateMeasUpdate() {}
    protected:
        int start();
        int execute();
 
        // Measurement EKF functions and class
        CalcRangeRangeRate<double> _measurement_fun;
        CalcRangeRangeRateMatrix<double> _H_meas_fun;
        EkfMeasurementUpdate<double, RANGE_RANGERATE_TARGET_STATES,
                             RANGE_RANGERATE_MEASUREMENTS,
                             RANGE_RANGERATE_OBSERVER_STATES> _ekf_meas_update;
 
        // I/O to EKF
        std::array<double, 6> _state_input;
        std::array<double, 6> _observer_state;
        std::array<double, 6> _state_output;
        std::array<double, 2> _measurement;
 
        // Measurement EKF variables
        Matrix<double, RANGE_RANGERATE_MEASUREMENTS, RANGE_RANGERATE_TARGET_STATES> _H;
        Matrix<double, RANGE_RANGERATE_MEASUREMENTS, RANGE_RANGERATE_MEASUREMENTS> _R;
 
        // Record of previous time
        clockwerk::Time _prev_time;
    };
}
 
#endif