ukf.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 UKF {
public:  
  UKF();
  
  ~UKF();
  
  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 UKalmanf(const colvec& u);
  
  void UKalmanf(const colvec& z, const colvec& u);
  
  colvec* GetCurrentState();
  
  colvec* GetCurrentOutput();
  
  colvec* GetCurrentEstimatedState();
  
  colvec* GetCurrentEstimatedOutput();
  
private:
 
  mat Q_;          
  mat R_;          
  colvec v_;       
  colvec w_;       
  
  mat sqrt_Q_;     
  mat sqrt_R_;     
  
  mat P_;          
  colvec x_m_;     
  colvec z_m_;     
 
protected:
  
  int nStates_;   
  int nOutputs_;  
  
  colvec x_;      
  colvec z_;      
};
 
 
#endif