Modules/BallLocator/GT2004ProcessModels/BaseModel.h

Go to the documentation of this file.

  /**
* @file BaseModel.h
* Contains base classes for implemting a KalmanBallLocator process model
*
* @author <a href="mailto:stefanuhrig@gmx.net">Stefan Uhrig</a>
*/
//------------------------------------------------------------------------------
#ifndef BASEMODEL_H_INCLUDED
#define BASEMODEL_H_INCLUDED
//------------------------------------------------------------------------------
#include "Representations/Motion/OdometryData.h"
#include "Tools/Streams/InOut.h"
#include "Tools/Math/Vector_n.h"
#include "Representations/Cognition/RobotPose.h"
//------------------------------------------------------------------------------
/**
* @class KalmanBallState
* Contains the supposed ball state
*/
class KalmanBallState
{
public:
  KalmanBallState();

public:
  double x;  ///< x-position in meter relative to robot
  double y;  ///< y-position in meter relative to robot
  double vx; ///< x-direction speed in meters per second
  double vy; ///< x-direction speed in meters per second
};
//------------------------------------------------------------------------------
/**
* @class KalmanUpdateResult
* Contains the results of an update call of a KalmanBallLocator process model
*/
class KalmanUpdateResult
{
public:
  KalmanUpdateResult();

public:
  KalmanBallState state; ///< state of the ball
  double liklihood;      ///< liklihood (not probability!) of the model
};
//------------------------------------------------------------------------------
/**
* @class KalmanPredictResult
* Contains the results of a predict call of a KalmanBallLocator process model
*/
class KalmanPredictResult
{
public:
  KalmanPredictResult();

public:
  KalmanBallState state; ///< predicted state of the ball
  double liklihood;      ///< predicted liklihood of the state
};
//------------------------------------------------------------------------------
/**
* @class KalmanProcessModelBase
* Virtual base class defining the functions necessary to implement a
* KalmanBallLocator process model.
*/
class KalmanProcessModelBase
{
public:
  KalmanProcessModelBase();
  virtual ~KalmanProcessModelBase() {}

public:
  /**
  * Returns the name of the process model
  * @return name of process model
  */
  virtual const char* getModelName() const = 0;
  /**
  * Returns the dimension of the state
  * @return dimension of the state
  */
  virtual int  getStateDim() const = 0;
  /**
  * Retrieves the internal kalman filter covariance matrix
  * @param pDest destination to store the covariance matrix to
  */
  virtual void getP(double* pDest) const = 0;
  /**
  * Retrieves the process error covariance matrix
  * @param pDest destination to store the covariance matrix to
  */
  virtual void getQ(double* pDest) const = 0;
  /** Sets the process error covariance matrix
  * @param pSource array containing the covariance matrix entries
  */
  virtual void setQ(const double* pSource) = 0;
  /**
  * Retrieves the measurement error covariance matrix
  * @param pDest destination to store the covariance matrix to
  */
  virtual void getR(double* pDest) const = 0;
  /** Sets the measurement error covariance matrix
  * @param pSource array containing the covariance matrix entries
  */
  virtual void setR(double* pSource) = 0;
  
  /** Resets the process model */
  virtual void reset() = 0;
  
  /**
  * Performs an update step of the process model
  * @param time time in seconds when the percepts were determined
  * @param x measured x-position of the ball in meters relative to the robot
  * @param y measured y-position of the ball in meters relative to the robot
  * @param pose the current robot pose, used for visualization
  * @param panningVelocity the head panning angular velocity, used to adjust the R covariance matrix
  * @return the update results of the process model
  */
  virtual KalmanUpdateResult update(double time, double x, double y, const RobotPose& pose, double panningVelocity) = 0;
  /**
  * Permorms a prediction of the ball state
  * @param time actual time in seconds
  * @return the predict results of the process model
  */
  virtual KalmanPredictResult predict(double time) = 0;

  /**
  * Function to adapt the state of the model when position and rotation of the robot changes.
  * @param lastOdometry last odometry of robot (original values in millimeters are used)
  * @param actualOdometry actual odometry of robot (original values in millimeters are used) 
  */
  virtual void adapt(const OdometryData& lastOdometry,
                     const OdometryData& actualOdometry) = 0;

  /**
  * Returns true, if the passed ball position is nonsense
  * (because it is far away from the field)
  * @return true, if passed position is nonsense
  */
  virtual bool isNonSensePos(double x, double y) const;

  /**
  * Returns true, if the model is to be used
  * @return true, if model is to be used
  */
  virtual bool useModel() const { return bUseModel; }

  /**
  * Writes data of the actual process model data to a stream (for debugging)
  * @param stream stream to write the process model data to
  * @return reference to the passed stream
  */
  virtual Out& toStream(Out& stream) const;
  /**
  * Reads settings for a process model from a stream (e.g. when covariance
  * matrices are changed by user.
  * @param stream stream to read settings from
  * @return reference to passed stream
  */
  virtual In& fromStream(In& stream);

protected:
  /**
  * Outputs information about the process model that caused an exception
  * and information about the type of exception
  * @param pszLoc description of calculation step the exception occured in
  * @param m reference to exception
  */
  void OutputException(const char* pszLoc, MVException& m) const;

protected:
  bool bUseModel; ///< true, if the model is to be used  
  static double pi;  ///< constant pi
  static double pi2; ///< constant 2*pi
  //! 1.2*length of field diagonal
  static double dFieldDiagonalLength;
};
//------------------------------------------------------------------------------
#endif
//------------------------------------------------------------------------------

/*
 * Change log :
 * $Log: BaseModel.h,v $
 * Revision 1.1  2004/07/14 21:40:15  spranger
 * moved kalmanprocessmodel to gt2004processmodel
 *
 * Revision 1.4  2004/06/18 10:24:51  nistico
 * Kalman measurement covariance matrix now adjusted also based on
 * head panning angular velocity
 *
 * Revision 1.3  2004/06/16 17:57:00  nistico
 * Speed covariance submatrix dynamically calculated
 * based on distance from ball
 * More detailed identification of exception
 * Bug fixes
 *
 * Revision 1.2  2004/06/15 21:40:14  uhrig
 * Added possibility to disable and enable process models in the Kalman filter.
 *
 * Revision 1.1.1.1  2004/05/22 17:15:25  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
 *
 *
 */

---