Modules/SelfLocator/SpecialPerceptSelfLocator.cpp

Go to the documentation of this file.

/**
 * @file Modules/SelfLocator/SpecialPerceptSelfLocator.cpp
 * 
 * This file contains a class for self-localization based on
 * special percepts, e.g. checkerboard percepts.
 *
 * @author <a href="mailto:dueffert@informatik.hu-berlin.de">Uwe Düffert</a>
 */

#include "SpecialPerceptSelfLocator.h"
#include "Tools/FieldDimensions.h"
#include "Platform/SystemCall.h"
#include <math.h>


SpecialPerceptSelfLocator::SpecialPerceptSelfLocator(const SelfLocatorInterfaces& interfaces)
: SelfLocator(interfaces),lastRobotPose(0,0,0),psdCrashCount(0),
/*
//6x6-Kalman:
H(1,0,0,0,0,0, 0,1,0,0,0,0, 0,0,1,0,0,0),
A(1,0,0,1,0,0, 0,1,0,0,1,0, 0,0,1,0,0,1, 0,0,0,1,0,0, 0,0,0,0,1,0, 0,0,0,0,0,1),
P(1000,10,10,10,10,10,  10,1000,10,10,10,10,  10,10,1000,10,10,10,
  10,10,10,1000,10,10,  10,10,10,10,1000,10,  10,10,10,10,10,1000),
R(10, 0.03, 0.03,   0.03, 20, 0.03,   0.03, 0.03, 5),
K(1,0,0, 0,1,0, 0,0,1, 0,0,0, 0,0,0, 0,0,0)
*/
/*
//2x2-Kalman:
H(1,0),
A(Vector2<double>(1,1),Vector2<double>(0,1)),
P(Vector2<double>(100,1),Vector2<double>(1,100)),
R(10),
K(1,0)
*/
robotPosX (0.15, 0.25, 0.0, 0, -3600, 0, 100),
robotPosY (0.15, 0.25, 0.0, 0, -yPosLeftFlags, yPosLeftFlags, 100),
robotPosR(0.15, 0.25, 0.0, 0, -pi, pi, 1),
lastFrame(0)
{
/*
  //quadratische Kurve:
  perceptPoseBuffer.init();
  perceptTimeBuffer.init();
*/
}

