ekf.hpp

Go to the documentation of this file.

/*
 * Copyright (C) 2024 robinAZERTY [https://github.com/robinAZERTY]
 *
 * This file is part of ESP32AlgebraFilters library.
 *
 * ESP32AlgebraFilters library is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * ESP32AlgebraFilters library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with ESP32AlgebraFilters library. If not, see <https://www.gnu.org/licenses/>.
 */
 
#ifndef EKF_HPP
#define EKF_HPP
 
#include "matrix.hpp"
#include "symMatrix.hpp"
using namespace operators;
 
template <size_t x_dim, size_t u_dim, size_t c_dim = 1, size_t z_num = 1, typename T = float>
class Ekf
{
private:
 
    Vector_f3<T> f = nullptr;
 
    size_t z_dim[z_num] = {0};
 
    Vector_f2<T> h[z_num] = {nullptr};
 
    Matrix_f3<T> Fx = nullptr;
    Matrix_f3<T> Fu = nullptr;
    Matrix_f3<T> Fc = nullptr;
 
    Matrix_f2<T> H[z_num] = {nullptr};
 
    Vector<T> y[z_num];
 
    Matrix<T> H_val[z_num];
 
    symMatrix<T> pre_S[z_num];
 
    ldl_matrix<T> S_inv[z_num];
 
    Vector<T> h_val[z_num];
 
    Vector<T> prev_X = Vector<T>(x_dim);
 
    Matrix<T> Fx_val_T = Matrix<T>(x_dim, x_dim);
    Matrix<T> Fu_val_T = Matrix<T>(u_dim, x_dim);
    Matrix<T> Fc_val_T = Matrix<T>(c_dim, x_dim);
 
    rowMajorMatrix<T> K;
 
    Matrix<T> H_P[z_num];
 
    inline void finite_diff_Fx(const size_t i);
    inline void finite_diff_Fu(const size_t i);
 
    void finite_diff_Fx()
    {
        for (size_t i = 0; i < x_dim; i++)
            finite_diff_Fx(i);
    };
 
    void finite_diff_Fu()
    {
        for (size_t i = 0; i < u_dim; i++)
            finite_diff_Fu(i);
    };
 
    void finite_diff_H(const size_t z_idx, const size_t i);
 
    void finite_diff_H(const size_t z_idx)
    {
        for (size_t i = 0; i < x_dim; i++)
            finite_diff_H(z_idx, i);
    };
 
    bool initted = false;
 
public:
    Vector<T> X = Vector<T>(x_dim);
 
    Vector<T> dx = Vector<T>(x_dim);
 
    symMatrix<T> P = symMatrix<T>(x_dim, x_dim);
 
    Vector<T> U = Vector<T>(u_dim);
 
    Vector<T> du = Vector<T>(u_dim);
 
    symMatrix<T> Cov_U = symMatrix<T>(u_dim);
 
    Vector<T> C = Vector<T>(c_dim);
 
    Vector<T> dc = Vector<T>(c_dim);
 
    Ekf();
 
    Ekf(Vector_f3<T> f) : Ekf() { setPredictionFunction(f); }
 
    ~Ekf() {};
 
    void setPredictionFunction(Vector_f3<T> f) { this->f = f; }
 
    void setJacobianFunction_Fx(Matrix_f3<T> Fx) { this->Fx = Fx; }
 
    void setJacobianFunction_Fu(Matrix_f3<T> Fu) { this->Fu = Fu; }
 
    void setMeasurementFunction(Vector_f2<T> h, size_t z_dim, size_t z_idx = 0);
 
    void setJacobianFunction_H(Matrix_f2<T> H, size_t z_idx = 0) { this->H[z_idx] = H; }
 
    inline void predict();
 
    inline void compute_H_P(const size_t z_idx = 0);
 
    inline void computeResidualPrecision(const symMatrix<T> &R, const size_t z_idx = 0);
 
    inline void computeResidual(const Vector<T> &Z, const size_t z_idx = 0);
 
    inline void updateStateCovariance(const size_t z_idx = 0);
 
    inline T mahalanobisDistance(const size_t z_idx = 0) { return y[z_idx].dot(*(S_inv[z_idx] * y[z_idx]).release()); }
 
    inline void update(const Vector<T> &Z, const symMatrix<T> &R, const size_t z_idx = 0);
};
 
#ifndef EKF_CPP
#include "ekf.cpp"
#endif
 
#endif // EKF_HPP