RLSFilter.h

Go to the documentation of this file.

//
// Created by Davide Pilastro on 2/19/21.
//
 
#ifndef RLS_DYN_MODEL_IDENT_RLS_H
#define RLS_DYN_MODEL_IDENT_RLS_H
 
#include <Eigen/Dense>
#include <type_traits>
using namespace Eigen;
 
namespace rls_filter {
 
template <bool B>
using EnableIfB = std::enable_if_t<B, int>;

template <typename T, int N>
class RLSFilter {
  static_assert((std::is_same<long double, T>::value ||
                 std::is_same_v<double, T> ||
                 std::is_same<float, T>::value),
                "T must be: long double, double or float");
 
 public:
  using VectorXt = Matrix<T, N, 1>;
  using MatrixXt = Matrix<T, N, N>;
 
 private:
  unsigned int n_;           
  T lam_;                    
  T lam_inv_;                
  T delta_;                  
  VectorXt w_;               
  MatrixXt P_;               
  MatrixXt P_supp_;          
  VectorXt g_;               
  T err_;                    
  unsigned long long count_; 
 
 public:
  template <int N1 = N, EnableIfB<(N1 > 0)> = 0>
  RLSFilter(T lam, T delta)
      : n_(N),
        lam_(1.0),
        lam_inv_(1.0),
        delta_(delta),
        w_(VectorXt::Zero()),
        P_(MatrixXt::Identity()),
        g_(VectorXt::Zero()),
        err_(0.0),
        count_(0) {
    setForgettingFactor(lam);
    setInitialCovarianceMatrixGain(delta);
    P_ *= delta_;
  }

  template <int N1 = N, EnableIfB<(N1 == -1)> = 0>
  RLSFilter(unsigned int n, T lam, T delta)
      : n_(n),
        lam_(1.0),
        lam_inv_(1.0),
        delta_(delta),
        w_(VectorXt::Zero(n_)),
        P_(MatrixXt::Identity(n_, n_)),
        g_(VectorXt::Zero(n_)),
        err_(0.0),
        count_(0) {
    setForgettingFactor(lam);
    setInitialCovarianceMatrixGain(delta);
    P_ *= delta_;
  }

  void update(const VectorXt &x, T y) {
    err_ = y - predict(x);
    P_supp_.noalias() = P_ * lam_inv_;
    g_.noalias() = (P_ * x) / (lam_ + (x.transpose() * P_ * x).value());
    P_.noalias() = (MatrixXt::Identity(n_, n_) - g_ * x.transpose()) * P_supp_;
    w_.noalias() += g_ * err_;
    count_++;
  };

  T predict(const VectorXt &x) const noexcept { return w_.transpose() * x; };

  void setEstimatedCoefficients(const VectorXt &w0) {
    if (w0.rows() == n_) {
      w_ = w0;
    } else {
      throw std::invalid_argument("Wrong initial state dimension.");
    }
  }

  void setForgettingFactor(double lam) {
    if ((lam > 0) && (lam <= 1.0)) {
      lam_ = lam;
      lam_inv_ = 1.0 / lam_;
    } else {
      throw std::invalid_argument(
          "Invalid forgetting factor (0 < lambda <= 1).");
    }
  }

  void setInitialCovarianceMatrixGain(double delta) {
    if (delta > 0.0) {
      delta_ = delta;
    } else {
      throw std::invalid_argument(
          "Invalid covariance matrix gain factor (delta > 0).");
    }
  }

  const VectorXt &estimatedCoefficients() const noexcept { return w_; };

  T a_priori_err() const noexcept { return err_; };

  const VectorXt &gains() const noexcept { return g_; };

  const MatrixXt &P() const noexcept { return P_; };

  unsigned long long count() const noexcept { return count_; };

  void reset() noexcept {
    w_ = VectorXt::Zero(n_);
    P_ = MatrixXt::Identity(n_, n_) * delta_;
    g_ = VectorXt::Zero(n_);
    err_ = 0.0;
    count_ = 0;
  };
};
 
}  // namespace rls_filter
 
#endif  // RLS_DYN_MODEL_IDENT_RLS_H