kalman.h

Go to the documentation of this file.

#ifndef KALMAN_H
#define KALMAN_H
 
#include <Eigen/Eigen>
#include <cstddef>
 
template <size_t state_d, size_t measure_d, size_t control_d>
class kalman_filter
{
#define mtx_t Eigen::Matrix
 protected:
  /*these matrices should be provided by user*/
  mtx_t<double, state_d, state_d> F;      
  mtx_t<double, measure_d, state_d> H;    
  mtx_t<double, state_d, control_d> B;    
  mtx_t<double, state_d, state_d> Q;      
  mtx_t<double, measure_d, measure_d> R;  
  mtx_t<double, control_d, 1> Uk;         
  mtx_t<double, measure_d, 1> Zk;         
 
  /*these matrices will be calculated*/
  mtx_t<double, state_d, 1> Xk_km1;        
  mtx_t<double, state_d, state_d> Pk_km1;  
  mtx_t<double, measure_d, 1> Yk;          
  mtx_t<double, measure_d, measure_d> Sk;  
  mtx_t<double, state_d, measure_d> K;     
  mtx_t<double, state_d, 1> Xk_k;          
  mtx_t<double, measure_d, 1> Yk_k;        
  mtx_t<double, state_d, state_d> Pk_k;    
 
  /*auxiliary matrices*/
  mtx_t<double, state_d, state_d> I;  // (I - Kk*Hk)
  kalman_filter() : I(mtx_t<double, state_d, state_d>::Identity()) {}
 
  // predict
  void estimate()
  {
    // Xk|k-1 = Fk*Xk-1|k-1 + Bk*Uk
    Xk_km1 = F * Xk_k + B * Uk;
    // Pk|k-1 = Fk*Pk-1|k-1*Fk(t) + Qk
    Pk_km1 = F * Pk_k * F.transpose() + Q;
  }
 
  // update
  void correct()
  {
    // Yk = Zk - Hk*Xk|k-1
    Yk = Zk - H * Xk_km1;
    // Sk = Rk + Hk*Pk|k-1*Hk(t)
    Sk = R + H * Pk_km1 * H.transpose();
 
    // Kk = Pk|k-1*Hk(t)*Sk(inv)
    K = Pk_km1 * H.transpose() * Sk.inverse();
    // xk|k = xk|k-1 + Kk*Yk
    Xk_k = Xk_km1 + K * Yk;
 
    // Pk|k = (I - Kk*Hk) * Pk|k-1 * (I - Kk*Hk).t() + (K * R * K.t())
    Pk_k = (I - K * H) * Pk_km1 * (I - K * H).transpose() +
           (K * R * K.transpose());
 
    // Yk|k = Zk - Hk*Xk|k
    Yk_k = Zk - H * Xk_k;
  }
 
#undef mtx_t
};
 
#endif  // KALMAN_H