Line data    Source code

       1             : /*
       2             :  * Copyright (C) 2024 robinAZERTY [https://github.com/robinAZERTY]
       3             :  *
       4             :  * This file is part of ESP32AlgebraFilters library.
       5             :  *
       6             :  * ESP32AlgebraFilters library is free software: you can redistribute it and/or modify
       7             :  * it under the terms of the GNU General Public License as published by
       8             :  * the Free Software Foundation, either version 3 of the License, or
       9             :  * (at your option) any later version.
      10             :  *
      11             :  * ESP32AlgebraFilters library is distributed in the hope that it will be useful,
      12             :  * but WITHOUT ANY WARRANTY; without even the implied warranty of
      13             :  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
      14             :  * GNU General Public License for more details.
      15             :  *
      16             :  * You should have received a copy of the GNU General Public License
      17             :  * along with ESP32AlgebraFilters library. If not, see <https://www.gnu.org/licenses/>.
      18             :  */
      19             : 
      20             : #ifndef EKF_HPP
      21             : #define EKF_HPP
      22             : 
      23             : #include "matrix.hpp"
      24             : #include "symMatrix.hpp"
      25             : using namespace operators;
      26             : 
      27             : /**
      28             :  * @class Ekf
      29             :  * @brief A generic implementation of an Extended Kalman Filter (EKF).
      30             :  *
      31             :  * This class provides the core EKF functionality, including state prediction and update steps for nonlinear systems.
      32             :  * It supports dynamic system models and allows for numerical differentiation of Jacobians for both state transition and measurement functions.
      33             :  *
      34             :  * You must provide several key functions and data to ensure the filter works properly:
      35             :  *
      36             :  * - **State Transition Function** (`f`): A function that models the evolution of the state vector over time, used for the prediction step.
      37             :  * - **Measurement Function** (`h`): A function that models the measurement process, mapping the state vector to the measurement space.
      38             :  * - **Initial State and Covariance** (`X` and `P`): The user must initialize the state vector `X` and the state covariance matrix `P` with reasonable values for the system.
      39             :  * - **Control Inputs** (`U`): If the system involves control inputs, the user must provide them during the prediction step.
      40             :  * - **Measurement Data** (`Z` and `R`): During the update step, the user must provide the measurement vector `Z` and the measurement noise covariance `R`.
      41             :  *
      42             :  * The EKF will then handle the state prediction, measurement update, and state covariance estimation.
      43             :  *
      44             :  * The filter supports multiple types of sensors, with each sensor having its own measurement function and Jacobian.
      45             :  * The number of sensor types (`z_num`) can be customized, allowing for flexibility in handling various measurements within the same filter.
      46             :  *
      47             :  * The class also supports numerical differentiation for computing Jacobians if analytical expressions are not available, making it versatile for different models.
      48             :  *
      49             :  * @tparam x_dim The dimension of the state vector.
      50             :  * @tparam u_dim The dimension of the control input vector.
      51             :  * @tparam c_dim The dimension of the system parameters (default is 1).
      52             :  * @tparam z_num The number of measurement types (default is 1).
      53             :  * @tparam T The data type used for calculations (default is float).
      54             :  */
      55             : template <size_t x_dim, size_t u_dim, size_t c_dim = 1, size_t z_num = 1, typename T = float>
      56             : class Ekf
      57             : {
      58             : private:
      59             : 
      60             :     /** State transition function */
      61             :     Vector_f3<T> f = nullptr;
      62             : 
      63             :     /** Measurement dimensions for each measurement type */
      64             :     size_t z_dim[z_num] = {0};
      65             : 
      66             :     /** Measurement functions for each measurement type */
      67             :     Vector_f2<T> h[z_num] = {nullptr};
      68             : 
      69             :     /** State transition Jacobians */
      70             :     Matrix_f3<T> Fx = nullptr;
      71             :     Matrix_f3<T> Fu = nullptr;
      72             :     Matrix_f3<T> Fc = nullptr;
      73             : 
      74             :     /** Measurement Jacobians for each measurement type */
      75             :     Matrix_f2<T> H[z_num] = {nullptr};
      76             : 
      77             :     /** Measurement residuals for each measurement type */
      78             :     Vector<T> y[z_num];
      79             : 
      80             :     /** Jacobian values for measurement functions */
      81             :     Matrix<T> H_val[z_num];
      82             : 
      83             :     /** partial computation of innovation covariance (without R added) */
      84             :     symMatrix<T> pre_S[z_num];
      85             : 
      86             :     /** innovation precision */
      87             :     ldl_matrix<T> S_inv[z_num];
      88             : 
      89             :     /** Predicted measurement for each measurement type */
      90             :     Vector<T> h_val[z_num];
      91             : 
      92             :     /** Previous state vector */
      93             :     Vector<T> prev_X = Vector<T>(x_dim);
      94             : 
      95             :     /** State transition matrices */
      96             :     Matrix<T> Fx_val_T = Matrix<T>(x_dim, x_dim);
      97             :     Matrix<T> Fu_val_T = Matrix<T>(u_dim, x_dim);
      98             :     Matrix<T> Fc_val_T = Matrix<T>(c_dim, x_dim);
      99             : 
     100             :     /** Kalman gain */
     101             :     rowMajorMatrix<T> K;
     102             : 
     103             :     /** Measurement Jacobian * State covariance for each measurement type */
     104             :     Matrix<T> H_P[z_num];
     105             : 
     106             :     inline void finite_diff_Fx(const size_t i);
     107             :     inline void finite_diff_Fu(const size_t i);
     108             : 
     109             :     /**
     110             :      * @brief Numerical differentiation for the state transition Jacobian (Fx) using finite differences.
     111             :      *
     112             :      * This function approximates the Jacobian of the state transition function with respect to the state vector.
     113             :      *
     114             :      * @param i The index of the state dimension to compute the Jacobian.
     115             :      */
     116        1499 :     void finite_diff_Fx()
     117             :     {
     118        5995 :         for (size_t i = 0; i < x_dim; i++)
     119        4496 :             finite_diff_Fx(i);
     120        1499 :     };
     121             : 
     122             :     /**
     123             :      * @brief Numerical differentiation for the control input transition Jacobian (Fu) using finite differences.
     124             :      *
     125             :      * This function approximates the Jacobian of the state transition function with respect to the control input vector.
     126             :      *
     127             :      * @param i The index of the control input dimension to compute the Jacobian.
     128             :      */
     129        1499 :     void finite_diff_Fu()
     130             :     {
     131        3997 :         for (size_t i = 0; i < u_dim; i++)
     132        2498 :             finite_diff_Fu(i);
     133        1499 :     };
     134             : 
     135             :     /**
     136             :      * @brief Numerical differentiation for the measurement Jacobian (H) using finite differences.
     137             :      *
     138             :      * This function approximates the Jacobian of the measurement function with respect to the state vector.
     139             :      *
     140             :      * @param z_idx The index of the measurement type.
     141             :      * @param i The index of the state dimension to compute the Jacobian.
     142             :      */
     143             :     void finite_diff_H(const size_t z_idx, const size_t i);
     144             : 
     145             :     /** Compute Jacobian for measurements using numerical differentiation */
     146         143 :     void finite_diff_H(const size_t z_idx)
     147             :     {
     148         565 :         for (size_t i = 0; i < x_dim; i++)
     149         422 :             finite_diff_H(z_idx, i);
     150         143 :     };
     151             : 
     152             :     /** Flag indicating whether the filter is initialized */
     153             :     bool initted = false;
     154             : 
     155             : public:
     156             :     /** Current state vector */
     157             :     Vector<T> X = Vector<T>(x_dim);
     158             : 
     159             :     /** Small state epsilon for numerical differentiation */
     160             :     Vector<T> dx = Vector<T>(x_dim);
     161             : 
     162             :     /** State covariance matrix */
     163             :     symMatrix<T> P = symMatrix<T>(x_dim, x_dim);
     164             : 
     165             :     /** Control input vector */
     166             :     Vector<T> U = Vector<T>(u_dim);
     167             : 
     168             :     /** Small control input epsilon for numerical differentiation */
     169             :     Vector<T> du = Vector<T>(u_dim);
     170             : 
     171             :     /** Process noise covariance matrix */
     172             :     symMatrix<T> Cov_U = symMatrix<T>(u_dim);
     173             : 
     174             :     /** System parameters vector */
     175             :     Vector<T> C = Vector<T>(c_dim);
     176             : 
     177             :     /** Small system parameters epsilon for numerical differentiation */
     178             :     Vector<T> dc = Vector<T>(c_dim);
     179             : 
     180             :     /**
     181             :      * @brief Default constructor for the EKF filter.
     182             :      *
     183             :      * Initializes the state vector `X`, the state covariance `P`, and other necessary vectors to default values.
     184             :      * Sets small values for numerical differentiation.
     185             :      */
     186             :     Ekf();
     187             : 
     188             :     /** Constructor with a state transition function */
     189           2 :     Ekf(Vector_f3<T> f) : Ekf() { setPredictionFunction(f); }
     190             : 
     191             :     /** Destructor */
     192          20 :     ~Ekf() {};
     193             : 
     194             :     /** Set the state prediction function */
     195           2 :     void setPredictionFunction(Vector_f3<T> f) { this->f = f; }
     196             : 
     197             :     /** Set the Jacobian for state transition */
     198             :     void setJacobianFunction_Fx(Matrix_f3<T> Fx) { this->Fx = Fx; }
     199             : 
     200             :     /** Set the Jacobian for control input transition */
     201             :     void setJacobianFunction_Fu(Matrix_f3<T> Fu) { this->Fu = Fu; }
     202             : 
     203             :     /**
     204             :      * @brief Sets the measurement function for a specific sensor type.
     205             :      *
     206             :      * This function defines the measurement function and measurement dimension for a particular sensor type.
     207             :      * It also initializes the corresponding Jacobians and matrices.
     208             :      *
     209             :      * @param h The measurement function for the sensor type.
     210             :      * @param z_dim The dimension of the measurement vector for the sensor.
     211             :      * @param z_idx The index of the sensor type (default is 0).
     212             :      */
     213             :     void setMeasurementFunction(Vector_f2<T> h, size_t z_dim, size_t z_idx = 0);
     214             : 
     215             :     /** Set the Jacobian for the measurement function */
     216             :     void setJacobianFunction_H(Matrix_f2<T> H, size_t z_idx = 0) { this->H[z_idx] = H; }
     217             : 
     218             :     /**
     219             :      * @brief Predicts the state using the state transition function.
     220             :      *
     221             :      * This function computes the predicted state `X` based on the previous state and the state transition function.
     222             :      * It also updates the state covariance matrix `P` based on the state transition Jacobian.
     223             :      *
     224             :      * Throws an exception if the state transition function is not set.
     225             :      */
     226             :     inline void predict();
     227             : 
     228             :     /**
     229             :      * @brief First part of the update function, computes the measurement prediction and the measurement Jacobian.
     230             :      *
     231             :      * Additionally, it computes the common expression `H*P` which is used to reduce the number of matrix multiplications during the update step.
     232             :      *
     233             :      * @param z_idx The index of the measurement type.
     234             :      */
     235             :     inline void compute_H_P(const size_t z_idx = 0);
     236             : 
     237             :     /**
     238             :      * @brief Second part of the update function, computes the residual and the residual precision.
     239             :      *
     240             :      * This function is a part of the update step.
     241             :      *
     242             :      * @param Z The new measurement vector.
     243             :      * @param R The measurement noise covariance matrix.
     244             :      * @param z_idx The index of the measurement type.
     245             :      */
     246             :     inline void computeResidualPrecision(const symMatrix<T> &R, const size_t z_idx = 0);
     247             : 
     248             :     /**
     249             :      * @brief Third part of the update function, computes the residual and updates the state and covariance.
     250             :      *
     251             :      *  This function compute the difference between the measurement and the predicted measurement
     252             :      *
     253             :      * @param Z The new measurement vector.
     254             :      * @param z_idx The index of the measurement type.
     255             :      */
     256             :     inline void computeResidual(const Vector<T> &Z, const size_t z_idx = 0);
     257             : 
     258             :     /**
     259             :      * @brief Final part of the update function, computes the state and covariance.
     260             :      *
     261             :      * This function compute the Kalman gain and updates the state and covariance based on the measurement residual and the residual precision.
     262             :      *
     263             :      * @param z_idx The index of the measurement type.
     264             :      * @param updateMahalanobis Flag indicating whether to update the Mahalanobis distance.
     265             :      */
     266             :     inline void updateStateCovariance(const size_t z_idx = 0);
     267             : 
     268             :     /**
     269             :      * @brief Compute the Mahalanobis distance for a specific measurement type relative to last update of residual and residual precision.
     270             :      *
     271             :      * Should be called after the "computeResidualPrecision" function
     272             :      * Can be used to monitor the likelihood of the given measurement.
     273             :      * It is useful for detecting outliers or resolving identification issues (when you receive a measurement and you don't know witch measurement type it is).
     274             :      * If everything is perfectly modelled, the average Mahalanobis distance should be 1 (after averaging over a long period).
     275             :      *
     276             :      * @param z_idx The index of the measurement type.
     277             :      * @return The Mahalanobis distance.
     278             :      */
     279          93 :     inline T mahalanobisDistance(const size_t z_idx = 0) { return y[z_idx].dot(*(S_inv[z_idx] * y[z_idx]).release()); }
     280             : 
     281             :     /**
     282             :      * @brief Updates the state and covariance matrix using a new measurement and measurement noise covariance.
     283             :      *
     284             :      * This function updates the state vector `X` and the state covariance matrix `P` based on the new measurement `Z`
     285             :      * and the measurement noise covariance matrix `R`. The Kalman gain is also computed and used to adjust the state.
     286             :      * Throws an exception if the measurement function is not set or if there is a dimension mismatch.
     287             :      * This function can be split into multiple parts for better readability and modularity as follows:
     288             :      * @code
     289             :      * compute_H_P(z_idx);
     290             :      * computeResidualPrecision(R, z_idx);
     291             :      * computeResidual(Z, z_idx);
     292             :      * updateStateCovariance(z_idx);
     293             :      * @endcode
     294             :      * Before calling @code updateStateCovariance @endcode, you can check the Mahalanobis distance using @ref mahalanobisDistance.
     295             :      *
     296             :      * @param Z The new measurement vector.
     297             :      * @param R The measurement noise covariance matrix.
     298             :      * @param z_idx The index of the measurement type (default is 0).
     299             :      */
     300             :     inline void update(const Vector<T> &Z, const symMatrix<T> &R, const size_t z_idx = 0);
     301             : };
     302             : 
     303             : #ifndef EKF_CPP
     304             : #include "ekf.cpp"
     305             : #endif
     306             : 
     307             : #endif // EKF_HPP