Modules/BallLocator/GT2004ProcessModels/FixedPositionModel.h

Go to the documentation of this file.

/**
* @file FixedPositionModel.h
* Contains a KalmanBallLocator process model for non-moving balls
*
* @author <a href="mailto:stefanuhrig@gmx.net">Stefan Uhrig</a>
*/
//------------------------------------------------------------------------------
#ifndef FIXEDPOSITIONMODEL_H_INCLUDED
#define FIXEDPOSITIONMODEL_H_INCLUDED
//------------------------------------------------------------------------------
#include "BaseModel.h"
#include "Tools/Math/Vector_n.h"
#include "Tools/Math/Matrix_nxn.h"
//------------------------------------------------------------------------------
/**
* @class KalmanFixedPositionModel
* Contains a KalmanBallLocator process model for non-moving balls
*/
class KalmanFixedPositionModel : public KalmanProcessModelBase
{
public:
  KalmanFixedPositionModel();
  virtual ~KalmanFixedPositionModel();

public:
  virtual const char* getModelName() const { return "FixedPositionModel"; }

  virtual int  getStateDim() const { return 2; }
  virtual void getP(double* pDest) const { P.copyTo(pDest); }
  virtual void getQ(double* pDest) const { globQ.copyTo(pDest); }
  virtual void setQ(const double* pSource) { globQ = pSource; }
  virtual void getR(double* pDest) const { globR.copyTo(pDest); }
  virtual void setR(double* pSource) { globR = pSource; }
  
  virtual void reset();
  virtual KalmanUpdateResult update(double time, double x, double y);
  virtual KalmanPredictResult predict(double time);

  virtual void adapt(const OdometryData& lastOdometry,
                     const OdometryData& actualOdometry);

private:
  /**
  * Actual state:
  * x_act[0] is x-position in meters
  * x_act[1] is y-position in meters
  */
  Vector_n<double, 2>   x_act;
  Matrix_nxn<double, 2> A;     ///< linear process model matrix
  Matrix_nxn<double, 2> P;     ///< internal covariance matrix
  Matrix_nxn<double, 2> Q;     ///< process model error covariance matrix
  Matrix_nxn<double, 2> R;     ///< measurement error covariance matrix

  bool inited;                 ///< true, if model is initied
  double lastLiklihood;        ///< last liklihood
  double lastTime;             ///< last time in seconds

  // Temporary variables for permorming updates und predictions
  Vector_n<double, 2>   x_minus;
  Matrix_nxn<double, 2> P_minus;
  Matrix_nxn<double, 2> C;
  Matrix_nxn<double, 2> C_inv;
  Matrix_nxn<double, 2> K;
  Vector_n<double, 2>   z;
  Vector_n<double, 2>   x_predict;

   /// global process covariance matrix that is adapted with dt
  Matrix_nxn<double, 2> globQ;
   /// global measurement covariance matrix
  Matrix_nxn<double, 2> globR;

  static double defaultA[4]; ///< default linear process model matrix
  static double defaultP[4]; ///< default internal covariance matrix
  static double defaultQ[4]; ///< default process error covariance matrix
  static double defaultR[4]; ///< default measurement error covariance matrix
};
//------------------------------------------------------------------------------
#endif
//------------------------------------------------------------------------------

/*
 * Change log :
 * $Log: FixedPositionModel.h,v $
 * Revision 1.1  2004/07/14 21:40:15  spranger
 * moved kalmanprocessmodel to gt2004processmodel
 *
 * Revision 1.1.1.1  2004/05/22 17:15:26  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.3  2004/04/20 14:12:16  uhrig
 *  - odometry processing corrected
 *  - fixed position model disabled
 *  - covariance matrices tuned
 *
 * Revision 1.2  2004/03/15 12:28:52  risler
 * change log added
 *
 *
 */

---