pf-nonlinear-complex-example.cpp

Example of a complex non-linear use-case of the Particle Filter (PF). The system we are interested in is a 4-wheel robot, moving at a low velocity. As such, it can be modeled using a bicycle model.

The state vector of the PF is:

where is the heading of the robot.

The measurement corresponds to the distance and relative orientation of the robot with different landmarks. Be and the position of the landmark along the x and y axis, the measurement vector can be written as:

Some noise is added to the measurement vector to simulate measurements which are not perfect.

The mean of several angles must be computed in the Particle Fitler inference. The definition we chose to use is the following:

As the Particle Filter inference uses a weighted mean, the actual implementation of the weighted mean of several angles is the following:

where is the weight associated to the measurements for the weighted mean.

Additionally, the addition and subtraction of angles must be carefully done, as the result must stay in the interval or . We decided to use the interval .

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See https://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */

 
#include <iostream>
 
// ViSP includes
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpGaussRand.h>
#ifdef VISP_HAVE_DISPLAY
#include <visp3/gui/vpPlot.h>
#endif
 
// PF includes
#include <visp3/core/vpParticleFilter.h>
 
 
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
 
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
 
namespace
{
double normalizeAngle(const double &angle)
{
  double angleIn0to2pi = vpMath::modulo(angle, 2. * M_PI);
  double angleInMinPiPi = angleIn0to2pi;
  if (angleInMinPiPi > M_PI) {
    // Substract 2 PI to be in interval [-Pi; Pi]
    angleInMinPiPi -= 2. * M_PI;
  }
  return angleInMinPiPi;
}

vpColVector stateAdd(const vpColVector &state, const vpColVector &toAdd)
{
  vpColVector add = state + toAdd;
  add[2] = normalizeAngle(add[2]);
  return add;
}

vpColVector stateMean(const std::vector<vpColVector> &states, const std::vector<double> &wm, const vpParticleFilter<vpColVector>::vpStateAddFunction &/*addFunc*/)
{
  vpColVector mean(3, 0.);
  unsigned int nbPoints = static_cast<unsigned int>(states.size());
  double sumCos = 0.;
  double sumSin = 0.;
  for (unsigned int i = 0; i < nbPoints; ++i) {
    mean[0] += wm[i] * states[i][0];
    mean[1] += wm[i] * states[i][1];
    sumCos += wm[i] * std::cos(states[i][2]);
    sumSin += wm[i] * std::sin(states[i][2]);
  }
  mean[2] = std::atan2(sumSin, sumCos);
  return mean;
}

std::vector<vpColVector> generateTurnCommands(const double &v, const double &angleStart, const double &angleStop, const unsigned int &nbSteps)
{
  std::vector<vpColVector> cmds;
  double dTheta = vpMath::rad(angleStop - angleStart) / static_cast<double>(nbSteps - 1);
  for (unsigned int i = 0; i < nbSteps; ++i) {
    double theta = vpMath::rad(angleStart) + dTheta * static_cast<double>(i);
    vpColVector cmd(2);
    cmd[0] = v;
    cmd[1] = theta;
    cmds.push_back(cmd);
  }
  return cmds;
}

std::vector<vpColVector> generateCommands()
{
  std::vector<vpColVector> cmds;
  // Starting by an straight line acceleration
  unsigned int nbSteps = 30;
  double dv = (1.1 - 0.001) / static_cast<double>(nbSteps - 1);
  for (unsigned int i = 0; i < nbSteps; ++i) {
    vpColVector cmd(2);
    cmd[0] = 0.001 + static_cast<double>(i) * dv;
    cmd[1] = 0.;
    cmds.push_back(cmd);
  }
 
  // Left turn
  double lastLinearVelocity = cmds[cmds.size() -1][0];
  std::vector<vpColVector> leftTurnCmds = generateTurnCommands(lastLinearVelocity, 0, 2, 15);
  cmds.insert(cmds.end(), leftTurnCmds.begin(), leftTurnCmds.end());
  for (unsigned int i = 0; i < 100; ++i) {
    cmds.push_back(cmds[cmds.size() -1]);
  }
 
  // Right turn
  lastLinearVelocity = cmds[cmds.size() -1][0];
  std::vector<vpColVector> rightTurnCmds = generateTurnCommands(lastLinearVelocity, 2, -2, 15);
  cmds.insert(cmds.end(), rightTurnCmds.begin(), rightTurnCmds.end());
  for (unsigned int i = 0; i < 200; ++i) {
    cmds.push_back(cmds[cmds.size() -1]);
  }
 
  // Left  turn again
  lastLinearVelocity = cmds[cmds.size() -1][0];
  leftTurnCmds = generateTurnCommands(lastLinearVelocity, -2, 0, 15);
  cmds.insert(cmds.end(), leftTurnCmds.begin(), leftTurnCmds.end());
  for (unsigned int i = 0; i < 150; ++i) {
    cmds.push_back(cmds[cmds.size() -1]);
  }
 
  lastLinearVelocity = cmds[cmds.size() -1][0];
  leftTurnCmds = generateTurnCommands(lastLinearVelocity, 0, 1, 25);
  cmds.insert(cmds.end(), leftTurnCmds.begin(), leftTurnCmds.end());
  for (unsigned int i = 0; i < 150; ++i) {
    cmds.push_back(cmds[cmds.size() -1]);
  }
  return cmds;
}
}

