Modules/BallLocator/GT2004ProcessModels/FixedPositionModel.cpp

Go to the documentation of this file.

//------------------------------------------------------------------------------
#include "Tools/Math/MVTools.h"
#include "FixedPositionModel.h"
#include "Tools/Debugging/Debugging.h"
//------------------------------------------------------------------------------
// Discrete process model update matrix
double KalmanFixedPositionModel::defaultA[4] =
{
  1.0, 0.0,
  0.0, 1.0
};
// Initial internal covarinace matrix of filter
double KalmanFixedPositionModel::defaultP[4] =
{
  1.0, 0.0,
  0.0, 1.0
};
// Process covariance matrix
// Assume a variance of 0.0001 m^2
// Must be multiplied with powers of dt
double KalmanFixedPositionModel::defaultQ[4] =
{
  0.01*0.01, 0.00*0.00,
  0.00*0.00, 0.01*0.01
};
// Measurement covariance matrix
// Assume a variance of 0.0016 m^2
double KalmanFixedPositionModel::defaultR[4] =
{
  0.04*0.04, 0.00*0.00,
  0.00*0.00, 0.04*0.04
};
//------------------------------------------------------------------------------
KalmanFixedPositionModel::KalmanFixedPositionModel() : KalmanProcessModelBase()
{
  A = defaultA;
  globQ = defaultQ;
  globR = defaultR;
  reset();
}
//------------------------------------------------------------------------------
KalmanFixedPositionModel::~KalmanFixedPositionModel()
{
}
//------------------------------------------------------------------------------
void KalmanFixedPositionModel::reset()
{
  inited = false;
  lastLiklihood = 0.0;
  P = defaultP;
  lastTime = 0.0;
  x_act[0] = 0.0;
  x_act[1] = 0.0;
}
//------------------------------------------------------------------------------
KalmanUpdateResult
  KalmanFixedPositionModel::update(double time, double x, double y)
{
  KalmanUpdateResult res;
  
  // If no measurement exists the model can't perform a time update
  if (!inited)
  {
    // Set state to measured state
    x_act[0] = x;
    x_act[1] = y;

    inited = true;
    lastTime = time;
    
    res.liklihood = 0.0;
    return res;
  }

  // Check time step
  double dt = time - lastTime;
  if (MVTools::isNearZero(dt))
  {
    // Time difference is too small --> return last state
    res.state.x = x_act[0];
    res.state.y = x_act[1];
    res.state.vx = 0.0;
    res.state.vy = 0.0;
    res.liklihood = lastLiklihood;
    lastTime = time;
    return res;
  }
  
  // Adapt covariance matrix
  double dt2 = dt*dt;
  Q = globQ;
  Q[0][0] *= dt2;
  Q[1][1] *= dt2;

  try
  {
    // Perform time update
    try
    {
      x_minus = A*x_act;                              // (2.1)
      P_minus = A*P*transpose(A) + Q;                 // (2.2)
    }
    catch (MVException m)
    {
      // We should not end up here
      OutputException("time update", m);
      reset();
      res.liklihood = 0.0;
      return res;
    }

    // Set measured state
    z[0] = x;
    z[1] = y;

    // Calculate weight (see equation 2.6)
    R = globR;
    C = P_minus + R;
    double determinant;
    
    try
    {
      C_inv = invert(C);  
      determinant = det(C);
      if (determinant < 0.0)
        throw MVException();
      if (MVTools::isNearZero(determinant))
        throw MVException();
    }
    catch (MVException m)
    {
      // We should not end up here
      OutputException("weight calculation", m);
      reset();
      res.liklihood = 0.0;
      return res;
    }   
    
    double exponent = -0.5*(z-x_minus)*(C_inv*(z-x_minus));
    if (exponent > MVTools::getMaxExpDouble())
    {
      // Sometimes exponent ist too high to represent the result with
      // a double. Set it to the maximum value
      exponent = MVTools::getMaxExpDouble();
    }
    res.liklihood = exp(exponent)/(2*pi * sqrt(determinant));

    // Measurement update
    try
    {
      K = P_minus * C_inv;                            // (2.3)
      x_act = x_minus + K*(z - x_minus);              // (2.4)
      P = (P.eye() - K)*P_minus;                      // (2.5)
    }
    catch (MVException m)
    {
      // We should not end up here
      OutputException("measurement update", m);
      reset();
      res.liklihood = 0.0;
      return res;
    }

    // Set state und save information of this update step
    lastLiklihood = res.liklihood;
    res.state.x = x_act[0];
    res.state.y = x_act[1];
    res.state.vx = 0.0;
    res.state.vy = 0.0;
    lastTime = time;
  }
  catch (...)
  {
    // We should not end up here
    OUTPUT(idText, text,
           "UnknownException in KalmanFixedPositionModel::update()");
    reset();
    res.liklihood = 0.0;
    return res;
  }

  return res;
}
//------------------------------------------------------------------------------
KalmanPredictResult KalmanFixedPositionModel::predict(double time)
{
  KalmanPredictResult res;

  // If model was not initialised yet we can't predict a state
  if (!inited)
    return res;

  // Predict a state using the process model
  try
  {
    x_predict = A*x_act;
  }
  catch (MVException m)
  {
    // We should not end up here
    OutputException("prediction", m);
    reset();
    res.liklihood = 0.0;
    return res;
  }
  res.state.x = x_predict[0];
  res.state.y = x_predict[1];
  res.state.vx = 0.0;
  res.state.vy = 0.0;
  res.liklihood = lastLiklihood;

  return res;
}
//------------------------------------------------------------------------------
void KalmanFixedPositionModel::adapt(const OdometryData& lastOdometry,
                                     const OdometryData& actualOdometry)
{
  // Adapt state to the new position of the robot
  Vector2<double> tempBallPosition = 
    (lastOdometry + Pose2D(x_act[0]*1000.0, x_act[1]*1000.0) -
     actualOdometry).translation;
  x_act[0] = tempBallPosition.x / 1000.0;
  x_act[1] = tempBallPosition.y / 1000.0;
}
//------------------------------------------------------------------------------

/*
 * Change log :
 * $Log: FixedPositionModel.cpp,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
 *
 *
 */

---