lkf.h

// clang: MatousFormat
#ifndef LKFSYSTEMMODELS_H
#define LKFSYSTEMMODELS_H
 
#include <mrs_lib/kalman_filter.h>
#include <iostream>
 
namespace mrs_lib
{
 
  /* class LKF //{ */
  template <int n_states, int n_inputs, int n_measurements>
  class LKF : public KalmanFilter<n_states, n_inputs, n_measurements>
  {
  public:
    /* LKF definitions (typedefs, constants etc) //{ */
    static constexpr int n = n_states;            
    static constexpr int m = n_inputs;            
    static constexpr int p = n_measurements;      
    using Base_class = KalmanFilter<n, m, p>; 
    using x_t = typename Base_class::x_t;                
    using u_t = typename Base_class::u_t;                
    using z_t = typename Base_class::z_t;                
    using P_t = typename Base_class::P_t;                
    using R_t = typename Base_class::R_t;                
    using Q_t = typename Base_class::Q_t;                
    using statecov_t = typename Base_class::statecov_t;  
    typedef Eigen::Matrix<double, n, n> A_t;  
    typedef Eigen::Matrix<double, n, m> B_t;  
    typedef Eigen::Matrix<double, p, n> H_t;  
    typedef Eigen::Matrix<double, n, p> K_t;  
    struct inverse_exception : public std::exception
    {
      const char* what() const throw()
      {
        return "LKF: could not compute matrix inversion!!! Fix your covariances (the measurement's is probably too low...)";
      }
    };
    //}
 
  public:
    LKF(){};
 
    LKF(const A_t& A, const B_t& B, const H_t& H) : A(A), B(B), H(H){};
 
    /* correct() method //{ */
    virtual statecov_t correct(const statecov_t& sc, const z_t& z, const R_t& R) const override
    {
      /* return correct_optimized(sc, z, R, H); */
      return correction_impl(sc, z, R, H);
    };
    //}
 
    /* predict() method //{ */
    virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, [[maybe_unused]] double dt) const override
    {
      statecov_t ret;
      ret.x = state_predict(A, sc.x, B, u);
      ret.P = covariance_predict(A, sc.P, Q, dt);
      return ret;
    };
    //}
 
  public:
    A_t A;  
    B_t B;  
    H_t H;  
  protected:
    /* covariance_predict() method //{ */
    static P_t covariance_predict(const A_t& A, const P_t& P, const Q_t& Q, const double dt)
    {
      return A * P * A.transpose() + dt*Q;
    }
    //}
 
    /* state_predict() method //{ */
    template <int check = n_inputs>
    static inline typename std::enable_if<check == 0, x_t>::type state_predict(const A_t& A, const x_t& x, [[maybe_unused]] const B_t& B,
                                                                               [[maybe_unused]] const u_t& u)
    {
      return A * x;
    }
 
    template <int check = n_inputs>
    static inline typename std::enable_if<check != 0, x_t>::type state_predict(const A_t& A, const x_t& x, const B_t& B, const u_t& u)
    {
      return A * x + B * u;
    }
    //}
 
  protected:
    /* invert_W() method //{ */
    static R_t invert_W(R_t W)
    {
      Eigen::ColPivHouseholderQR<R_t> qr(W);
      if (!qr.isInvertible())
      {
        // add some stuff to the tmp matrix diagonal to make it invertible
        R_t ident = R_t::Identity(W.rows(), W.cols());
        W += 1e-9 * ident;
        qr.compute(W);
        if (!qr.isInvertible())
        {
          // never managed to make this happen except for explicitly putting NaNs in the input
          throw inverse_exception();
        }
      }
      const R_t W_inv = qr.inverse();
      return W_inv;
    }
    //}
 
    /* computeKalmanGain() method //{ */
    virtual K_t computeKalmanGain(const statecov_t& sc, [[maybe_unused]] const z_t& z, const R_t& R, const H_t& H) const
    {
      // calculation of the kalman gain K
      const R_t W = H * sc.P * H.transpose() + R;
      const R_t W_inv = invert_W(W);
      const K_t K = sc.P * H.transpose() * W_inv;
      return K;
    }
    //}
 
    /* correction_impl() method //{ */
    template<int check=n>
    typename std::enable_if<check >= 0, statecov_t>::type correction_impl(const statecov_t& sc, const z_t& z, const R_t& R, const H_t& H) const
    {
      // the correction phase
      statecov_t ret;
      const K_t K = computeKalmanGain(sc, z, R, H);
      ret.x = sc.x + K * (z - (H * sc.x));
      ret.P = (P_t::Identity() - (K * H)) * sc.P;
      return ret;
    }
 
    template<int check=n>
    typename std::enable_if<check < 0, statecov_t>::type correction_impl(const statecov_t& sc, const z_t& z, const R_t& R, const H_t& H) const
    {
      // the correction phase
      statecov_t ret;
      const K_t K = computeKalmanGain(sc, z, R, H);
      ret.x = sc.x + K * (z - (H * sc.x));
      ret.P = (P_t::Identity(sc.P.rows(), sc.P.cols()) - (K * H)) * sc.P;
      return ret;
    }
    //}
 