vpColVector computeMotionFromCommand(const vpColVector &u, const vpColVector &x, const double &dt, const double &w)
{
  double heading = x[2];
  double vel = u[0];
  double steeringAngle = u[1];
  double distance = vel * dt;
 
  if (std::abs(steeringAngle) > 0.001) {
    // The robot is turning
    double beta = (distance / w) * std::tan(steeringAngle);
    double radius = w / std::tan(steeringAngle);
    double sinh = std::sin(heading);
    double sinhb = std::sin(heading + beta);
    double cosh = std::cos(heading);
    double coshb = std::cos(heading + beta);
    vpColVector motion(3);
    motion[0] = -radius * sinh + radius * sinhb;
    motion[1] = radius * cosh - radius * coshb;
    motion[2] = beta;
    return motion;
  }
  else {
    // The robot is moving in straight line
    vpColVector motion(3);
    motion[0] = distance * std::cos(heading);
    motion[1] = distance * std::sin(heading);
    motion[2] = 0.;
    return motion;
  }
}

class vpProcessFunctor
{
public:
  vpProcessFunctor(const double &w)
    : m_w(w)
  { }

  vpColVector processFunction(const vpColVector &x, const double &dt)
  {
    vpColVector motion = computeMotionFromCommand(m_u, x, dt, m_w);
    vpColVector newState = x + motion;
    newState[2] = normalizeAngle(newState[2]);
    return newState;
  }

  void setCommands(const vpColVector &u)
  {
    m_u = u;
  }
private:
  double m_w; 
  vpColVector m_u; 
};

class vpBicycleModel
{
public:
  vpBicycleModel(const double &w)
    : m_w(w)
  { }

  vpColVector computeMotion(const vpColVector &u, const vpColVector &x, const double &dt)
  {
    return computeMotionFromCommand(u, x, dt, m_w);
  }

  vpColVector move(const vpColVector &u, const vpColVector &x, const double &dt)
  {
    vpColVector motion = computeMotion(u, x, dt);
    vpColVector newX = x + motion;
    newX[2] = normalizeAngle(newX[2]);
    return newX;
  }
private:
  double m_w; // The length of the wheelbase.
};

class vpLandmarkMeasurements
{
public:
  vpLandmarkMeasurements(const double &x, const double &y, const double &range_std, const double &rel_angle_std)
    : m_x(x)
    , m_y(y)
    , m_rngRange(range_std, 0., 4224)
    , m_rngRelativeAngle(rel_angle_std, 0., 2112)
  { }

  vpColVector state_to_measurement(const vpColVector &particle)
  {
    vpColVector meas(2);
    double dx = m_x - particle[0];
    double dy = m_y - particle[1];
    meas[0] = std::sqrt(dx * dx + dy * dy);
    meas[1] = normalizeAngle(std::atan2(dy, dx));
    return meas;
  }

