kalman-filter-base.hpp

Go to the documentation of this file.

 
#ifndef KALMANFILTERBASEHPP
#define KALMANFILTERBASEHPP
 
#include <state-observation/api.h>
#include <state-observation/observer/zero-delay-observer.hpp>
#include <state-observation/tools/state-vector-arithmetics.hpp>
 
namespace stateObservation
{

class STATE_OBSERVATION_DLLAPI KalmanFilterBase : public ZeroDelayObserver, protected StateVectorArithmetics
{
public:
  typedef Matrix Amatrix;

  typedef Matrix Cmatrix;

  typedef Matrix Qmatrix;

  typedef Matrix Rmatrix;

  typedef Matrix Pmatrix;
 
  typedef Eigen::LLT<Pmatrix> LLTPMatrix;

  typedef Vector StateVectorTan;

  typedef Vector MeasureVectorTan;

  KalmanFilterBase();

  KalmanFilterBase(Index n, Index m, Index p = 0);

  KalmanFilterBase(Index n, Index nt, Index m, Index mt, Index p);

  virtual void setA(const Amatrix & A);
 
  virtual Matrix getA() const;

  virtual void clearA();

  virtual void setC(const Cmatrix & C);
 
  virtual Matrix getC() const;

  virtual void clearC();

  virtual void setR(const Rmatrix & R);
  inline void setMeasurementCovariance(const Rmatrix & R)
  {
    setR(R);
  }

  virtual Matrix getR() const;
  inline Matrix getMeasurementCovariance() const
  {
    return getR();
  }

  virtual void clearR();

  virtual void setQ(const Qmatrix & Q);
  inline void setProcessCovariance(const Qmatrix & Q)
  {
    setQ(Q);
  }

  virtual Matrix getQ() const;
  inline Matrix getProcessCovariance() const
  {
    return getQ();
  }

  virtual void clearQ();

  virtual void setStateCovariance(const Pmatrix & P);

  virtual void clearStates();

  virtual void clearStateCovariance();

  virtual Pmatrix getStateCovariance() const;

  virtual void reset();

  Amatrix getAmatrixConstant(double c) const;

  Amatrix getAmatrixRandom() const;

  Amatrix getAmatrixZero() const;

  Amatrix getAmatrixIdentity() const;

  bool checkAmatrix(const Amatrix &) const;

  Cmatrix getCmatrixConstant(double c) const;

  Cmatrix getCmatrixRandom() const;

  Cmatrix getCmatrixZero() const;

  bool checkCmatrix(const Cmatrix &) const;

  Qmatrix getQmatrixConstant(double c) const;

  Qmatrix getQmatrixRandom() const;

  Qmatrix getQmatrixZero() const;

  Qmatrix getQmatrixIdentity() const;

  bool checkQmatrix(const Qmatrix &) const;

  Rmatrix getRmatrixConstant(double c) const;

  Rmatrix getRmatrixRandom() const;

  Rmatrix getRmatrixZero() const;

  Rmatrix getRmatrixIdentity() const;

  bool checkRmatrix(const Rmatrix &) const;

  Pmatrix getPmatrixConstant(double c) const;

  Pmatrix getPmatrixRandom() const;

  Pmatrix getPmatrixZero() const;

  Pmatrix getPmatrixIdentity() const;

  bool checkPmatrix(const Pmatrix &) const;

  virtual StateVectorTan stateTangentVectorConstant(double c) const;

  virtual StateVectorTan stateTangentVectorRandom() const;

  virtual StateVectorTan stateTangentVectorZero() const;

  virtual bool checkStateTangentVector(const StateVectorTan & v) const;

  virtual MeasureVectorTan measureTangentVectorConstant(double c) const;

  virtual MeasureVectorTan measureTangentVectorRandom() const;

  virtual MeasureVectorTan measureTangentVectorZero() const;

  virtual bool checkMeasureTangentVector(const MeasureVectorTan &) const;

  virtual void setStateSize(Index n);

  virtual void setStateSize(Index n, Index nt);

  virtual void setMeasureSize(Index m);

  virtual void setMeasureSize(Index m, Index mt);

  virtual MeasureVector getSimulatedMeasurement(TimeIndex k);

  virtual const StateVector & getInnovation();

  inline const StateVector & updateStatePrediction();

  inline MeasureVector updateStateAndMeasurementPrediction();

  const StateVector & getLastPrediction() const;

  const MeasureVector & getLastPredictedMeasurement() const;

  const MeasureVector & getLastMeasurement() const;

  const Matrix & getLastGain() const;

  void setStateArithmetics(StateVectorArithmetics * arith);
 
protected:
  Index nt_;

  Index mt_;

  typedef Matrix Kmatrix;

  virtual StateVector oneStepEstimation_();

  virtual StateVector prediction_(TimeIndex k) = 0;

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

  virtual MeasureVector predictSensor_(TimeIndex k);

  Matrix a_;

  Matrix c_;

  Matrix q_;

  Matrix r_;

  Matrix pr_;

  IndexedVector xbar_;

  IndexedVector ybar_;

  Vector innovation_;
 
  struct optimizationContainer
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    Matrix pbar;
    Vector xhat;
    Vector inoMeas;
    Matrix inoMeasCov;
    Matrix inoMeasCovInverse;
    LLTPMatrix inoMeasCovLLT;
    Matrix kGain;
    Matrix t;
    Matrix mKc;
  } oc_;
 
  StateVectorArithmetics * arithm_;
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
 
inline const Vector & KalmanFilterBase::updateStatePrediction()
{
  prediction_(this->x_.getTime() + 1);
  return xbar_();
}
 
/*inline*/ Vector KalmanFilterBase::updateStateAndMeasurementPrediction()
{
  updateStatePrediction();
  predictSensor_(this->x_.getTime() + 1);
  return ybar_();
}
 
} // namespace stateObservation
 
#endif // KALMANFILTERBASEHPP