pf-nonlinear-example.cpp

Example of a simple non-linear use-case of the Particle Filter (PF).

The system we are interested in is an aircraft flying in the sky and observed by a radar station. Its velocity is not completely constant: a Gaussian noise is added to the velocity to simulate the effect of wind on the motion of the aircraft.

We consider the plan perpendicular to the ground and passing by both the radar station and the aircraft. The x-axis corresponds to the position on the ground and the y-axis to the altitude.

The state vector of the PF corresponds to a constant velocity model and can be written as:

The measurement corresponds to the distance and angle between the ground and the aircraft observed by the radar station. Be and the position of the radar station along the x and y axis, the measurement vector can be written as:

Some noise is added to the measurement vector to simulate a sensor which is not perfect.

/*
 * 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/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
{
vpColVector fx(const vpColVector &particle, const double &dt)
{
  vpColVector point(4);
  point[0] = particle[1] * dt + particle[0];
  point[1] = particle[1];
  point[2] = particle[3] * dt + particle[2];
  point[3] = particle[3];
  return point;
}
 
void computeCoordinatesFromMeasurement(const vpColVector &z, const double &xRadar, const double &yRadar, double &x, double &y)
{
  double dx = z[0] * std::cos(z[1]);
  double dy = z[0] * std::sin(z[1]);
  x = dx + xRadar;
  y = dy + yRadar;
}
 
double computeError(const double &x0, const double &y0, const double &x1, const double &y1)
{
  double dx = x0 - x1;
  double dy = y0 - y1;
  double error = std::sqrt(dx * dx + dy * dy);
  return error;
}
 
double computeStateError(const vpColVector &state, const vpColVector &gt_X)
{
  double error = computeError(state[0], state[2], gt_X[0], gt_X[1]);
  return error;
}
 
double computeMeasurementsError(const vpColVector &z, const double &xRadar, const double &yRadar, const vpColVector &gt_X)
{
  double xMeas = 0., yMeas = 0.;
  computeCoordinatesFromMeasurement(z, xRadar, yRadar, xMeas, yMeas);
  double error = computeError(xMeas, yMeas, gt_X[0], gt_X[1]);
  return error;
}
}
 
class vpRadarStation
{
public:
  vpRadarStation(const double &x, const double &y, const double &range_std, const double &elev_angle_std,
                 const double &distMaxAllowed)
    : m_x(x)
    , m_y(y)
    , m_rngRange(range_std, 0., 4224)
    , m_rngElevAngle(elev_angle_std, 0., 2112)
  {
    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 meas(2);
    double dx = particle[0] - m_x;
    double dy = particle[2] - m_y;
    meas[0] = std::sqrt(dx * dx + dy * dy);
    meas[1] = std::atan2(dy, dx);
    return meas;
  }
 
  vpColVector measureGT(const vpColVector &pos)
  {
    double dx = pos[0] - m_x;
    double dy = pos[1] - m_y;
    double range = std::sqrt(dx * dx + dy * dy);
    double elevAngle = std::atan2(dy, dx);
    vpColVector measurements(2);
    measurements[0] = range;
    measurements[1] = elevAngle;
    return measurements;
  }
 
  vpColVector measureWithNoise(const vpColVector &pos)
  {
    vpColVector measurementsGT = measureGT(pos);
    vpColVector measurementsNoisy = measurementsGT;
    measurementsNoisy[0] += m_rngRange();
    measurementsNoisy[1] += m_rngElevAngle();
    return measurementsNoisy;
  }
 
  double likelihood(const vpColVector &particle, const vpColVector &meas)
  {
    double xParticle = particle[0];
    double yParticle = particle[2];
    double xMeas = 0., yMeas = 0.;
    computeCoordinatesFromMeasurement(meas, m_x, m_y, xMeas, yMeas);
    double dist = computeError(xParticle, yParticle, xMeas, yMeas);
    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:
  double m_x; // The position on the ground of the radar
  double m_y; // The altitude of the radar
  vpGaussRand m_rngRange; // Noise simulator for the range measurement
  vpGaussRand m_rngElevAngle; // Noise simulator for the elevation angle measurement
  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.
};
 
class vpACSimulator
{
public:
  vpACSimulator(const vpColVector &X0, const vpColVector &vel, const double &vel_std)
    : m_pos(X0)
    , m_vel(vel)
    , m_rngVel(vel_std, 0.)
  {
 
  }
 
  vpColVector update(const double &dt)
  {
    vpColVector dx = m_vel * dt;
    dx[0] += m_rngVel() * dt;
    dx[1] += m_rngVel() * dt;
    m_pos += dx;
    return m_pos;
  }
 
private:
  vpColVector m_pos; // Position of the simulated aircraft
  vpColVector m_vel; // Velocity of the simulated aircraft
  vpGaussRand m_rngVel; // Random generator for slight variations of the velocity of the aircraft
};
 
struct SoftwareArguments
{
  // --- Main loop parameters---
  static const int SOFTWARE_CONTINUE = 42;
  bool m_useDisplay; 
  unsigned int m_nbStepsWarmUp; 
  unsigned int m_nbSteps; 
  double m_dt; // Period, expressed in seconds
  double m_sigmaRange; // Standard deviation of the range measurement, expressed in meters.
  double m_sigmaElevAngle; // Standard deviation of the elevation angle measurent, expressed in radians.
  double m_stdevAircraftVelocity; // Standard deviation of the velocity of the simulated aircraft, to make it deviate a bit from the constant velocity model
  double m_radar_X; // Radar position along the X-axis, in meters
  double m_radar_Y; // Radar position along the Y-axis, in meters
  double m_gt_X_init; // Ground truth initial position along the X-axis, in meters
  double m_gt_Y_init; // Ground truth initial position along the Y-axis, in meters
  double m_gt_vX_init; // Ground truth initial velocity along the X-axis, in meters
  double m_gt_vY_init; // Ground truth initial velocity along the Y-axis, in meters
  // --- PF parameters---
  unsigned int m_N; 
  double m_maxDistanceForLikelihood; 
  double m_ampliMaxX; 
  double m_ampliMaxY; 
  double m_ampliMaxVx; 
  double m_ampliMaxVy; 
  long m_seedPF; 
  int m_nbThreads; 
 
  SoftwareArguments()
    : m_useDisplay(true)
    , m_nbStepsWarmUp(200)
    , m_nbSteps(300)
    , m_dt(3.)
    , m_sigmaRange(5)
    , m_sigmaElevAngle(vpMath::rad(0.5))
    , m_stdevAircraftVelocity(0.2)
    , m_radar_X(0.)
    , m_radar_Y(0.)
    , m_gt_X_init(-500.)
    , m_gt_Y_init(1000.)
    , m_gt_vX_init(10.)
    , m_gt_vY_init(5.)
    , m_N(500)
    , m_maxDistanceForLikelihood(50.)
    , m_ampliMaxX(20.)
    , m_ampliMaxY(200.)
    , m_ampliMaxVx(1.)
    , m_ampliMaxVy(0.5)
    , 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-main") && ((i+1) < argc)) {
        m_nbSteps = std::atoi(argv[i + 1]);
        ++i;
      }
      else if ((arg == "--nb-steps-warmup") && ((i+1) < argc)) {
        m_nbStepsWarmUp = std::atoi(argv[i + 1]);
        ++i;
      }
      else if ((arg == "--dt") && ((i+1) < argc)) {
        m_dt = std::atoi(argv[i + 1]);
        ++i;
      }
      else if ((arg == "--stdev-range") && ((i+1) < argc)) {
        m_sigmaRange = std::atof(argv[i + 1]);
        ++i;
      }
      else if ((arg == "--stdev-elev-angle") && ((i+1) < argc)) {
        m_sigmaElevAngle = vpMath::rad(std::atof(argv[i + 1]));
        ++i;
      }
      else if ((arg == "--stdev-aircraft-vel") && ((i+1) < argc)) {
        m_stdevAircraftVelocity = std::atof(argv[i + 1]);
        ++i;
      }
      else if ((arg == "--radar-X") && ((i+1) < argc)) {
        m_radar_X = std::atof(argv[i + 1]);
        ++i;
      }
      else if ((arg == "--radar-Y") && ((i+1) < argc)) {
        m_radar_Y = std::atof(argv[i + 1]);
        ++i;
      }
      else if ((arg == "--gt-X0") && ((i+1) < argc)) {
        m_gt_X_init = std::atof(argv[i + 1]);
        ++i;
      }
      else if ((arg == "--gt-Y0") && ((i+1) < argc)) {
        m_gt_Y_init = std::atof(argv[i + 1]);
        ++i;
      }
      else if ((arg == "--gt-vX0") && ((i+1) < argc)) {
        m_gt_vX_init = std::atof(argv[i + 1]);
        ++i;
      }
      else if ((arg == "--gt-vY0") && ((i+1) < argc)) {
        m_gt_vY_init = std::atof(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-vX") && ((i+1) < argc)) {
        m_ampliMaxVx = std::atof(argv[i + 1]);
        ++i;
      }
      else if ((arg == "--ampli-max-vY") && ((i+1) < argc)) {
        m_ampliMaxVy = std::atof(argv[i + 1]);
        ++i;
      }
      else if (arg == "-d") {
        m_useDisplay = 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-main <uint>] [--nb-steps-warmup <uint>]" << std::endl;
    std::cout << "  [--dt <double>] [--stdev-range <double>] [--stdev-elev-angle <double>] [--stdev-aircraft-vel <double>]" << std::endl;
    std::cout << "  [--radar-X <double>] [--radar-Y <double>]" << std::endl;
    std::cout << "  [--gt-X0 <double>] [--gt-Y0 <double>] [--gt-vX0 <double>] [--gt-vY0 <double>]" << 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-vX <double>] [--ampli-max-vY <double>]" << std::endl;
    std::cout << "  [-d, --no-display] [-h]" << std::endl;
  }
 
  void printDetails()
  {
    std::cout << std::endl << std::endl;
    std::cout << "DETAILS" << std::endl;
    std::cout << "  --nb-steps-main" << std::endl;
    std::cout << "    Number of steps in the main loop." << std::endl;
    std::cout << "    Default: " << m_nbSteps << std::endl;
    std::cout << 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 << "  --dt" << std::endl;
    std::cout << "    Timestep of the simulation, in seconds." << std::endl;
    std::cout << "    Default: " << m_dt << std::endl;
    std::cout << std::endl;
    std::cout << "  --stdev-range" << std::endl;
    std::cout << "    Standard deviation of the range measurements, in meters." << std::endl;
    std::cout << "    Default: " << m_sigmaRange << std::endl;
    std::cout << std::endl;
    std::cout << "  --stdev-elev-angle" << std::endl;
    std::cout << "    Standard deviation of the elevation angle measurements, in degrees." << std::endl;
    std::cout << "    Default: " << vpMath::deg(m_sigmaElevAngle) << std::endl;
    std::cout << std::endl;
    std::cout << "  --stdev-aircraft-vel" << std::endl;
    std::cout << "    Standard deviation of the aircraft velocity, in m/s." << std::endl;
    std::cout << "    Default: " << m_stdevAircraftVelocity << std::endl;
    std::cout << std::endl;
    std::cout << "  --radar-X" << std::endl;
    std::cout << "    Position along the X-axis of the radar, in meters." << std::endl;
    std::cout << "    Be careful, because singularities happen if the aircraft flies above the radar." << std::endl;
    std::cout << "    Default: " << m_radar_X << std::endl;
    std::cout << std::endl;
    std::cout << "  --radar-Y" << std::endl;
    std::cout << "    Position along the Y-axis of the radar, in meters." << std::endl;
    std::cout << "    Be careful, because singularities happen if the aircraft flies above the radar." << std::endl;
    std::cout << "    Default: " << m_radar_Y << std::endl;
    std::cout << std::endl;
    std::cout << "  --gt-X0" << std::endl;
    std::cout << "    Initial position along the X-axis of the aircraft, in meters." << std::endl;
    std::cout << "    Be careful, because singularities happen if the aircraft flies above the radar." << std::endl;
    std::cout << "    Default: " << m_gt_X_init << std::endl;
    std::cout << std::endl;
    std::cout << "  --gt-Y0" << std::endl;
    std::cout << "    Initial position along the Y-axis of the aircraft, in meters." << std::endl;
    std::cout << "    Be careful, because singularities happen if the aircraft flies above the radar." << std::endl;
    std::cout << "    Default: " << m_gt_Y_init << std::endl;
    std::cout << std::endl;
    std::cout << "  --gt-vX0" << std::endl;
    std::cout << "    Initial velocity along the X-axis of the aircraft, in m/s." << std::endl;
    std::cout << "    Be careful, because singularities happen if the aircraft flies above the radar." << std::endl;
    std::cout << "    Default: " << m_gt_vX_init << std::endl;
    std::cout << std::endl;
    std::cout << "  --gt-vY0" << std::endl;
    std::cout << "    Initial velocity along the Y-axis of the aircraft, in m/s." << std::endl;
    std::cout << "    Be careful, because singularities happen if the aircraft flies above the radar." << std::endl;
    std::cout << "    Default: " << m_gt_vY_init << std::endl;
    std::cout << std::endl;
    std::cout << "  --max-distance-likelihood" << std::endl;
    std::cout << "    Maximum tolerated distance between a particle and 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-vX" << std::endl;
    std::cout << "    Maximum amplitude of the noise added to a particle to the velocity along the X-axis component." << std::endl;
    std::cout << "    Default: " << m_ampliMaxVx << std::endl;
    std::cout << std::endl;
    std::cout << "  --ampli-max-vY" << std::endl;
    std::cout << "    Maximum amplitude of the noise added to a particle to the velocity along the Y-axis component." << std::endl;
    std::cout << "    Default: " << m_ampliMaxVy << 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 << "  -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;
  }
 
  // Initialize the attributes of the PF
  std::vector<double> stdevsPF = { args.m_ampliMaxX /3., args.m_ampliMaxVx /3., args.m_ampliMaxY /3. , args.m_ampliMaxVy /3. };
  int seedPF = args.m_seedPF;
  unsigned int nbParticles = args.m_N;
  int nbThreads = args.m_nbThreads;
 
  vpColVector X0(4);
  X0[0] = 0.9 * args.m_gt_X_init; // x, i.e. 10% of error with regard to ground truth
  X0[1] = 0.9 * args.m_gt_vX_init; // dx/dt, i.e. 10% of error with regard to ground truth
  X0[2] = 0.9 * args.m_gt_Y_init; // y, i.e. 10% of error with regard to ground truth
  X0[3] = 0.9 * args.m_gt_vY_init; // dy/dt, i.e. 10% of error with regard to ground truth
 
  vpParticleFilter<vpColVector>::vpProcessFunction f = fx;
  vpRadarStation radar(args.m_radar_X, args.m_radar_Y, args.m_sigmaRange, args.m_sigmaElevAngle, args.m_maxDistanceForLikelihood);
  using std::placeholders::_1;
  using std::placeholders::_2;
  vpParticleFilter<vpColVector>::vpLikelihoodFunction likelihoodFunc = std::bind(&vpRadarStation::likelihood, &radar, _1, _2);
  vpParticleFilter<vpColVector>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter<vpColVector>::simpleResamplingCheck;
  vpParticleFilter<vpColVector>::vpResamplingFunction resamplingFunc = vpParticleFilter<vpColVector>::simpleImportanceResampling;
 
  // Initialize the PF
  vpParticleFilter<vpColVector> filter(nbParticles, stdevsPF, seedPF, nbThreads);
  filter.init(X0, f, likelihoodFunc, checkResamplingFunc, resamplingFunc);
 
#ifdef VISP_HAVE_DISPLAY
  vpPlot *plot = nullptr;
  if (args.m_useDisplay) {
  // Initialize the plot
    plot = new vpPlot(4);
    plot->initGraph(0, 3);
    plot->setTitle(0, "Position along X-axis");
    plot->setUnitX(0, "Time (s)");
    plot->setUnitY(0, "Position (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);
 
    plot->initGraph(1, 3);
    plot->setTitle(1, "Velocity along X-axis");
    plot->setUnitX(1, "Time (s)");
    plot->setUnitY(1, "Velocity (m/s)");
    plot->setLegend(1, 0, "GT");
    plot->setLegend(1, 1, "Filtered");
    plot->setLegend(1, 2, "Measure");
    plot->setColor(1, 0, vpColor::red);
    plot->setColor(1, 1, vpColor::blue);
    plot->setColor(1, 2, vpColor::black);
 
    plot->initGraph(2, 3);
    plot->setTitle(2, "Position along Y-axis");
    plot->setUnitX(2, "Time (s)");
    plot->setUnitY(2, "Position (m)");
    plot->setLegend(2, 0, "GT");
    plot->setLegend(2, 1, "Filtered");
    plot->setLegend(2, 2, "Measure");
    plot->setColor(2, 0, vpColor::red);
    plot->setColor(2, 1, vpColor::blue);
    plot->setColor(2, 2, vpColor::black);
 
    plot->initGraph(3, 3);
    plot->setTitle(3, "Velocity along Y-axis");
    plot->setUnitX(3, "Time (s)");
    plot->setUnitY(3, "Velocity (m/s)");
    plot->setLegend(3, 0, "GT");
    plot->setLegend(3, 1, "Filtered");
    plot->setLegend(3, 2, "Measure");
    plot->setColor(3, 0, vpColor::red);
    plot->setColor(3, 1, vpColor::blue);
    plot->setColor(3, 2, vpColor::black);
  }
#endif
 
  // Initialize the simulation
  vpColVector ac_pos(2);
  ac_pos[0] = args.m_gt_X_init;
  ac_pos[1] = args.m_gt_Y_init;
  vpColVector ac_vel(2);
  ac_vel[0] = args.m_gt_vX_init;
  ac_vel[1] = args.m_gt_vY_init;
  vpACSimulator ac(ac_pos, ac_vel, args.m_stdevAircraftVelocity);
  vpColVector gt_Xprec = ac_pos;
  vpColVector gt_Vprec = ac_vel;
  double averageFilteringTime = 0.;
  double meanErrorFilter = 0., meanErrorNoise = 0.;
  double xNoise_prec = 0., yNoise_prec = 0.;
 
  // Warmup loop
  const unsigned int nbStepsWarmUp = args.m_nbStepsWarmUp;
  for (unsigned int i = 0; i < nbStepsWarmUp; ++i) {
    // Update object pose
    vpColVector gt_X = ac.update(args.m_dt);
 
    // Perform the measurement
    vpColVector z = radar.measureWithNoise(gt_X);
 
    // Use the UKF to filter the measurement
    double t0 = vpTime::measureTimeMicros();
    filter.filter(z, args.m_dt);
    averageFilteringTime += vpTime::measureTimeMicros() - t0;
    gt_Xprec = gt_X;
 
    // Save the noisy position
    computeCoordinatesFromMeasurement(z, args.m_radar_X, args.m_radar_Y, xNoise_prec, yNoise_prec);
  }
 
  for (unsigned int i = 0; i < args.m_nbSteps; ++i) {
    // Perform the measurement
    vpColVector gt_X = ac.update(args.m_dt);
    vpColVector gt_V = (gt_X - gt_Xprec) / args.m_dt;
    vpColVector z = radar.measureWithNoise(gt_X);
 
    // Use the PF to filter the measurement
    double t0 = vpTime::measureTimeMicros();
    filter.filter(z, args.m_dt);
    averageFilteringTime += vpTime::measureTimeMicros() - t0;
 
    // Compute the error between GT and filtered state for statistics at the end of the program
    vpColVector Xest = filter.computeFilteredState();
    vpColVector gtState = vpColVector({ gt_Xprec[0], gt_Vprec[0], gt_Xprec[1], gt_Vprec[1] });
    double normErrorFilter = computeStateError(Xest, gt_X);
    meanErrorFilter += normErrorFilter;
 
    // Compute the error between GT and noisy measurements for statistics at the end of the program
    double xNoise = 0., yNoise = 0.;
    computeCoordinatesFromMeasurement(z, args.m_radar_X, args.m_radar_Y, xNoise, yNoise);
    double normErrorNoise = computeMeasurementsError(z, args.m_radar_X, args.m_radar_Y, gt_X);
    meanErrorNoise += normErrorNoise;
 
#ifdef VISP_HAVE_DISPLAY
    if (args.m_useDisplay) {
    // Plot the ground truth, measurement and filtered state
      plot->plot(0, 0, i, gt_X[0]);
      plot->plot(0, 1, i, Xest[0]);
      plot->plot(0, 2, i, xNoise);
 
      double vxNoise = (xNoise - xNoise_prec) / args.m_dt;
      plot->plot(1, 0, i, gt_V[0]);
      plot->plot(1, 1, i, Xest[1]);
      plot->plot(1, 2, i, vxNoise);
 
      plot->plot(2, 0, i, gt_X[1]);
      plot->plot(2, 1, i, Xest[2]);
      plot->plot(2, 2, i, yNoise);
 
      double vyNoise = (yNoise - yNoise_prec) / args.m_dt;
      plot->plot(3, 0, i, gt_V[1]);
      plot->plot(3, 1, i, Xest[3]);
      plot->plot(3, 2, i, vyNoise);
    }
#endif
 
    gt_Xprec = gt_X;
    gt_Vprec = gt_V;
    xNoise_prec = xNoise;
    yNoise_prec = yNoise;
  }
 
  // COmpute and display the error statistics and computation time
  meanErrorFilter /= static_cast<double>(args.m_nbSteps);
  meanErrorNoise /= static_cast<double>(args.m_nbSteps);
  averageFilteringTime = averageFilteringTime / (static_cast<double>(args.m_nbSteps) + static_cast<double>(nbStepsWarmUp));
  std::cout << "Mean error filter = " << meanErrorFilter << "m" << std::endl;
  std::cout << "Mean error noise = " << meanErrorNoise << "m" << std::endl;
  std::cout << "Mean filtering time = " << averageFilteringTime << "us" << std::endl;
 
  if (args.m_useDisplay) {
    std::cout << "Press Enter to quit..." << std::endl;
    std::cin.get();
  }
 
#ifdef VISP_HAVE_DISPLAY
  if (args.m_useDisplay) {
    delete plot;
  }
#endif
 
  // For the unit tests that uses this program
  const double maxError = 150.;
  if (meanErrorFilter > maxError) {
    std::cerr << "Error: max tolerated error = " << maxError << ", mean error = " << meanErrorFilter << std::endl;
    return -1;
  }
  else if (meanErrorFilter >= meanErrorNoise) {
    std::cerr << "Error: mean error without filter = " << meanErrorNoise << ", mean error with filter = " << 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