  vpColVector measureGT(const vpColVector &pos)
  {
    double dx = m_x - pos[0];
    double dy = m_y - pos[1];
    double range = std::sqrt(dx * dx + dy * dy);
    double orientation = normalizeAngle(std::atan2(dy, dx));
    vpColVector measurements(2);
    measurements[0] = range;
    measurements[1] = orientation;
    return measurements;
  }

  vpColVector measureWithNoise(const vpColVector &pos)
  {
    vpColVector measurementsGT = measureGT(pos);
    vpColVector measurementsNoisy = measurementsGT;
    measurementsNoisy[0] += m_rngRange();
    measurementsNoisy[1] += m_rngRelativeAngle();
    measurementsNoisy[1] = normalizeAngle(measurementsNoisy[1]);
    return measurementsNoisy;
  }

  void computePositionFromMeasurements(const vpColVector &meas, double &x, double &y)
  {
    double alpha = meas[1];
    x = m_x - meas[0] * std::cos(alpha);
    y = m_y - meas[0] * std::sin(alpha);
  }
 
private:
  double m_x; 
  double m_y; 
  vpGaussRand m_rngRange; 
  vpGaussRand m_rngRelativeAngle; 
};

class vpLandmarksGrid
{
public:
  vpLandmarksGrid(const std::vector<vpLandmarkMeasurements> &landmarks, const double &distMaxAllowed)
    : m_landmarks(landmarks)
    , m_nbLandmarks(static_cast<unsigned int>(landmarks.size()))
  {
    double sigmaDistance = distMaxAllowed / 3.;
    double sigmaDistanceSquared = sigmaDistance * sigmaDistance;
    m_constantDenominator = 1. / std::sqrt(2. * M_PI * sigmaDistanceSquared);
    m_constantExpDenominator = -1. / (2. * sigmaDistanceSquared);
  }

  vpColVector state_to_measurement(const vpColVector &particle)
  {
    vpColVector measurements(2*m_nbLandmarks);
    for (unsigned int i = 0; i < m_nbLandmarks; ++i) {
      vpColVector landmarkMeas = m_landmarks[i].state_to_measurement(particle);
      measurements[2*i] = landmarkMeas[0];
      measurements[(2*i) + 1] = landmarkMeas[1];
    }
    return measurements;
  }

  vpColVector measureGT(const vpColVector &pos)
  {
    vpColVector measurements(2*m_nbLandmarks);
    for (unsigned int i = 0; i < m_nbLandmarks; ++i) {
      vpColVector landmarkMeas = m_landmarks[i].measureGT(pos);
      measurements[2*i] = landmarkMeas[0];
      measurements[(2*i) + 1] = landmarkMeas[1];
    }
    return measurements;
  }

  vpColVector measureWithNoise(const vpColVector &pos)
  {
    vpColVector measurements(2*m_nbLandmarks);
    for (unsigned int i = 0; i < m_nbLandmarks; ++i) {
      vpColVector landmarkMeas = m_landmarks[i].measureWithNoise(pos);
      measurements[2*i] = landmarkMeas[0];
      measurements[(2*i) + 1] = landmarkMeas[1];
    }
    return measurements;
  }

  void computePositionFromMeasurements(const vpColVector &meas, double &x, double &y)
  {
    x = 0.;
    y = 0.;
    for (unsigned int i = 0; i < m_nbLandmarks; ++i) {
      vpColVector landmarkMeas({ meas[2*i], meas[(2*i) + 1] });
      double xLand = 0., yLand = 0.;
      m_landmarks[i].computePositionFromMeasurements(landmarkMeas, xLand, yLand);
      x += xLand;
      y += yLand;
    }
    x /= static_cast<double>(m_nbLandmarks);
    y /= static_cast<double>(m_nbLandmarks);
  }

