EkfTimeUpdate.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 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 "gncutils/dynamics/Rates.hpp"
#include "gncutils/integrator/Integrator.hpp"
 
namespace warpos {

    template <uint32 N>
    class EkfTimeUpdate {
    public:
        EkfTimeUpdate(Integrator<N + N*N>& integ) : _integrator(integ) {}

        int runUpdate(floating_point time_prev, const std::array<floating_point, N> &state_prev, const clockwerk::Matrix<N, N> &cov_prev,
                      floating_point time_up, std::array<floating_point, N> *state_up, clockwerk::Matrix<N, N> *cov_up); 
 
        clockwerk::Matrix<N, N> Q;                     
    private:
        std::array<floating_point, N + N*N> _full_state_initial;
        std::array<floating_point, N + N*N> _full_state_post;
 
        // STM
        clockwerk::Matrix<N, N> _phi;
        clockwerk::Matrix<N, N> _phi_transpose;
 
        // Temporary calculation variables
        clockwerk::Matrix<N, N> _tmp_1;
        clockwerk::Matrix<N, N> _tmp_2;

        Integrator<N + N*N>& _integrator;
    }; 
 
    template<uint32 N>
    int EkfTimeUpdate<N>::runUpdate(floating_point time_prev, const std::array<floating_point, N> &state_prev, const clockwerk::Matrix<N, N> &cov_prev,
                                    floating_point time_up, std::array<floating_point, N> *state_up, clockwerk::Matrix<N, N> *cov_up) {
        // 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
        int16 err = _integrator.step(time_prev, time_up, _full_state_initial, _full_state_post);
        if(err > 0) {
            return err;
        }
 
        // 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];
        }
        multiply(_phi, cov_prev, _tmp_1);
        multiply(_tmp_1, _phi_transpose, _tmp_2);
        multiply(time_up - time_prev, Q, _tmp_1);
        eAdd(_tmp_2, _tmp_1, *cov_up);
 
        return NO_ERROR;
    }
 
}
 
#endif