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