  double likelihood(const vpColVector &particle, const vpColVector &meas)
  {
    double meanX = 0., meanY = 0.;
    computePositionFromMeasurements(meas, meanX, meanY);
    double dx = meanX - particle[0];
    double dy = meanY - particle[1];
    double dist = std::sqrt(dx * dx + dy * dy);
    double likelihood = std::exp(m_constantExpDenominator * dist) * m_constantDenominator;
    likelihood = std::min(likelihood, 1.0); // Clamp to have likelihood <= 1.
    likelihood = std::max(likelihood, 0.); // Clamp to have likelihood >= 0.
    return likelihood;
  }
 
private:
  std::vector<vpLandmarkMeasurements> m_landmarks; 
  const unsigned int m_nbLandmarks; 
  double m_constantDenominator; // Denominator of the Gaussian function used in the likelihood computation.
  double m_constantExpDenominator; // Denominator of the exponential in the Gaussian function used in the likelihood computation.
};
 
struct SoftwareArguments
{
  // --- Main loop parameters---
  static const int SOFTWARE_CONTINUE = 42;
  bool m_useDisplay; 
  bool m_useUserInteraction; 
  unsigned int m_nbStepsWarmUp; 
  // --- PF parameters---
  unsigned int m_N; 
  double m_maxDistanceForLikelihood; 
  double m_ampliMaxX; 
  double m_ampliMaxY; 
  double m_ampliMaxTheta; 
  long m_seedPF; 
  int m_nbThreads; 
 
  SoftwareArguments()
    : m_useDisplay(true)
    , m_useUserInteraction(true)
    , m_nbStepsWarmUp(200)
    , m_N(500)
    , m_maxDistanceForLikelihood(0.5)
    , m_ampliMaxX(0.25)
    , m_ampliMaxY(0.25)
    , m_ampliMaxTheta(0.1)
    , m_seedPF(4224)
    , m_nbThreads(1)
  { }
 
  int parseArgs(const int argc, const char *argv[])
  {
    int i = 1;
    while (i < argc) {
      std::string arg(argv[i]);
      if ((arg == "--nb-steps-warmup") && ((i+1) < argc)) {
        m_nbStepsWarmUp = std::atoi(argv[i + 1]);
        ++i;
      }
      else if ((arg == "--max-distance-likelihood") && ((i+1) < argc)) {
        m_maxDistanceForLikelihood = std::atof(argv[i + 1]);
        ++i;
      }
      else if (((arg == "-N") || (arg == "--nb-particles")) && ((i+1) < argc)) {
        m_N = std::atoi(argv[i + 1]);
        ++i;
      }
      else if ((arg == "--seed") && ((i+1) < argc)) {
        m_seedPF = std::atoi(argv[i + 1]);
        ++i;
      }
      else if ((arg == "--nb-threads") && ((i+1) < argc)) {
        m_nbThreads = std::atoi(argv[i + 1]);
        ++i;
      }
      else if ((arg == "--ampli-max-X") && ((i+1) < argc)) {
        m_ampliMaxX = std::atof(argv[i + 1]);
        ++i;
      }
      else if ((arg == "--ampli-max-Y") && ((i+1) < argc)) {
        m_ampliMaxY = std::atof(argv[i + 1]);
        ++i;
      }
      else if ((arg == "--ampli-max-theta") && ((i+1) < argc)) {
        m_ampliMaxTheta = std::atof(argv[i + 1]);
        ++i;
      }
      else if (arg == "-d") {
        m_useDisplay = false;
      }
      else if (arg == "-c" ) {
        m_useUserInteraction = false;
      }
      else if ((arg == "-h") || (arg == "--help")) {
        printUsage(std::string(argv[0]));
        SoftwareArguments defaultArgs;
        defaultArgs.printDetails();
        return 0;
      }
      else {
        std::cout << "WARNING: unrecognised argument \"" << arg << "\"";
        if (i + 1 < argc) {
          std::cout << " with associated value(s) { ";
          int nbValues = 0;
          int j = i + 1;
          bool hasToRun = true;
          while ((j < argc) && hasToRun) {
            std::string nextValue(argv[j]);
            if (nextValue.find("--") == std::string::npos) {
              std::cout << nextValue << " ";
              ++nbValues;
            }
            else {
              hasToRun = false;
            }
            ++j;
          }
          std::cout << "}" << std::endl;
          i += nbValues;
        }
      }
      ++i;
    }
    return SOFTWARE_CONTINUE;
  }
 
private:
  void printUsage(const std::string &softName)
  {
    std::cout << "SYNOPSIS" << std::endl;
    std::cout << "  " << softName << " [--nb-steps-warmup <uint>]" << std::endl;
    std::cout << "  [--max-distance-likelihood <double>] [-N, --nb-particles <uint>] [--seed <int>] [--nb-threads <int>]" << std::endl;
    std::cout << "  [--ampli-max-X <double>] [--ampli-max-Y <double>] [--ampli-max-theta <double>]" << std::endl;
    std::cout << "  [-d, --no-display] [-h]" << std::endl;
    std::cout << "  [-c] [-h]" << std::endl;
  }
 
