EkfTimeUpdate.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_TIME_UPDATE_HPP
#define GNC_NAVIGATION_EKF_EKF_TIME_UPDATE_HPP
 
#include "core/Matrix.hpp"
#include "core/CartesianVector.hpp"
#include "core/matrixmath.hpp"
#include "core/vectormath.hpp"
#include "utils/Rates.hpp"
#include "utils/Integrator.hpp"
 
namespace clockwerk {
 
    /**
     * @brief Class to perform EKF time update step
     */
    template <typename T, unsigned int N>
    class EkfTimeUpdate {
    public:
        /// @brief Function to run a single extended kalman filter time update step
        /// @param time_prev The time at which the previous state and covariance are known
        /// @param state_prev The previous state as an N element state vector
        /// @param cov_prev The previous covariance as an N*N matrix
        /// @param time_up The time to which the state and covariance will be propagated
        /// @param state_up The updated state at time time_up
        /// @param cov_up The updated covariance at time time_up
        /// @return Error code corresponding to success/failure
        int runUpdate(T time_prev, const std::array<T, N> &state_prev, const Matrix<T, N, N> &cov_prev,
                      T time_up, std::array<T, N> *state_up, Matrix<T, N, N> *cov_up);
 
        /// The integrator to be used internally by the EKF stepper.
        /// This integrator should be pre-configured to use a rates
        /// function with size N + N*N to propagate dynamics
        /// In most cases should probably be RK4
        Integrator<T, N + N*N>* integrator_ptr = nullptr;
 
        /// @brief Optional return of the phi matrix calculated by EKF
        Matrix<T, N, N> phi;
    private:
        std::array<T, N + N*N> _full_state_initial;
        std::array<T, N + N*N> _full_state_post;
        Matrix<T, N, N> _phi_transpose;
    };
 
    template<typename T, unsigned int N>
    int EkfTimeUpdate<T, N>::runUpdate(T time_prev, const std::array<T, N> &state_prev, const Matrix<T, N, N> &cov_prev,
                                       T time_up, std::array<T, N> *state_up, Matrix<T, N, N> *cov_up) {
        // First ensure our rates and integrator are valid
        if(integrator_ptr == nullptr) {
            return ERROR_NULLPTR;
        }
 
        // Now generate a full state vector including our phi matrix
        unsigned int one_idx = N;
        for(unsigned int i = 0; i < N + N*N; i++) {
            if(i < N) {
                _full_state_initial[i] = state_prev[i];
            } else if(i == one_idx) {
                _full_state_initial[i] = 1.0;
                one_idx = one_idx + N + 1;
            } else {
                _full_state_initial[i] = 0.0;
            }
        }
 
        // Now propagate
        integrator_ptr->step(time_prev, time_up, _full_state_initial, _full_state_post);
 
        // Now extract our Phi matrix, then use it to propagate forward covariance
        phi.setFromArray(&_full_state_post[N]);
        phi.transpose(_phi_transpose);
 
        // Finally, set our output state and updated covariance matrix
        for(unsigned int i = 0; i < N; i++) {
            (*state_up)[i] = _full_state_post[i];
        }
        *cov_up = phi*cov_prev*_phi_transpose;
 
        return NO_ERROR;
    }
 
}
 
#endif