EkfDynamics.hpp

Go to the documentation of this file.

/******************************************************************************
* 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 GNCUTILS_EKF_EKFDYNAMICS_HPP
#define GNCUTILS_EKF_EKFDYNAMICS_HPP
 
#include "gncutils/dynamics/Rates.hpp"
#include "core/matrixmath.hpp"
 
namespace warpos {

    template <uint32 N, uint32 O, uint32 D>
    class EkfDynamics : public Rates<N+N*N+O>{
    public:
        virtual int16 calculateDynamics(floating_point time, 
                                        const std::array<floating_point, N> &tar_state, 
                                        const std::array<floating_point, O> &obs_state,
                                        std::array<floating_point, N> &tar_state_dot) {
            return WARNING_NOT_OVERRIDDEN;
        }

        virtual int16 calculateDynamicsJacobian(floating_point time,
                                                const std::array<floating_point, N> &tar_state,
                                                const std::array<floating_point, O> &obs_state,
                                                std::array<floating_point, N*N> &dynamics_Jacobian) {
            return WARNING_NOT_OVERRIDDEN;
        }

        virtual int16 calculateDynamicsNoiseMappingMatrix(floating_point time,
                                                          const std::array<floating_point, N> &tar_state,
                                                          const std::array<floating_point, O> &obs_state,
                                                          std::array<floating_point, N*D> &dynamics_mapping) {
            return WARNING_NOT_OVERRIDDEN;
        }

        virtual int16 calculateNoiseCovariance(floating_point time,
                                               const std::array<floating_point, N> &tar_state,
                                               const std::array<floating_point, O> &obs_state,
                                               std::array<floating_point, D*D> &process_covariance) {
            return WARNING_NOT_OVERRIDDEN;
        }

        virtual int16 calculateObserverDynamics(floating_point time,
                                                const std::array<floating_point, N> &tar_state,
                                                const std::array<floating_point, O> &obs_state,
                                                std::array<floating_point, O> &obs_state_dot) {
            return WARNING_NOT_OVERRIDDEN;
        }

        int16 calculateRates(floating_point time, 
                             const std::array<floating_point, N+N*N+O> &state, 
                             std::array<floating_point, N+N*N+O> &out_rates) override {
            // Split input into state vector, state covariance, and observer state
            for (uint32 i = 0; i < N; ++i) {
                _state_vector[i] = state[i];
            }
            _PP.setFromArray(&state[N]);
            for (uint32 i = 0; i < O; ++i) {
                _observer_vector[i] = state[N+N*N+i];
            }
 
            // ----- First get dynamics function outputs ----- //
            // Dynamics function Jacobian and transpose
            _error = calculateDynamicsJacobian(time, _state_vector, _observer_vector, _tmp_F_array);
            if (_error > 0) {return _error;} else {_saved_warning = _error;}
            _F.setFromArray(&_tmp_F_array[0]);
            _F.transpose(_F_tpose);
            // Dynamics mapping matrix and transpose
            _error = calculateDynamicsNoiseMappingMatrix(time, _state_vector, _observer_vector, _tmp_M_array);
            if (_error > 0) {return _error;} else {_saved_warning = _error;}
            _M.setFromArray(&_tmp_M_array[0]);
            _M.transpose(_M_tpose);
            // Process noise covariance matrix
            _error = calculateNoiseCovariance(time, _state_vector, _observer_vector, _tmp_Q_array);
            if (_error > 0) {return _error;} else {_saved_warning = _error;}
            _Q.setFromArray(&_tmp_Q_array[0]);
 
            // Compute mean state derivative
            _error = calculateDynamics(time, _state_vector, _observer_vector, _m_dot);
            if (_error > 0) {return _error;} else {_saved_warning = _error;}
            for (uint32 i = 0; i < N; ++i) {
                out_rates[i] = _m_dot[i];
            }
 
            // Compute state covariance derivative
            _P_dot = _F*_PP + _PP*_F_tpose + _M*_Q*_M_tpose;
            _P_dot.getAsArray(&out_rates[N]);
 
            // Compute the observer state derivative
            _error = calculateObserverDynamics(time, _state_vector, _observer_vector, _o_dot);
            if (_error > 0) {return _error;} else {_saved_warning = _error;}
            for (uint32 i = 0; i < O; ++i) {
                out_rates[i+N+N*N] = _o_dot[i];
            }
 
            return _saved_warning;
        }
    private:
        // Internal variable for tracking errors
        int16 _error;
        int16 _saved_warning;
 
        // State vector and its derivative
        std::array<floating_point, N> _state_vector, _m_dot;
        // Observer vector and its derivative
        std::array<floating_point, O> _observer_vector, _o_dot;
 
        // F matrix (dynamics function jacobian) for internal use
        std::array<floating_point, N*N> _tmp_F_array;
        clockwerk::Matrix<N, N> _F, _F_tpose;
 
        // M matrix (mapping from noise space to state space) for internal use
        std::array<floating_point, N*D> _tmp_M_array;
        clockwerk::Matrix<N, D> _M;
        clockwerk::Matrix<D, N> _M_tpose;
 
        // Q matrix (process noise covariance) for internal use
        std::array<floating_point, D*D> _tmp_Q_array;
        clockwerk::Matrix<D, D> _Q;
 
        // Temporary variables for covariance and state
        clockwerk::Matrix<N, N> _PP, _P_dot;
    };
 
}
 
#endif