  void printDetails()
  {
    std::cout << std::endl << std::endl;
    std::cout << "DETAILS" << std::endl;
    std::cout << "  --nb-steps-warmup" << std::endl;
    std::cout << "    Number of steps in the warmup loop." << std::endl;
    std::cout << "    Default: " << m_nbStepsWarmUp << std::endl;
    std::cout << std::endl;
    std::cout << "  --max-distance-likelihood" << std::endl;
    std::cout << "    Maximum distance between a particle and the average position computed from the measurements." << std::endl;
    std::cout << "    Above this value, the likelihood of the particle is 0." << std::endl;
    std::cout << "    Default: " << m_maxDistanceForLikelihood << std::endl;
    std::cout << std::endl;
    std::cout << "  -N, --nb-particles" << std::endl;
    std::cout << "    Number of particles of the Particle Filter." << std::endl;
    std::cout << "    Default: " << m_N << std::endl;
    std::cout << std::endl;
    std::cout << "  --seed" << std::endl;
    std::cout << "    Seed to initialize the Particle Filter." << std::endl;
    std::cout << "    Use a negative value makes to use the current timestamp instead." << std::endl;
    std::cout << "    Default: " << m_seedPF << std::endl;
    std::cout << std::endl;
    std::cout << "  --nb-threads" << std::endl;
    std::cout << "    Set the number of threads to use in the Particle Filter (only if OpenMP is available)." << std::endl;
    std::cout << "    Use a negative value to use the maximum number of threads instead." << std::endl;
    std::cout << "    Default: " << m_nbThreads << std::endl;
    std::cout << std::endl;
    std::cout << "  --ampli-max-X" << std::endl;
    std::cout << "    Maximum amplitude of the noise added to a particle along the X-axis." << std::endl;
    std::cout << "    Default: " << m_ampliMaxX << std::endl;
    std::cout << std::endl;
    std::cout << "  --ampli-max-Y" << std::endl;
    std::cout << "    Maximum amplitude of the noise added to a particle along the Y-axis." << std::endl;
    std::cout << "    Default: " << m_ampliMaxY << std::endl;
    std::cout << std::endl;
    std::cout << "  --ampli-max-theta" << std::endl;
    std::cout << "    Maximum amplitude of the noise added to a particle affecting the heading of the robot." << std::endl;
    std::cout << "    Default: " << m_ampliMaxTheta << std::endl;
    std::cout << std::endl;
    std::cout << "  -d, --no-display" << std::endl;
    std::cout << "    Deactivate display." << std::endl;
    std::cout << "    Default: display is ";
#ifdef VISP_HAVE_DISPLAY
    std::cout << "ON" << std::endl;
#else
    std::cout << "OFF" << std::endl;
#endif
    std::cout << std::endl;
    std::cout << "  -c" << std::endl;
    std::cout << "    Deactivate user interaction." << std::endl;
    std::cout << "    Default: user interaction enabled: " << m_useUserInteraction << std::endl;
    std::cout << std::endl;
    std::cout << "  -h, --help" << std::endl;
    std::cout << "    Display this help." << std::endl;
    std::cout << std::endl;
  }
};
 
