EkfMeasurementUpdate.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_EKF_MEASUREMENT_UPDATE_HPP
#define GNCUTILS_EKF_EKF_MEASUREMENT_UPDATE_HPP
 
#include "core/Matrix.hpp"
#include "gncutils/EKF/Measurements.hpp"
#include "constants/mathconstants.h"
 
namespace warpos {
    const floating_point MINIMUM_COV_DIAG_EPSILON = 1e-6;

    template <uint32 N, uint32 M, uint32 O>
    class EkfMeasurementUpdate {
    public:
        EkfMeasurementUpdate(Measurements<N, M, O> &measurement_ref);

        int16 generateResidual(floating_point time, 
                               const std::array<floating_point, N> &state_minus,
                               const std::array<floating_point, O> &obs_state,
                               const std::array<floating_point, M> &measurement,
                               std::array<floating_point, M> *residual);

        int16 runUpdate(floating_point time,
                        const std::array<floating_point, N> &state_minus,
                        const clockwerk::Matrix<N, N> &cov_minus,
                        const std::array<floating_point, O> &obs_state,
                        const std::array<floating_point, M> &residual,
                        std::array<floating_point, N> *state_plus,
                        clockwerk::Matrix<N, N> *cov_plus);

        void setMeasurementNoiseMatrix(const clockwerk::Matrix<M, M> &R) {_R = R;}
    private:
        Measurements<N, M, O> &measurement_ref;
 
        // Error code for internal use
        int16 _error;
 
        // Identity matrix for calculation
        clockwerk::Matrix<N, N> _identity_n_n;
 
        // Measurement vector from estimated state for internal use
        std::array<floating_point, M> _est_meas;
 
        // H matrix (measurement function jacobian) for internal use
        std::array<floating_point, M*N> _tmp_H_array;
        clockwerk::Matrix<M, N> _H;
        clockwerk::Matrix<N, M> _H_tpose;
 
        // R matrix (measurement noise covariance) for internal use
        std::array<floating_point, M*M> _tmp_R_array;
        clockwerk::Matrix<M, M> _R;
 
        // W matrix (innovation covariance) for internal use
        clockwerk::Matrix<M, M> _W, _W_inv;
 
        // K matrix (Kalman gain) for internal use
        clockwerk::Matrix<N, M> _K;
 
        // Covariance input
        clockwerk::Matrix<N, N> _cov_input;
 
        // Temporary variables for updating the state mean
        clockwerk::Matrix<N, 1> _update;
        clockwerk::Matrix<M, 1> _residual;
 
        clockwerk::Matrix<N, N> _tmp_n_n_1;
        clockwerk::Matrix<N, N> _tmp_n_n_2;
        clockwerk::Matrix<N, M> _tmp_n_m_1;
        clockwerk::Matrix<M, N> _tmp_m_n_1;
        clockwerk::Matrix<M, M> _tmp_m_m_1;
    }; 
 
    template <uint32 N, uint32 M, uint32 O>
    EkfMeasurementUpdate<N, M, O>::EkfMeasurementUpdate(Measurements<N, M, O> &measurement_ref) 
        : measurement_ref(measurement_ref) {
        for(unsigned int i = 0; i < N; i++) {
            _identity_n_n[i][i] = 1.0;
        }
    }
 
    template <uint32 N, uint32 M, uint32 O>
    int16 EkfMeasurementUpdate<N, M, O>::generateResidual(floating_point time, 
                                                             const std::array<floating_point, N> &state_minus,
                                                             const std::array<floating_point, O> &obs_state,
                                                             const std::array<floating_point, M> &measurement,
                                                             std::array<floating_point, M> *residual) {
        // Verify our output pointers are set
        if(residual == nullptr) {
            return ERROR_NULLPTR;
        }
 
        // Calculate our estimated measurement
        _error = measurement_ref.calculateMeasurements(time, state_minus, obs_state, _est_meas.data());
        if(_error) {return _error;}
 
        // Calculate our residual
        for(uint32 i = 0; i < M; i++) {
            (*residual)[i] = measurement[i] - _est_meas[i];
        }
 
        return NO_ERROR;
    }
 
    template <uint32 N, uint32 M, uint32 O>
    int16 EkfMeasurementUpdate<N, M, O>::runUpdate(floating_point time,
                                                      const std::array<floating_point, N> &state_minus,
                                                      const clockwerk::Matrix<N, N> &cov_minus,
                                                      const std::array<floating_point, O> &obs_state,
                                                      const std::array<floating_point, M> &residual,
                                                      std::array<floating_point, N> *state_plus,
                                                      clockwerk::Matrix<N, N> *cov_plus) {
        // Verify our output pointers are set
        if(state_plus == nullptr || cov_plus == nullptr) {
            return ERROR_NULLPTR;
        }
        
        // ----- First get measurement function outputs ----- //
        // Measurement function Jacobian and transpose
        _error = measurement_ref.calculateMeasurementsMatrix(time, state_minus, obs_state, _tmp_H_array.data());
        if (_error) {return _error;}
        _H.setFromArray(&_tmp_H_array[0]);
        _H.transpose(_H_tpose);
 
        // Copy into our input covariance
        for(uint32 i = 0; i < N; i++) {
            for(uint32 j = 0; j < N; j++) {
                if(i == j) {
                    _cov_input[i][j] = cov_minus.get(i, j) < MINIMUM_COV_DIAG_EPSILON ? MINIMUM_COV_DIAG_EPSILON : cov_minus.get(i, j);
                } else {
                    _cov_input[i][j] = cov_minus.get(i, j);
                }
            }
        }
        // And enforce symmetry
        clockwerk::eAdd(_cov_input, _cov_input.transpose(), _cov_input);
        clockwerk::multiply(0.5, _cov_input, _cov_input);
 
        // Now compute the innovation covariance
        clockwerk::multiply(_H, _cov_input, _tmp_m_n_1);
        clockwerk::multiply(_tmp_m_n_1, _H_tpose, _tmp_m_m_1);
        clockwerk::eAdd(_tmp_m_m_1, _R, _W);
 
        // Now compute the Kalman gain
        _error = _W.inverse(_W_inv);
        if (_error) {return _error;}
        clockwerk::multiply(_cov_input, _H_tpose, _tmp_n_m_1);
        clockwerk::multiply(_tmp_n_m_1, _W_inv, _K);
 
        // Calculate our update from our innovation
        _residual.setFromArray(&residual[0]);
        multiply(_K, _residual, _update);
        for(uint32 i = 0; i < N; i++) {
            (*state_plus)[i] = state_minus[i] + _update.get(i,0);
        }
 
        // Calculate our update covariance
        clockwerk::multiply(_K, _H, _tmp_n_n_1);
        clockwerk::eSubtract(_identity_n_n, _tmp_n_n_1, _tmp_n_n_1);
        clockwerk::multiply(_tmp_n_n_1, _cov_input, _tmp_n_n_2);
        clockwerk::multiply(_tmp_n_n_2, _tmp_n_n_1.transpose(), *cov_plus);
        clockwerk::multiply(_K, _R, _tmp_n_m_1);
        clockwerk::multiply(_tmp_n_m_1, _K.transpose(), _tmp_n_n_1);
        clockwerk::eAdd(*cov_plus, _tmp_n_n_1, *cov_plus);
 
        return NO_ERROR;
    }
 
}
 
#endif