CreateMotionModel.h

Go to the documentation of this file.

//-*-c++-*-
#ifndef _CreateMotionModel_h_
#define _CreateMotionModel_h_

#include "Shared/RobotInfo.h"

#ifdef TGT_IS_CREATE

#include "Behaviors/BehaviorBase.h"
#include "Shared/zignor.h"
#include "Shared/Measures.h"
#include "Shared/ParticleFilter.h"
#include "Shared/WorldState.h"

template<class ParticleT>
class CreateMotionModel : public BehaviorBase, public ParticleFilter<ParticleT>::MotionModel {
private:
  float lastdist;
  AngTwoPi lastang;

  /* mean and variance for added gaussian noise */
  float dmean;
  AngTwoPi amean;
  float ddvar, davar, aavar, advar;

  static const float minUpdateDistance;
  static const float minUpdateAngle;

public:
  bool wasUpdated;
  
  typedef typename ParticleFilter<ParticleT>::MotionModel::particle_type particle_type;
  typedef typename ParticleFilter<ParticleT>::MotionModel::particle_collection particle_collection;
  typedef typename particle_collection::size_type index_t;

  //! Constructor
  CreateMotionModel(float dm=0.1f, float am=0.2f, float ddv=0.001f, float dav=0.001f, float aav=0.001f, float adv=0.000001f) :
    BehaviorBase("CreateMotionModel"), ParticleFilter<ParticleT>::MotionModel(),
    lastdist(state->sensors[EncoderDistanceOffset]),
    lastang(state->sensors[EncoderAngleOffset]/180*(float)M_PI),
    dmean(dm), amean(am), ddvar(ddv), davar(dav), aavar(aav), advar(adv),
    wasUpdated(false)
  {}

  float sampleNormal(float mean, float var) {
    // N(m,s^2) ~ s*N(0,1) + m
    return mean + (float)DRanNormalZig32()*std::sqrt(var);
  }

  virtual void updateMotion(particle_collection& particles, particle_type& estimate) {
    float dist = state->sensors[EncoderDistanceOffset];
    AngTwoPi ang = state->sensors[EncoderAngleOffset]/180*(float)M_PI;

    float dd = dist - lastdist;
    AngSignPi da = float(ang - lastang);

    // if we haven't moved far enough, don't bother moving the particles
    if (fabs(dd) < minUpdateDistance && fabs(da) < minUpdateAngle) {
      // std::cout << "Returning distance was not far enough\n";
      wasUpdated = false;
      return;
    }

    lastdist = dist;
    lastang = ang;

    // First update the estimate, using zero noise
    float dx, dy, dtheta;
    computeCreateMotion(dd, float(da), estimate.theta, dx, dy, dtheta);
    estimate.x += dx;
    estimate.y += dy;
    estimate.theta += dtheta;

    // Now  update the particles using our noise model
    for(typename particle_collection::iterator it=particles.begin(); it!=particles.end(); ++it) {
      // noise perturbed distance and angle travelled
      // Note: original code was:
      //
      //    float dnoise = sampleNormal(dmean*dd, ddvar*dd*dd + davar*da*da);
      //    float anoise = sampleNormal(amean*da, aavar*da*da + advar*dd*dd);
      //
      // but with current parameters, this caused a 10% distance
      // overshoot, while the actual robot seems to show a 3%
      // undershoot.  Also, for long turns (90 degrees) the angular
      // error was way too high.  So the version below gives zero mean
      // translational error; might replace this later with original
      // code plus better parameter settings.  Our trick doesn't work
      // for the angular component because if we don't command a turn,
      // the Create will always report 0 angular change so scaling by
      // da would zero the noise entirely.

      float dnoise, anoise;
      dnoise = dmean * dd * sampleNormal(0, ddvar*dd*dd + davar*da*da);
      anoise = float(amean) * sampleNormal(0, aavar*da*da + advar*dd*dd);
      float ddn = dd + dnoise;
      AngSignPi dan = float(da) + anoise;
      computeCreateMotion(ddn, float(dan), float(it->theta), dx, dy, dtheta);
      it->x += dx;
      it->y += dy;
      it->theta += dtheta;
    }

    wasUpdated = true;
  }


private:
  // compute distance update for a given linear distance
  // and a given angular distance traveled
  virtual void computeCreateMotion(float len, float omega, float theta, float &dx, float &dy, float &dtheta) { 
    // check for linear motion
    if ( omega == 0 ) {
      dx = len*std::cos(theta);
      dy = len*std::sin(theta);
      dtheta = 0;
    }
    // curved motion, if len = 0, just rotation
    else {
      // solve for radius of circle
      // note that we don't need to use abs here
      // if len, omega are negative then we want r to be
      // negative because we are effectively flipping the circle
      float r = len / omega;
      
      // find new angles
      // would be this if theta was position around circle:
      //dx = r*cos(theta + omega) - r*cos(theta);
      //dy = r*sin(theta + omega) - r*sin(theta);
      // but it's actually the tangent to the circle,
      // so we shift by -pi/2 giving:
      dx = r*std::sin(theta + omega) - r*std::sin(theta);
      dy = -r*std::cos(theta + omega) + r*std::cos(theta);
      dtheta = omega;
    }
  }

};

template<class ParticleT> const float CreateMotionModel<ParticleT>::minUpdateDistance = 5.0f;    
template<class ParticleT> const float CreateMotionModel<ParticleT>::minUpdateAngle = 0.05f;    

#endif

#endif