void SpecialPerceptSelfLocator::execute()
{
  selfLocatorSamples.link(NULL, 0,0);
  Pose2D odometry = odometryData - lastOdometry;
  lastOdometry = odometryData;
  robotPose.setFrameNumber(specialPercept.frameNumber);
  static PIDsmoothedValue rspeed(0.1, 0.15, 0.0, 0, -3, 3, 100);
  
  if (specialPercept.type==SpecialPercept::checkerboard)
  {
    psdCrashCount=0;
    /*
    //6x6-Kalman:
    Vector3<double> z(specialPercept.checkerPose.translation.x,specialPercept.checkerPose.translation.y,specialPercept.checkerPose.rotation);
    static Matrix_6x1 x = K * z;
    
    //@todo: framediff * wiederholen ???:
    //@todo: Kalman dokumentieren, wie passt man bei Kalman R ordentlich an
    Matrix_6x1 xpre = A*x;
    Matrix_6x6 Ppre = A*P*A.transpose();
    
    K = Ppre * H.transpose() * (H * Ppre * H.transpose() * R).inverse();
    x = xpre + K * (z - (H * xpre));
    P = (Matrix_6x6() - (K * H)) * Ppre;
    */
    
    /*
    //2x2-Kalman nahezu:
    double z=specialPercept.checkerPose.translation.x;
    static Vector2<double> x = K * z;
    
    //@todo: framediff * wiederholen ???:
    //@todo: Kalman dokumentieren, wie passt man bei Kalman R ordentlich an
    Math::
      Vector2<double> xpre = (A*x);
    Matrix2x2<double> Ppre = A*P*A.transpose();
    
    K = Ppre * H.transpose() / (H * Ppre * H.transpose() * R);
    x = xpre + K * (z - (H * xpre));
    P = (Matrix2x2<double>(Vector2<double>(1-K.x*H.x,-K.y*H.x),Vector2<double>(1-K.x*H.y,-K.y*H.y))) * Ppre;
    */
    
    /*
    //quadratische Kurve:
    unsigned long time=specialPercept.frame; //SystemCall::getCurrentSystemTime();
    perceptTimeBuffer.add(time);
    perceptPoseBuffer.add(specialPercept.checkerPose);
    long tdiff=perceptTimeBuffer[0]-perceptTimeBuffer[perceptTimeBuffer.getNumberOfEntries()-1];
    
    if ((perceptTimeBuffer.getNumberOfEntries()>4)&&(tdiff<1000))
    {
      //some helping variables:
      double St0=0,St1=0,St2=0,St3=0,St4=0,Sx1=0,St1x1=0,St2x1=0,Sy1=0,St1y1=0,St2y1=0,Sr1=0,St1r1=0,St2r1=0;
      for (int i=0;i<perceptTimeBuffer.getNumberOfEntries();i++)
      {
        double t=perceptTimeBuffer[0]-perceptTimeBuffer[i];
        double x=perceptPoseBuffer[i].translation.x;
        double y=perceptPoseBuffer[i].translation.y;
        //rotation only works because robot is always facing forwards -> no -pi/pi jump
        double r=perceptPoseBuffer[i].rotation;
        St0 ++;
        St1 += t;
        St2 += t*t;
        St3 += t*t*t;
        St4 += t*t*t*t;
        
        Sx1 += x;
        St1x1 += t*x;
        St2x1 += t*t*x;
        
        Sy1 += y;
        St1y1 += t*y;
        St2y1 += t*t*y;
        
        Sr1 += r;
        St1r1 += t*r;
        St2r1 += t*t*r;
      }
      
      //approximate the t coordinates with quadratic equation f_x(t)=m_x*t^2+n_x*t+o_x, minimize square error, result (Mathematica):
      double divisor = (St0*St3*St3 + St1*St1*St4 - St2*(2*St1*St3 + St0*St4 - St2*St2));
      //double m_x = (St0*(St1x1*St3 - St2*St2x1) + St2*St2*Sx1 - St1*(St1x1*St2 + St3*Sx1 - St1*St2x1)) / divisor;
      //double n_x = (St2*(St1x1*St2 - St1*St2x1 - St3*Sx1) + St0*(St2x1*St3 - St1x1*St4) + St1*St4*Sx1) / divisor;
      double o_x = (St1*(St1x1*St4 - St2x1*St3) + St3*St3*Sx1 - St2*(St1x1*St3 + St4*Sx1 - St2*St2x1)) / divisor;
      //approximate the y coordinates with quadratic equation f_y(t)=m_y*t^2+n_y*t+o_y, minimize square error, result (Mathematica):
      //double m_y = (St0*(St1y1*St3 - St2*St2y1) + St2*St2*Sy1 - St1*(St1y1*St2 + St3*Sy1 - St1*St2y1)) / divisor;
      //double n_y = (St2*(St1y1*St2 - St1*St2y1 - St3*Sy1) + St0*(St2y1*St3 - St1y1*St4) + St1*St4*Sy1) / divisor;
      double o_y = (St1*(St1y1*St4 - St2y1*St3) + St3*St3*Sy1 - St2*(St1y1*St3 + St4*Sy1 - St2*St2y1)) / divisor;
      //approximate the rotation with quadratic equation f_r(t)=m_r*t^2+n_r*t+o_r, minimize square error, result (Mathematica):
      //double m_r = (St0*(St1r1*St3 - St2*St2r1) + St2*St2*Sr1 - St1*(St1r1*St2 + St3*Sr1 - St1*St2r1)) / divisor;
      //double n_r = (St2*(St1r1*St2 - St1*St2r1 - St3*Sr1) + St0*(St2r1*St3 - St1r1*St4) + St1*St4*Sr1) / divisor;
      double o_r = (St1*(St1r1*St4 - St2r1*St3) + St3*St3*Sr1 - St2*(St1r1*St3 + St4*Sr1 - St2*St2r1)) / divisor;
      
      //2do: Ausreisserfilter?
      
      //double t=time;
      //robotPose.translation.x = m_x*m_x*t+n_x*t+o_x;
      //robotPose.translation.y = m_y*m_y*t+n_y*t+o_y;
      //robotPose.rotation = m_r*m_r*t+n_r*t+o_r;
      robotPose.translation.x = o_x;
      robotPose.translation.y = o_y;
      robotPose.rotation = o_r;
      double validity = 1/
        (
        1+
        fabs(specialPercept.checkerPose.translation.x-robotPose.translation.x)/15+
        fabs(specialPercept.checkerPose.translation.y-robotPose.translation.y)/50+
        fabs(specialPercept.checkerPose.rotation-robotPose.rotation)/0.2
        );
      robotPose.setValidity(validity);
    }
    */
    
    //filter away single outliers
    lastWasOutlier = ((!lastWasOutlier)&&
      ((fabs(specialPercept.checkerPose.translation.x-robotPosX.getVal())>300)||
      (fabs(specialPercept.checkerPose.translation.y-robotPosY.getVal())>300)||
      (fabs(specialPercept.checkerPose.rotation-robotPosR.getVal())>0.4)));

    //PIDsmoothed:
    if (lastWasOutlier)
    {
      Pose2D pose = lastRobotPose + odometry;
      robotPose.translation=pose.translation;
      robotPose.rotation=pose.rotation;
      //dont modify validity here, we only misdetected for one single frame
    }
    else
    {
      if (specialPercept.frameNumber-lastFrame>=125)
      {
        robotPosX.resetTo(specialPercept.checkerPose.translation.x);
        robotPosY.resetTo(specialPercept.checkerPose.translation.y);
        robotPosR.resetTo(specialPercept.checkerPose.rotation);
        //2do: 25/30 according to robot type:
        rspeed.resetTo(30*odometry.rotation);
      }
      else
      {
        robotPosX.approximateVal(specialPercept.checkerPose.translation.x);
        robotPosY.approximateVal(specialPercept.checkerPose.translation.y);
        robotPosR.approximateVal(specialPercept.checkerPose.rotation);
      }
      robotPose.translation.x=robotPosX.getVal();
      robotPose.translation.y=robotPosY.getVal();
      robotPose.rotation=robotPosR.getVal();
      robotPose.setValidity(0.9);
      lastFrame=specialPercept.frameNumber;
    }
  }
  else
  {
    lastWasOutlier=false;
    if (psdPercept.numOfPercepts>0)
    {
      for (int i=0;i<psdPercept.numOfPercepts;i++)
      {
        if ((psdPercept[i].isValid)&&
            (!psdPercept[i].tooFarAway)&&
            (psdPercept[i].frameNumber>lastFrame)&&
            ((psdPercept[i].x)*(psdPercept[i].x)+(psdPercept[i].y)*(psdPercept[i].y)<=450*450)&&
            (psdPercept[i].z>130))
        {
          psdCrashCount++;
          //we can only crash into the wall if we are near to it. otherwise it is a measurement error due to stamping or noise
          if ((psdCrashCount>3)&&((robotPose.translation.x>-660)||(psdPercept[i].frameNumber-lastFrame>125)))
          {
            robotPose.translation.x=-400;
            robotPose.translation.y=0;
            robotPose.setValidity(0.05);
            //OUTPUT(idText,text, "psd < 450");
            return;
          }
        }
      }
      //OUTPUT(idText,text, "no checkerboard, psd: x=" << psdPercept[0].x << ", z=" << psdPercept[0].z);
    }
    //we use this approximation while measuring turning:
    robotPose = lastRobotPose + odometry;
    robotPose.setValidity(0.4);
  }
  
  //2do: 25/30 according to robot type:
  rspeed.approximateVal(30*(robotPose.rotation-lastRobotPose.rotation));
  robotPose.speedbyDistanceToGoal = rspeed.getVal();

  lastRobotPose.translation=robotPose.translation;
  lastRobotPose.rotation=robotPose.rotation;
}