    // NOT USED METHODS
    /* correction_optimized() method //{ */
    // No notable performance gain was observed for the matrix sizes we use, so this is not used.
    static statecov_t correction_optimized(const statecov_t& sc, const z_t& z, const R_t& R, const H_t& H)
    {
      statecov_t ret = sc;
      const H_t B(H*sc.P.transpose());
      const K_t K((B*H.transpose() + R).ldlt().solve(B).transpose());
      ret.x.noalias() += K*(z - H*sc.x);
      ret.P.noalias() -= K*H*sc.P;
      return ret;
    }
 
    /* static statecov_t correction_optimized(const statecov_t& sc, const z_t& z, const R_t& R, const H_t& H) */
    /* { */
    /*   statecov_t ret; */
    /*   const H_t B = H*sc.P.transpose(); */
    /*   const K_t K = (B*H.transpose() + R).partialPivLu().solve(B).transpose(); */
    /*   ret.x = sc.x + K*(z - H*sc.x); */
    /*   ret.P = sc.P - K*H*sc.P; */
    /*   return ret; */
    /* } */
    //}
  };
  //}
 
  /* class dtMatrixLKF //{ */
  template <int n_states, int n_inputs, int n_measurements>
  class varstepLKF : public LKF<n_states, n_inputs, n_measurements>
  {
  public:
    /* LKF definitions (typedefs, constants etc) //{ */
    static const int n = n_states;
    static const int m = n_inputs;
    static const int p = n_measurements;
    using Base_class = LKF<n, m, p>;
 
    using x_t = typename Base_class::x_t;
    using u_t = typename Base_class::u_t;
    using z_t = typename Base_class::z_t;
    using P_t = typename Base_class::P_t;
    using R_t = typename Base_class::R_t;
    using statecov_t = typename Base_class::statecov_t;
    using A_t = typename Base_class::A_t;  // measurement mapping p*n
    using B_t = typename Base_class::B_t;  // process covariance n*n
    using H_t = typename Base_class::H_t;  // measurement mapping p*n
    using Q_t = typename Base_class::Q_t;  // process covariance n*n
 
    using generateA_t = std::function<A_t(double)>;
    using generateB_t = std::function<B_t(double)>;
    //}
 
  public:
 
    varstepLKF(const generateA_t& generateA, const generateB_t& generateB, const H_t& H)
      : m_generateA(generateA), m_generateB(generateB)
    {
      Base_class::H = H;
    };
 
    /* predict() method //{ */
    virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, double dt) const override
    {
      statecov_t ret;
      A_t A = m_generateA(dt);
      B_t B = m_generateB(dt);
      ret.x = Base_class::state_predict(A, sc.x, B, u);
      ret.P = Base_class::covariance_predict(A, sc.P, Q, dt);
      return ret;
    };
    //}
    
  private:
    generateA_t m_generateA;
    generateB_t m_generateB;
  };
  //}
 
  /* class LKF_MRS_odom //{ */
  class LKF_MRS_odom : public LKF<3, 1, 1>
  {
  public:
    /* LKF definitions (typedefs, constants etc) //{ */
    static const int n = 3;
    static const int m = 1;
    static const int p = 1;
    using Base_class = LKF<n, m, p>;
 
    using x_t = typename Base_class::x_t;
    using u_t = typename Base_class::u_t;
    using z_t = typename Base_class::z_t;
    using P_t = typename Base_class::P_t;
    using R_t = typename Base_class::R_t;
    using statecov_t = typename Base_class::statecov_t;
    using A_t = typename Base_class::A_t;  // measurement mapping p*n
    using B_t = typename Base_class::B_t;  // process covariance n*n
    using H_t = typename Base_class::H_t;  // measurement mapping p*n
    using Q_t = typename Base_class::Q_t;  // process covariance n*n
 
    using coeff_A_t = A_t;                            // matrix of constant coefficients in matrix A
    typedef Eigen::Matrix<unsigned, n, n> dtexp_A_t;  // matrix of dt exponents in matrix A
    using coeff_B_t = B_t;                            // matrix of constant coefficients in matrix B
    typedef Eigen::Matrix<unsigned, n, m> dtexp_B_t;  // matrix of dt exponents in matrix B
    //}
 
  public:
    LKF_MRS_odom(const std::vector<H_t>& Hs, const double default_dt = 1);
    virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, double dt) const override;
    virtual statecov_t correct(const statecov_t& sc, const z_t& z, const R_t& R, int param = 0) const;
 
  public:
    x_t state_predict_optimized(const x_t& x_prev, const u_t& u, double dt) const;
    P_t covariance_predict_optimized(const P_t& P, const Q_t& Q, double dt) const;
 
  private:
    std::vector<H_t> m_Hs;
  };
  //}
 
}  // namespace mrs_lib
 
#endif // LKFSYSTEMMODELS_H