ekf.h

Go to the documentation of this file.

 
#ifndef EKF_H
#define EKF_H
 
#define _USE_MATH_DEFINES
#include <math.h>
 
#include <assert.h>
#include <armadillo>
 
using namespace std;
using namespace arma;
 
class EKF {
public:  
  EKF();
  
  ~EKF();
  
  void InitSystem(int n_states, int n_outputs, const mat& Q, const mat& R);
  virtual colvec f(const colvec &x, const colvec &u);
  
  virtual colvec h(const colvec &x);
  void InitSystemState(const colvec& x0);
  
  void InitSystemStateCovariance(const mat& P0);
 
  void EKalmanf(const colvec& u);
  
  void EKalmanf(const colvec& z, const colvec& u);
  
  colvec* GetCurrentState();
  
  colvec* GetCurrentOutput();
  
  colvec* GetCurrentEstimatedState();
  
  colvec* GetCurrentEstimatedOutput();
  
private:
  void CalcF(const colvec &x, const colvec &u);
  
  void CalcH(const colvec &x);
 
  mat F_;          
  mat H_;          
  mat Q_;          
  mat R_;          
  colvec v_;       
  colvec w_;       
  
  mat sqrt_Q_;     
  mat sqrt_R_;     
  
 
 
  colvec x_m_;     
  colvec x_p_;     
  
  mat P_p_;        
  mat P_m_;        
  
  colvec z_m_;     
  
  double epsilon_; 
 
protected:
  
  int nStates_;   
  int nOutputs_;  
  
  colvec x_;      
  colvec z_;      
};
 
 
#endif