Modules/BallLocator/GT2004ProcessModels/ConstantSpeedModel.cpp

Go to the documentation of this file.

/**
* @file ConstantSpeedModel.cpp
* Contains a KalmanBallLocator process model for balls with constant speed
*
* @author <a href="mailto:stefanuhrig@gmx.net">Stefan Uhrig</a>
*/
//------------------------------------------------------------------------------
#include "Tools/Math/MVTools.h"
#include "ConstantSpeedModel.h"
#include "Tools/Debugging/Debugging.h"
//------------------------------------------------------------------------------
// Discrete process update matrix (dt must be set during update)
double KalmanConstantSpeedModel::defaultA[16] =
{
  1.0, 0.0, 0.0, 0.0,
  0.0, 1.0, 0.0, 0.0,
  0.0, 0.0, 1.0, 0.0,
  0.0, 0.0, 0.0, 1.0
};
// Initial internal covariance matrix of filter
double KalmanConstantSpeedModel::defaultP[16] = 
{
  1.0, 0.0, 0.0, 0.0,
  0.0, 1.0, 0.0, 0.0,
  0.0, 0.0, 1.0, 0.0,
  0.0, 0.0, 0.0, 1.0
};
// Process covariance matrix
// Entries must be multiplied with powers of dt during update
// Assume variances of 0.004 m^2 and 0.004 m^2 per s^2
double KalmanConstantSpeedModel::defaultQ[16] =
{
  0.004, 0.000, 0.004, 0.000,
  0.000, 0.004, 0.000, 0.004,
  0.004, 0.000, 0.004, 0.000,
  0.000, 0.004, 0.000, 0.004
};
// Measurement covariance matrix
// Assume variances of 0.0001 m^2 and 0.0001 m^2 per s^2
double KalmanConstantSpeedModel::defaultR[16] =
{
  0.01*0.01, 0.00*0.00, 0.00*0.00, 0.00*0.00,
  0.00*0.00, 0.01*0.01, 0.00*0.00, 0.00*0.00,
  0.00*0.00, 0.00*0.00, 0.03*0.03, 0.00*0.00,
  0.00*0.00, 0.00*0.00, 0.00*0.00, 0.03*0.03
};
//------------------------------------------------------------------------------
KalmanConstantSpeedModel::KalmanConstantSpeedModel() : KalmanProcessModelBase(), 
      panningVelocityThreshold(pi/3) //current threshold is 60°/s
{
  A = defaultA;
  globQ = defaultQ;
  globR = defaultR;

  // Calculate variance adaption polynom
  double x[2] =
  {
    0.0, // 0m
    1.5 // 1.5m
  };
  double fx[2] =
  {
    0.01*0.01, // 1cm
    0.1*0.1   // 10cm
  };
  double Vx[2] =
  {
    0.0, // 0m
    2.5 // 2.5m
  };
  double Vfx[2] =
  {
    0.0001, 
    0.1   
  };
  calculateVarianceAdaptionPolynom(x, fx, varianceAdaptionPosition);
  calculateVarianceAdaptionPolynom(Vx, Vfx, varianceAdaptionSpeed);
 
  reset();
}
//------------------------------------------------------------------------------
KalmanConstantSpeedModel::~KalmanConstantSpeedModel()
{
}
//------------------------------------------------------------------------------
void KalmanConstantSpeedModel::calculateVarianceAdaptionPolynom(
                                 double x[2], double fx[2], double result[2])
{
  double dMatrixValues[4] =
  {
    x[0]*x[0],  1,
    x[1]*x[1],  1
  };
  Matrix_nxn<double, 2> A(dMatrixValues);
  Vector_n<double, 2> b(fx);

  Vector_n<double, 2> sol = A.solve(b);
  result[0] = sol[0];
  result[1] = sol[1];
}
//------------------------------------------------------------------------------
void KalmanConstantSpeedModel::reset()
{
  initState = NotInited;
  P = defaultP;
  lastLiklihood = 0.0;
  lastTime = 0.0;
  double zeros[4] = {0.0, 0.0, 0.0, 0.0};
  x_act = zeros;
}
//------------------------------------------------------------------------------
KalmanUpdateResult
  KalmanConstantSpeedModel::update(double time, double x, double y, const RobotPose& pose, double panningVelocity)
{
  KalmanUpdateResult res;

  // Initialise filter if necessary
  if (initState == NotInited)
  {
    // Initialise position
    x_act[0] = z[0] = x;
    x_act[1] = z[1] = y;

    initState = PositionInited;
    lastTime = time;

    res.liklihood = 0.0;
    return res;
  }

  // Determine and check time difference 
  double dt = time - lastTime;
  if (MVTools::isNearZero(dt))
  {
    // Send last state if time difference is near zero
    res.state.x = x_act[0];
    res.state.y = x_act[1];
    res.state.vx = x_act[2];
    res.state.vy = x_act[3];
    res.liklihood = lastLiklihood;
    return res;
  }

  if (initState == PositionInited)
  {
    // Initialise speed
    x_act[2] = z[2] = (x - x_act[0]) / dt;
    x_act[3] = z[3] = (y - x_act[1]) / dt;
    x_act[0] = z[0] = x;
    x_act[1] = z[1] = y;

    initState = Ready;
    lastTime = time;

    res.liklihood = 0.0;
    return res;
  }

  
  try
  {
    // Adapt process covariance matrix
    double dt2 = dt*dt;
    double dt3 = dt2*dt;
    double dt4 = dt2*dt2;
    Q = globQ;
    Q[0][0] *= dt4;
    Q[0][2] *= dt3;
    Q[1][1] *= dt4;
    Q[1][3] *= dt3;
    Q[2][0] *= dt3;
    Q[2][2] *= dt2;
    Q[3][1] *= dt3;
    Q[3][3] *= dt2;
  
    // Perform time update
    try
    {
      A[0][2] = dt;
      A[1][3] = dt;
      x_minus = A*x_act;                              // (2.1)
      P_minus = A*P*transpose(A) + Q;                 // (2.2)
  
      // Update measured state
      z[2] = (x - z[0])/dt;
      z[3] = (y - z[1])/dt;
      z[0] = x;
      z[1] = y;
    }
    catch (MVException m)
    {
      // We shouldn't end up here      
      OutputException("time-update", m);
      reset();
      res.liklihood = 0.0;
      return res;
    }

    // Adapt measurement covariance matrix
    double radiussquare = x*x + y*y;
    double angle = atan2(y,x);
    double ca = cos(angle);
    double sa = sin(angle);

    double posA, posB, spdA, spdB;
    
    double abAspectRatio = (panningVelocity < panningVelocityThreshold) ? 0.15 : 0.15*panningVelocity/(panningVelocityThreshold); 
    double bPanningFactor = (panningVelocity < panningVelocityThreshold) ? 1.0 : 1.0*panningVelocity/(panningVelocityThreshold);
    posB = varianceAdaptionPosition[0]*radiussquare +
        varianceAdaptionPosition[1];
    posB *= bPanningFactor;
    posA = posB*abAspectRatio;

    spdB = varianceAdaptionSpeed[0]*radiussquare +
        varianceAdaptionSpeed[1];
    spdB *= bPanningFactor;
    spdA = spdB*abAspectRatio;
  
    Matrix2x2<double> Lambda(Vector2<double>(posB, 0), Vector2<double>(0, posA));
    Matrix2x2<double> M(Vector2<double>(ca, sa), Vector2<double>(-sa, ca));
    Matrix2x2<double> Rxy = M*Lambda*M.transpose(); 
    Lambda[0][0] = spdB;
    Lambda[1][1] = spdA;
    Matrix2x2<double> RVxy = M*Lambda*M.transpose(); 

    Lambda[0][0] = sqrt(posB);
    Lambda[1][1] = sqrt(posA);
    Matrix2x2<double> draw = M*Lambda;
    
    Vector2<double> ellipseA(1000.0*draw[0][0], 1000.0*draw[0][1]);
    Vector2<double> ellipseB(1000.0*draw[1][0], 1000.0*draw[1][1]);
    Vector2<double> ballMeasuredPos(x*1000.0, y*1000.0);
    ellipseA += ballMeasuredPos;
    ellipseB += ballMeasuredPos;
    ellipseA = Geometry::relative2FieldCoord(pose, ellipseA.x,
                                  ellipseA.y);
    ellipseB = Geometry::relative2FieldCoord(pose, ellipseB.x,
                                  ellipseB.y);
    ballMeasuredPos = Geometry::relative2FieldCoord(pose, ballMeasuredPos.x,
                                  ballMeasuredPos.y);
    LINE(ballLocatorField, ballMeasuredPos.x, ballMeasuredPos.y, 
         ellipseA.x, ellipseA.y,
         3, 0, Drawings::pink);
    LINE(ballLocatorField, ballMeasuredPos.x, ballMeasuredPos.y, 
         ellipseB.x, ellipseB.y,
         3, 0, Drawings::pink);
         
    R = globR;
    R[0][0] = Rxy[0][0];//NOTE: this is *not* some kind of transposition, it's because
    R[0][1] = Rxy[1][0];// the 2 matrixes use different storing conventions
    R[1][0] = Rxy[0][1];// this will have to be corrected in a new Matrix class
    R[1][1] = Rxy[1][1];// release

    R[2][2] = RVxy[0][0];
    R[2][3] = RVxy[1][0];
    R[3][2] = RVxy[0][1];
    R[3][3] = RVxy[1][1];

    // Calculate weight (see equation 2.6)
    C = P_minus + R;
    double determinant;
    try
    {
      C_inv = invert(C);
      determinant = det(C);
      if (determinant < 0.0)
        throw MVException(MVException::DetNegative);
      if (MVTools::isNearZero(determinant))
        throw MVException(MVException::DetCloseToZero);
    }
    catch (MVException m)
    {
      // We shouldn't end up here      
      OutputException("invert_c", m);
      reset();
      res.liklihood = 0.0;
      return res;
    }
    
    double exponent = -0.5*(z - x_minus)*(C_inv*(z - x_minus));
    MVException m;
    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();
    }
    double denominator = 4*pi*pi*sqrt(determinant);
    res.liklihood = exp(exponent)/(denominator);

    // 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 = x_act[2];
    res.state.vy = x_act[3];
    lastTime = time;
  }
  catch (...)
  {
    // We should not end up here
    OUTPUT(idText, text,
           "UnknownException in KalmanConstantSpeedModel::update()");
    reset();
    res.liklihood = 0.0;
    return res;
  }

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

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

  // Calculate difference between actual time and the time the ball was
  // last seen.
  double dt = time - lastTime;

  // Adapt process matrix A to time difference
  A[0][2] = dt;
  A[1][3] = dt;
    
  // 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 = x_predict[2];
  res.state.vy = x_predict[3];
  res.liklihood = lastLiklihood;

  return res;
}
//------------------------------------------------------------------------------
void KalmanConstantSpeedModel::adapt(const OdometryData& lastOdometry,
                                     const OdometryData& actualOdometry)
{
  // Adapt state and measured 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;
    
  tempBallPosition = 
    (lastOdometry + Pose2D(z[0]*1000.0, z[1]*1000.0) -
     actualOdometry).translation;
  z[0] = tempBallPosition.x / 1000.0;
  z[1] = tempBallPosition.y / 1000.0;

  double vx, vy;
  double speed_rot = lastOdometry.getAngle() - actualOdometry.getAngle();
  vx = x_act[2];
  vy = x_act[3];
  x_act[2] = vx*cos(speed_rot) - vy*sin(speed_rot);
  x_act[3] = vx*sin(speed_rot) + vy*cos(speed_rot);
  
  vx = z[2];
  vy = z[3];
  z[2] = vx*cos(speed_rot) - vy*sin(speed_rot);
  z[3] = vx*sin(speed_rot) + vy*cos(speed_rot);
}
//------------------------------------------------------------------------------

