kalman_filter_aloamgarm.h

Go to the documentation of this file.

// clang: MatousFormat
#ifndef SYSTEMMODELALOAMGARM_H
#define SYSTEMMODELALOAMGARM_H
 
#include <Eigen/Dense>
 
#include <boost/circular_buffer.hpp>
#include <ros/ros.h>
 
namespace mrs_lib
{
  /* KalmanFilter virtual class //{ */
  template <int n_states, int n_inputs, int n_measurements>
  class KalmanFilterAloamGarm
  {
    public:
      /* states, inputs etc. definitions (typedefs, constants etc) //{ */
      static const int n = n_states;            
      static const int m = n_inputs;            
      static const int p = n_measurements;      
      typedef Eigen::Matrix<double, n, 1> x_t;  
      typedef Eigen::Matrix<double, m, 1> u_t;  
      typedef Eigen::Matrix<double, p, 1> z_t;  
      typedef Eigen::Matrix<double, n, n> P_t;  
      typedef Eigen::Matrix<double, p, p> R_t;  
      typedef Eigen::Matrix<double, n, n> Q_t;  
      //}
 
      /* statecov_t struct //{ */
      struct statecov_t
      {
        x_t x;  
        P_t P;  
        std::shared_ptr<boost::circular_buffer<double>> nis_buffer = nullptr;
        ros::Time stamp = ros::Time(0);
        bool measurement_jumped = false;
 
        statecov_t& operator=(statecov_t other)
        {
          if (this != &other) // not a self-assignment
          {
            x = other.x;
            P = other.P;
            stamp = other.stamp;
            if (other.nis_buffer != nullptr){
              nis_buffer = std::make_shared<boost::circular_buffer<double>>(*other.nis_buffer);
            }
            measurement_jumped = other.measurement_jumped;
          }
          return *this;
        }
 
      };
      //}
 
    public:
      virtual statecov_t correct(const statecov_t& sc, const z_t& z, const R_t& R) const = 0;
 
      virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, double dt) const = 0;
  };
  //}
}
 
#endif // SYSTEMMODELALOAMGARM_H