/*
 * Change log :
 * 
 * $Log: SpecialPerceptSelfLocator.cpp,v $
 * Revision 1.2  2004/06/09 08:20:46  dueffert
 * some fine tuning
 *
 * Revision 1.1.1.1  2004/05/22 17:20:49  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.15  2004/05/11 16:32:37  dueffert
 * psd crash detection improved
 *
 * Revision 1.14  2004/05/03 18:52:27  dueffert
 * comments corrected
 *
 * Revision 1.13  2004/03/29 15:31:13  dueffert
 * misdetection of near checkerboard corrected
 *
 * Revision 1.12  2004/03/26 09:22:22  dueffert
 * crash detection improved
 *
 * Revision 1.11  2004/03/19 09:20:48  dueffert
 * allow higher distance to checkerboard for 400mm/s-measurement
 *
 * Revision 1.10  2004/03/15 12:34:05  dueffert
 * cool filter for single outliers implemented
 *
 * Revision 1.9  2004/03/08 02:11:48  roefer
 * Interfaces should be const
 *
 * Revision 1.8  2004/02/29 13:42:30  dueffert
 * beautified
 *
 * Revision 1.7  2004/02/23 16:45:02  dueffert
 * ERS7 adaptions of evolution
 *
 * Revision 1.6  2004/01/20 16:20:03  dueffert
 * frameNumber handling corrected
 *
 * Revision 1.5  2004/01/16 15:47:51  dueffert
 * some improvements
 *
 * Revision 1.4  2003/11/14 19:02:26  goehring
 * frameNumber added
 *
 * Revision 1.3  2003/11/13 16:47:38  goehring
 * frameNumber added
 *
 * Revision 1.2  2003/11/10 13:34:07  dueffert
 * checkerboard localization improved
 *
 * Revision 1.3  2003/08/09 14:53:10  dueffert
 * files and docu beautified
 *
 * Revision 1.2  2003/07/30 14:47:46  dueffert
 * SpecialPerceptSelfLocator for Checkboard added
 *
 * Revision 1.1.1.1  2003/07/02 09:40:24  cvsadm
 * created new repository for the competitions in Padova from the 
 * tamara CVS (Tuesday 2:00 pm)
 *
 * removed unused solutions
 *
 * Revision 1.2  2003/05/21 13:36:20  brunn
 * set selfLocatorSamples to NULL as these locators do not have any samples
 *
 * Revision 1.1  2003/01/22 14:59:49  dueffert
 * checkerboard stuff added
 *
 *
 */

---