extended-kalman-filter.hpp

 
#ifndef STATEOBSERVER_EXTENDEDKALMANFILTERHPP
#define STATEOBSERVER_EXTENDEDKALMANFILTERHPP
 
#include <state-observation/api.h>
#include <state-observation/dynamical-system/dynamical-system-functor-base.hpp>
#include <state-observation/observer/kalman-filter-base.hpp>
 
namespace stateObservation
{
 
class STATE_OBSERVATION_DLLAPI ExtendedKalmanFilter : public KalmanFilterBase
{
 
public:
 
  ExtendedKalmanFilter(Index n, Index m);

 
  ExtendedKalmanFilter(Index n,
                       Index m,
                       Index p,
                       bool directInputOutputFeedthrough = true,
                       bool directInputStateProcessFeedthrough = true);

 
  ExtendedKalmanFilter(Index n,
                       Index nt,
                       Index m,
                       Index mt,
                       Index p,
                       bool directInputOutputFeedthrough,
                       bool directInputStateProcessFeedthrough);

  void setFunctor(DynamicalSystemFunctorBase * f);
  DynamicalSystemFunctorBase * getFunctor(void) const;

  DynamicalSystemFunctorBase * functor() const;

  void clearFunctor();

  void setDirectInputOutputFeedthrough(bool b = true);

  void setDirectInputStateFeedthrough(bool b = true);

  virtual Amatrix getAMatrixFD(const Vector & dx);

  virtual Cmatrix getCMatrixFD(const Vector & dx);

  virtual void reset();
 
protected:
  virtual StateVector prediction_(TimeIndex k);

  virtual MeasureVector simulateSensor_(const StateVector & x, TimeIndex k);

  virtual MeasureVector predictSensor_(TimeIndex k);

  bool directInputOutputFeedthrough_;

  bool directInputStateProcessFeedthrough_;

  DynamicalSystemFunctorBase * f_;
 
  // optimization
  struct Optimization
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    ObserverBase::InputVector u_;
    KalmanFilterBase::Amatrix a_;
    KalmanFilterBase::Cmatrix c_;
    ObserverBase::StateVector x_;
    ObserverBase::StateVector dx_;
    ObserverBase::StateVector xp_;
    ObserverBase::MeasureVector yp_;
 
  } opt;
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
 
} // namespace stateObservation
 
#endif // EXTENDEDKALMANFILTERHPP