Modules/BallLocator/GT2004ProcessModels/ConstantSpeedModel.h

Go to the documentation of this file.

/**
* @file ConstantSpeedModel.h
* Contains a KalmanBallLocator process model for balls with constant speed
*
* @author <a href="mailto:stefanuhrig@gmx.net">Stefan Uhrig</a>
*/
//------------------------------------------------------------------------------
#ifndef CONSTANTSPEEDMODEL_H_INCLUDED
#define CONSTANTSPEEDMODEL_H_INCLUDED
//------------------------------------------------------------------------------
#include "BaseModel.h"
#include "Tools/Math/Vector_n.h"
#include "Tools/Math/Matrix_nxn.h"
#include "Tools/Math/Matrix2x2.h"
#include "Tools/Math/Geometry.h"
#include "Tools/Debugging/DebugDrawings.h"
//------------------------------------------------------------------------------
/**
* @class KalmanConstantSpeedModel
* Contains a KalmanBallLocator process model for balls with constant speed
*/
class KalmanConstantSpeedModel : public KalmanProcessModelBase
{
public:
  KalmanConstantSpeedModel();
  virtual ~KalmanConstantSpeedModel();

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

  virtual int  getStateDim() const { return 4; }
  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,
                                    const RobotPose& pose,
                                    double panningVelocity);
  virtual KalmanPredictResult predict(double time);

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

private:
  /**
  * Calculates the coefficients for the variance adpation Polynom.
  * @param x The x values to take
  * @param fx The f(x) values at the x positions
  * @param result The resultung coefficients
  */
  void calculateVarianceAdaptionPolynom(double x[2], double fx[2],
                                        double result[2]);

  const double panningVelocityThreshold;

  /**
  * Actual state:
  * x_act[0] is x-position in meters relative to robot
  * x_act[1] is y-position in meters relative to robot
  * x_act[2] is x-direction speed in meters per second
  * x_act[3] is y-direction speed in meters per second
  */  
  Vector_n<double, 4>   x_act;
  Vector_n<double, 4>   z;     ///< measured state
  Matrix_nxn<double, 4> A;     ///< linear process model matrix
  Matrix_nxn<double, 4> P;     ///< internal covariance matrix
  Matrix_nxn<double, 4> Q;     ///< process model error covariance matrix
  Matrix_nxn<double, 4> R;     ///< measurement error covariance matrix

  enum InitState
  {
    NotInited,
    PositionInited,
    Ready
  };
  InitState initState;  ///< initialization state of the filter
  double lastLiklihood; ///< last liklihood of model (for predict function)
  double lastTime;      ///< last time update permormed

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

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

  /**
  * Coefficients of polynom of degree 2 for variance adaption
  * Constant coefficient is the last one
  */
  double varianceAdaptionPosition[3];
  double varianceAdaptionSpeed[3];

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

/*
 * Change log :
 * $Log: ConstantSpeedModel.h,v $
 * Revision 1.1  2004/07/14 21:40:15  spranger
 * moved kalmanprocessmodel to gt2004processmodel
 *
 * Revision 1.7  2004/07/07 10:15:39  uhrig
 * Fixed doucmentation (for doxygen)
 *
 * Revision 1.6  2004/06/22 09:43:17  nistico
 * The radial component of the measurement variance is
 * also affected by panning velocity now
 *
 * Revision 1.5  2004/06/18 10:24:51  nistico
 * Kalman measurement covariance matrix now adjusted also based on
 * head panning angular velocity
 *
 * Revision 1.4  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.3  2004/06/15 22:14:10  uhrig
 * Tidied up a little bit
 *
 * Revision 1.2  2004/06/15 17:33:00  nistico
 * Measurement model for position now depends on distance, retuned speed
 * component variance.
 * The code needs to be cleaned up and beautified.
 *
 * 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
 *
 *
 */

---