/*
 * Change log :
 * $Log: ConstantSpeedModel.cpp,v $
 * Revision 1.1  2004/07/14 21:40:15  spranger
 * moved kalmanprocessmodel to gt2004processmodel
 *
 * Revision 1.12  2004/07/07 10:15:39  uhrig
 * Fixed doucmentation (for doxygen)
 *
 * Revision 1.11  2004/06/30 18:05:18  nistico
 * Kalman "critical" panning speed threshold raised to 60°/s
 *
 * Revision 1.10  2004/06/22 09:43:17  nistico
 * The radial component of the measurement variance is
 * also affected by panning velocity now
 *
 * Revision 1.9  2004/06/22 09:10:48  uhrig
 * Bugfix in predict function (process matrix A was not adapted to time difference)
 *
 * Revision 1.8  2004/06/21 11:55:54  nistico
 * Comments / cleaning
 *
 * Revision 1.7  2004/06/20 20:04:38  kindler
 * - adapted to new Matrix2x2 operator[] access method.
 *
 * Revision 1.6  2004/06/18 10:24:51  nistico
 * Kalman measurement covariance matrix now adjusted also based on
 * head panning angular velocity
 *
 * Revision 1.5  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.4  2004/04/20 14:12:16  uhrig
 *  - odometry processing corrected
 *  - fixed position model disabled
 *  - covariance matrices tuned
 *
 * Revision 1.3  2004/04/07 12:28:56  risler
 * ddd checkin after go04 - first part
 *
 * Revision 1.2  2004/03/30 15:47:30  risler
 * covariance matrix fixed should be symmetric
 *
 * Revision 1.1.1.1  2004/03/29 08:28:51  Administrator
 * initial transfer from tamara
 *
 * Revision 1.2  2004/03/15 12:28:52  risler
 * change log added
 *
 *
 */

---