ekf2.h

Go to the documentation of this file.

 
#ifndef EKF2_H
#define EKF2_H
 
#define _USE_MATH_DEFINES
#include <math.h>
 
#include <assert.h>
#include <armadillo>
 
using namespace std;
using namespace arma;
 
class EKF2 {
public:
    EKF2();
 
    ~EKF2();
 
    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:
    mat CalcFx(const colvec &x, const colvec &u);
 
    mat CalcHx(const colvec &x);
 
    mat CalcFxx(const colvec &x, const colvec &u, const int i);
 
    mat CalcHxx(const colvec &x, const int i);
    
    colvec e(const int i, const int max_len);
 
    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