EkfMeasurementUpdate.hpp

/******************************************************************************
* 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.
******************************************************************************/
 
#ifndef GNC_NAVIGATION_EKF_EKF_MEASUREMENT_UPDATE_HPP
#define GNC_NAVIGATION_EKF_EKF_MEASUREMENT_UPDATE_HPP
 
#include "core/Matrix.hpp"
#include "core/CartesianVector.hpp"
#include "core/matrixmath.hpp"
#include "core/vectormath.hpp"
#include "utils/Measurements.hpp"
#include "utils/Integrator.hpp"
 
namespace clockwerk {
 
    /**
     * @brief Generic, templated EKF measurement update class
     * 
     * This class performs an EKF measurement update using the current
     * target state, an "observer state" which provides information (i.e. 
     * ground station information), and the associated measurement and 
     * H matrix functions. It requires three templated arguments, which
     * are as follows:
     * N - The number of target states estimated in the EKF
     * M - The number of measurement states provided
     * O - The number of observer states used to support measurement generation
     * 
     * Two "functions" which take the form of a Measurements class must
     * also be set: The function which generates measurements, and the
     * function which generates the H matrix.
     * 
     * To run the measurement update, both functions are required. The first
     * generates measurement residuals, and the second to actuall provide
     * the state update
     */
    template <typename T, unsigned int N, unsigned int M, unsigned int O>
    class EkfMeasurementUpdate {
    public:
        /// @brief Function to generate residual and H matrix from current state
        /// @param state The current state of the filter
        /// @param obs_state An observer state used to generate measurements
        /// @param measurement The actual measurement derived from sensors
        /// @param residual Implicit return of the difference measurement - estimated measurement
        /// @param H Implicit return of the measurement sensitivity matrix for the system
        /// @return Error code corresponding to success/failure
        int generateResidualHMatrix(const std::array<T, N> &state, const std::array<T, O> &obs_state,
                                    const std::array<T, M> &measurement, CartesianVector<T, M> *residual,
                                    Matrix<T, M, N> *H);
 
        /// @brief Function to perform state measurement update in EKF
        /// @param residual The difference measurement - estimated measurement
        /// @param H The measurement sensitivity matrix for the system
        /// @param state_minus The system state prior to measurement update
        /// @param cov_minus The system covariance prior to measurement update
        /// @param meas_cov The covariance of the measurement
        /// @param state_plus Implicit return of the state of the system after measurement update
        /// @param cov_plus Implicit return of the covariance of the system after measurement update
        /// @return Error code corresponding to success/failure
        int updateState(const CartesianVector<T, M> &residual, const Matrix<T, M, N> &H,
                        const std::array<T, N> &state_minus, const Matrix<T, N, N> &cov_minus,
                        const Matrix<T, M, M> &meas_cov, std::array<T, N> *state_plus,
                        Matrix<T, N, N> *cov_plus);
 
        /// @brief Function to generate covariance for measurement update evaluation
        /// @param H The measurement sensitivity matrix for the system
        /// @param state_cov The covariance in the system state
        /// @param meas_cov The covariance associated with the measurement
        /// @param residual_cov Implicit return of the expected covariance of the residual
        /// @return Error code corresponding to success/failure
        int calculateMeasurementCovariance(const Matrix<T, M, N> &H, const Matrix<T, N, N> &state_cov,
                                           const Matrix<T, M, M> &meas_cov, Matrix<T, M, M> *residual_cov);
 
        /// Pointer to the measurement class to calculate the estimated measurement given the current
        /// state of the object. The measurement class should be configured to use size 
        Measurements<T, N, M, O>* measurement_ptr = nullptr;
 
        /// Pointer to the measurement class to calculate the H matrix from the estimated
        /// spacecraft state
        Measurements<T, N, M*N, O>* H_ptr = nullptr;
    private:
        // Measurement vector from estimated state for internal use
        std::array<T, M> _est_meas;
 
        // H matrix for internal use
        std::array<T, M*N> _tmp_H_array;
        Matrix<T, N, M> _H_tspose;
 
        // K matrix for internal use
        Matrix<T, M, M> _tmp;
        Matrix<T, M, M> _tmp_inv;
        Matrix<T, N, M> _K;
 
        // Update vector for internal use
        CartesianVector<T, N> _update;
 
        // Update matrix for covariance
        Matrix<T, N, N> _tmp_cov;
    };
 
    template<typename T, unsigned int N, unsigned int M, unsigned int O>
    int EkfMeasurementUpdate<T, N, M, O>::generateResidualHMatrix(const std::array<T, N> &state, const std::array<T, O> &obs_state,
                                                                  const std::array<T, M> &measurement, CartesianVector<T, M> *residual,
                                                                  Matrix<T, M, N> *H) {
        // Verify that our measurement functions are set
        if(measurement_ptr == nullptr || H_ptr == nullptr) {
            return ERROR_NULLPTR;
        }
 
        if(residual == nullptr || H == nullptr) {
            return ERROR_NULLPTR;
        }
 
        // Calculate our estimated measurement and our H from our state
        int err = measurement_ptr->calculateMeasurements(T(), state, obs_state, _est_meas);
        if(err) {return err;}
 
        err = H_ptr->calculateMeasurements(T(), state, obs_state, _tmp_H_array);
 
        if(err) {return err;}
        H->setFromArray(&_tmp_H_array[0]);
 
        // Calculate our residual
        for(unsigned int i = 0; i < M; i++) {
            (*residual)[i] = measurement[i] - _est_meas[i];
        }
 
        return NO_ERROR;
    }
 
    template<typename T, unsigned int N, unsigned int M, unsigned int O>
    int EkfMeasurementUpdate<T, N, M, O>::updateState(const CartesianVector<T, M> &residual, const Matrix<T, M, N> &H,
                                                      const std::array<T, N> &state_minus, const Matrix<T, N, N> &cov_minus,
                                                      const Matrix<T, M, M> &meas_cov, std::array<T, N> *state_plus,
                                                      Matrix<T, N, N> *cov_plus) {
 
        if(state_plus == nullptr || cov_plus == nullptr) {
            return ERROR_NULLPTR;
        }
 
        // First invert our H matrix
        H.transpose(_H_tspose);
 
        // Now do some math to calculate our K matrix
        _tmp = H*cov_minus*_H_tspose + meas_cov;
        int err = _tmp.inverse(_tmp_inv); if(err) {return err;}
        _K = cov_minus*_H_tspose*_tmp_inv;
 
        // Calculate our update from our innovation
        _update = _K*residual;
        for(unsigned int i = 0; i < N; i++) {
            (*state_plus)[i] = state_minus[i] + _update[i];
        }
 
        // Update our covariance
        _tmp_cov.eye();
        _tmp_cov = _tmp_cov - _K*H;
        (*cov_plus) = _tmp_cov*cov_minus*_tmp_cov.transpose();
 
        return NO_ERROR;
    }
 
    template<typename T, unsigned int N, unsigned int M, unsigned int O>
    int EkfMeasurementUpdate<T, N, M, O>::calculateMeasurementCovariance(const Matrix<T, M, N> &H, const Matrix<T, N, N> &state_cov,
                                                                         const Matrix<T, M, M> &meas_cov, Matrix<T, M, M> *residual_cov) {
        if(residual_cov == nullptr) {return ERROR_NULLPTR;}
        H.transpose(_H_tspose);
        *residual_cov = H*state_cov*_H_tspose + meas_cov;
        return NO_ERROR;
    }
 
}
 
#endif