ukf.h

Go to the documentation of this file.

// clang: MatousFormat
#ifndef UKF_H
#define UKF_H
 
#include <mrs_lib/kalman_filter.h>
 
namespace mrs_lib
{
 
  template <int n_states, int n_inputs, int n_measurements>
  class UKF : KalmanFilter<n_states, n_inputs, n_measurements>
  {
  protected:
    /* protected UKF definitions (typedefs, constants etc) //{ */
    static constexpr int n = n_states;            
    static constexpr int m = n_inputs;            
    static constexpr int p = n_measurements;      
    static constexpr int w = 2 * n + 1;           
    using Base_class = KalmanFilter<n, m, p>; 
    using X_t = typename Eigen::Matrix<double, n, w>;    
    using Z_t = typename Eigen::Matrix<double, p, w>;    
    using Pzz_t = typename Eigen::Matrix<double, p, p>;  
    using K_t = typename Eigen::Matrix<double, n, p>;    
    //}
 
  public:
    /* public UKF definitions (typedefs, constants etc) //{ */
    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 W_t = typename Eigen::Matrix<double, w, 1>;
    using statecov_t = typename Base_class::statecov_t;
    using transition_model_t = typename std::function<x_t(const x_t&, const u_t&, double)>;
    using observation_model_t = typename std::function<z_t(const x_t&)>;
 
    struct square_root_exception : public std::exception
    {
      const char* what() const throw()
      {
        return "UKF: taking the square root of covariance update produced NANs!!!";
      }
    };
 
    struct inverse_exception : public std::exception
    {
      const char* what() const throw()
      {
        return "UKF: inverting of Pzz in correction update produced NANs!!!";
      }
    };
    //}
 
  public:
    /* UKF constructor //{ */
    UKF();
 
    UKF(const transition_model_t& transition_model, const observation_model_t& observation_model, const double alpha = 1e-3, const double kappa = 1, const double beta = 2);
    //}
 
    /* correct() method //{ */
    virtual statecov_t correct(const statecov_t& sc, const z_t& z, const R_t& R) const override;
    //}
 
    /* predict() method //{ */
    virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, double dt) const override;
    //}
 
    /* setConstants() method //{ */
    void setConstants(const double alpha, const double kappa, const double beta);
    //}
 
    /* setTransitionModel() method //{ */
    void setTransitionModel(const transition_model_t& transition_model);
    //}
 
    /* setObservationModel() method //{ */
    void setObservationModel(const observation_model_t& observation_model);
    //}
 
  protected:
    /* protected methods and member variables //{ */
 
    void computeWeights();
 
    X_t computeSigmas(const x_t& x, const P_t& P) const;
 
    P_t computePaSqrt(const P_t& P) const;
 
    Pzz_t computeInverse(const Pzz_t& Pzz) const;
 
    virtual K_t computeKalmanGain([[maybe_unused]] const x_t& x, [[maybe_unused]] const z_t& inn, const K_t& Pxz, const Pzz_t& Pzz) const;
 
    double m_alpha, m_kappa, m_beta, m_lambda;
    W_t m_Wm;
    W_t m_Wc;
 
    transition_model_t m_transition_model;
    observation_model_t m_observation_model;
 
    //}
  };
 
}  // namespace mrs_lib
 
#include <mrs_lib/impl/ukf.hpp>
 
#endif