int main(const int argc, const char *argv[])
{
  SoftwareArguments args;
  int returnCode = args.parseArgs(argc, argv);
  if (returnCode != SoftwareArguments::SOFTWARE_CONTINUE) {
    return returnCode;
  }
 
  const double dt = 0.1; // Period of 0.1s
  const double step = 1.; // Number of update of the robot position between two PF filtering
  const double sigmaRange = 0.3; // Standard deviation of the range measurement: 0.3m
  const double sigmaBearing = vpMath::rad(0.5); // Standard deviation of the bearing angle: 0.5deg
  const double wheelbase = 0.5; // Wheelbase of 0.5m
  const std::vector<vpLandmarkMeasurements> landmarks = { vpLandmarkMeasurements(5, 10, sigmaRange, sigmaBearing)
                                                        , vpLandmarkMeasurements(10, 5, sigmaRange, sigmaBearing)
                                                        , vpLandmarkMeasurements(15, 15, sigmaRange, sigmaBearing)
                                                        , vpLandmarkMeasurements(20, 5, sigmaRange, sigmaBearing)
                                                        , vpLandmarkMeasurements(0, 30, sigmaRange, sigmaBearing)
                                                        , vpLandmarkMeasurements(50, 30, sigmaRange, sigmaBearing)
                                                        , vpLandmarkMeasurements(40, 10, sigmaRange, sigmaBearing) }; // Vector of landmarks constituting the grid
  std::vector<vpColVector> cmds = generateCommands();
  const unsigned int nbCmds = static_cast<unsigned int>(cmds.size());
 
  // Initialize the attributes of the PF
  std::vector<double> stdevsPF = { args.m_ampliMaxX / 3., args.m_ampliMaxY / 3., args.m_ampliMaxTheta / 3. }; 
  int seedPF = args.m_seedPF;
  unsigned int nbParticles = args.m_N;
  int nbThreads = args.m_nbThreads;
 
  vpColVector X0(3);
  X0[0] = 2.; // x = 2m
  X0[1] = 6.; // y = 6m
  X0[2] = 0.3; // robot orientation = 0.3 rad
 
  vpProcessFunctor processFtor(wheelbase);
  vpLandmarksGrid grid(landmarks, args.m_maxDistanceForLikelihood);
  vpBicycleModel robot(wheelbase);
  using std::placeholders::_1;
  using std::placeholders::_2;
  vpParticleFilter<vpColVector>::vpProcessFunction f = std::bind(&vpProcessFunctor::processFunction, &processFtor, _1, _2);
  vpParticleFilter<vpColVector>::vpLikelihoodFunction likelihoodFunc = std::bind(&vpLandmarksGrid::likelihood, &grid, _1, _2);
  vpParticleFilter<vpColVector>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter<vpColVector>::simpleResamplingCheck;
  vpParticleFilter<vpColVector>::vpResamplingFunction resamplingFunc = vpParticleFilter<vpColVector>::simpleImportanceResampling;
  vpParticleFilter<vpColVector>::vpFilterFunction weightedMeanFunc = stateMean;
  vpParticleFilter<vpColVector>::vpStateAddFunction addFunc = stateAdd;
 
  // Initialize the PF
  vpParticleFilter<vpColVector> filter(nbParticles, stdevsPF, seedPF, nbThreads);
  filter.init(X0, f, likelihoodFunc, checkResamplingFunc, resamplingFunc, weightedMeanFunc, addFunc);
 
#ifdef VISP_HAVE_DISPLAY
  vpPlot *plot = nullptr;
  if (args.m_useDisplay) {
  // Initialize the plot
    plot = new vpPlot(1);
    plot->initGraph(0, 3);
    plot->setTitle(0, "Position of the robot");
    plot->setUnitX(0, "Position along x(m)");
    plot->setUnitY(0, "Position along y (m)");
    plot->setLegend(0, 0, "GT");
    plot->setLegend(0, 1, "Filtered");
    plot->setLegend(0, 2, "Measure");
    plot->setColor(0, 0, vpColor::red);
    plot->setColor(0, 1, vpColor::blue);
    plot->setColor(0, 2, vpColor::black);
  }
#endif
 
  // Initialize the simulation
  vpColVector robot_pos = X0;
  vpColVector noMotionCommand(2, 0.);
 
  // Warm-up step
  double averageFilteringTime = 0.;
  for (unsigned int i = 0; i < args.m_nbStepsWarmUp; ++i) {
     // Perform the measurement
    vpColVector z = grid.measureWithNoise(robot_pos);
 
    double t0 = vpTime::measureTimeMicros();
    // Update the functor command
    processFtor.setCommands(noMotionCommand);
    // Use the PF to filter the measurement
    filter.filter(z, dt);
    averageFilteringTime += vpTime::measureTimeMicros() - t0;
  }
 
  double meanErrorFilter = 0., meanErrorNoise = 0.;
  for (unsigned int i = 0; i < nbCmds; ++i) {
    robot_pos = robot.move(cmds[i], robot_pos, dt / step);
    if (i % static_cast<int>(step) == 0) {
      // Perform the measurement
      vpColVector z = grid.measureWithNoise(robot_pos);
 
      double t0 = vpTime::measureTimeMicros();
      // Update the functor command
      processFtor.setCommands(cmds[i]);
        // Use the PF to filter the measurement
      filter.filter(z, dt);
      averageFilteringTime += vpTime::measureTimeMicros() - t0;

      vpColVector Xest = filter.computeFilteredState();

      // Compute the error between the filtered state and the Ground Truth
      // to have statistics at the end of the program
      double dxFilter = Xest[0] - robot_pos[0];
      double dyFilter = Xest[1] - robot_pos[1];
      double errorFilter = std::sqrt(dxFilter * dxFilter + dyFilter * dyFilter);
      meanErrorFilter += errorFilter;
 
      // Compute the error between the noisy measurements and the Ground Truth
      // to have statistics at the end of the program
      double xMeas = 0., yMeas = 0.;
      grid.computePositionFromMeasurements(z, xMeas, yMeas);
      double dxMeas = xMeas - robot_pos[0];
      double dyMeas = yMeas - robot_pos[1];
      meanErrorNoise += std::sqrt(dxMeas * dxMeas + dyMeas * dyMeas);
 
#ifdef VISP_HAVE_DISPLAY
      if (args.m_useDisplay) {
        // Plot the filtered state
        plot->plot(0, 1, Xest[0], Xest[1]);
        plot->plot(0, 2, xMeas, yMeas);
      }
#endif
    }
 
#ifdef VISP_HAVE_DISPLAY
    if (args.m_useDisplay) {
    // Plot the ground truth
      plot->plot(0, 0, robot_pos[0], robot_pos[1]);
    }
#endif
  }
 
  // Display the statistics that were computed
  averageFilteringTime = averageFilteringTime / (static_cast<double>(nbCmds + args.m_nbStepsWarmUp));
  meanErrorFilter = meanErrorFilter / (static_cast<double>(nbCmds));
  meanErrorNoise = meanErrorNoise / (static_cast<double>(nbCmds));
  std::cout << "Mean error filter = " << meanErrorFilter << std::endl;
  std::cout << "Mean error noise = " << meanErrorNoise << std::endl;
  std::cout << "Mean filtering time = " << averageFilteringTime << "us" << std::endl;
 
  if (args.m_useUserInteraction) {
    std::cout << "Press Enter to quit..." << std::endl;
    std::cin.get();
  }
 
#ifdef VISP_HAVE_DISPLAY
  if (args.m_useDisplay) {
    delete plot;
  }
#endif
 
  // Check if the results are the one expected, when this program is used for the unit tests
  const double maxError = 0.15;
  if (meanErrorFilter > meanErrorNoise) {
    std::cerr << "Error: noisy measurements error = " << meanErrorNoise << ", filter error = " << meanErrorFilter << std::endl;
    return -1;
  }
  else if (meanErrorFilter > maxError) {
    std::cerr << "Error: max tolerated error = " << maxError << ", average error = " << meanErrorFilter << std::endl;
    return -1;
  }
  return 0;
}
#else
int main()
{
  std::cout << "This example is only available if you compile ViSP in C++11 standard or higher." << std::endl;
  return 0;
}
#endif