Modules/BallLocator/GT2004ProcessModels/BaseModel.cpp

Go to the documentation of this file.

/**
* @file BaseModel.cpp
*
* @author <a href="mailto:stefanuhrig@gmx.net">Stefan Uhrig</a>
*/
//------------------------------------------------------------------------------
#include "BaseModel.h"
#include "Tools/FieldDimensions.h"
#include "Tools/Debugging/Debugging.h"
#include <math.h>
#include <string.h>
//------------------------------------------------------------------------------
double KalmanProcessModelBase::pi = 3.1415926535897932384626433832795;
double KalmanProcessModelBase::pi2 = 2*3.1415926535897932384626433832795;
double KalmanProcessModelBase::dFieldDiagonalLength =
  1.2*sqrt((2.0*xPosOpponentGoal)*(2.0*xPosOpponentGoal) +
           (2.0*yPosLeftFlags)*(2.0*yPosLeftFlags));
//------------------------------------------------------------------------------
KalmanBallState::KalmanBallState() : x(0.0), y(0.0), vx(0.0), vy(0.0)
{
}
//------------------------------------------------------------------------------
KalmanUpdateResult::KalmanUpdateResult() : liklihood(0.0)
{
}
//------------------------------------------------------------------------------
KalmanPredictResult::KalmanPredictResult() : liklihood(0.0)
{
}
//------------------------------------------------------------------------------
KalmanProcessModelBase::KalmanProcessModelBase() 
: bUseModel(true)
{
}
//------------------------------------------------------------------------------
bool KalmanProcessModelBase::isNonSensePos(double x, double y) const
{
  double dist = sqrt(x*x + y*y);
  if (dist > dFieldDiagonalLength)
    return true;
  else
    return false;
}
//------------------------------------------------------------------------------
Out& KalmanProcessModelBase::toStream(Out& stream) const
{
  // Write model name
  stream << strlen(getModelName()) << getModelName();

  // Write "use" flag
  int nUse = bUseModel?1 : 0;
  stream << nUse;
  
  // Write dimension of state
  int n = getStateDim();
  stream << n;

  int i;
  double* M = new double[n*n];
  
  // Write process covariance matrix Q
  getQ(M);
  for (i = 0; i < n*n; ++i)
    stream << M[i];

  // Write measurement covariance matrix R
  getR(M);
  for (i = 0; i < n*n; ++i)
    stream << M[i];
  
  delete [] M;  
  return stream;
}
//------------------------------------------------------------------------------
In& KalmanProcessModelBase::fromStream(In& stream)
{
  // Read "use" flag of model
  int nUse;
  stream >> nUse;
  bUseModel = (nUse != 0);
  
  // Read state dimension
  int i, n;
  stream >> n;

  double* M = new double[n*n];

  // Read process covarinace matrix Q
  for (i = 0; i < n*n; ++i)
    stream >> M[i];
  setQ(M);

  // Read measurement covariance matrix R
  for (i = 0; i < n*n; ++i)
    stream >> M[i];
  setR(M);

  delete [] M;

  // Reset model because parameters have changed
  reset();
  return stream;
}
//------------------------------------------------------------------------------
void KalmanProcessModelBase::OutputException(const char* pszLoc,
                                             MVException& m) const
{
  OUTPUT(idText, text,
         getModelName() << ": Exception "
         << m.getDescription() << " in " << pszLoc);
}
//------------------------------------------------------------------------------

/*
 * Change log :
 * $Log: BaseModel.cpp,v $
 * Revision 1.1  2004/07/14 21:40:15  spranger
 * moved kalmanprocessmodel to gt2004processmodel
 *
 * Revision 1.3  2004/06/15 22:14:09  uhrig
 * Tidied up a little bit
 *
 * 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.4  2004/04/22 14:28:02  roefer
 * .NET errors/warnings removed
 *
 * 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
 *
 *
 */

---