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.
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:
Additionally, the addition and subtraction of angles must be carefully done, as the result must stay in the interval ![$[- \pi ; \pi ]$](form_1713.png)
or ![$[0 ; 2 \pi ]$](form_1714.png)
. We decided to use the interval .
#include <iostream>
#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
#include <visp3/core/vpParticleFilter.h>
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
#ifdef ENABLE_VISP_NAMESPACE
#endif
namespace
{
double normalizeAngle(const double &angle)
{
double angleInMinPiPi = angleIn0to2pi;
if (angleInMinPiPi > M_PI) {
angleInMinPiPi -= 2. * M_PI;
}
return angleInMinPiPi;
}
{
add[2] = normalizeAngle(add[2]);
return add;
}
{
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);
cmd[0] = v;
cmd[1] = theta;
cmds.push_back(cmd);
}
return cmds;
}
std::vector<vpColVector> generateCommands()
{
std::vector<vpColVector> cmds;
unsigned int nbSteps = 30;
double dv = (1.1 - 0.001) / static_cast<double>(nbSteps - 1);
for (unsigned int i = 0; i < nbSteps; ++i) {
cmd[0] = 0.001 + static_cast<double>(i) * dv;
cmd[1] = 0.;
cmds.push_back(cmd);
}
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]);
}
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]);
}
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;
}
}
{
double heading = x[2];
double vel = u[0];
double steeringAngle = u[1];
double distance = vel * dt;
if (std::abs(steeringAngle) > 0.001) {
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);
motion[0] = -radius * sinh + radius * sinhb;
motion[1] = radius * cosh - radius * coshb;
motion[2] = beta;
return motion;
}
else {
motion[0] = distance * std::cos(heading);
motion[1] = distance * std::sin(heading);
motion[2] = 0.;
return motion;
}
}
{
public:
: m_w(w)
{ }
{
vpColVector motion = computeMotionFromCommand(m_u, x, dt, m_w);
newState[2] = normalizeAngle(newState[2]);
return newState;
}
{
m_u = u;
}
private:
double m_w;
};
{
public:
: m_w(w)
{ }
{
return computeMotionFromCommand(u, x, dt, m_w);
}
{
newX[2] = normalizeAngle(newX[2]);
return newX;
}
private:
double m_w;
};
{
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)
{ }
{
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;
}
{
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));
measurements[0] = range;
measurements[1] = orientation;
return measurements;
}
{
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;
};
{
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);
}
{
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;
}
{
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;
}
{
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 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);
likelihood = std::max(likelihood, 0.);
return likelihood;
}
private:
std::vector<vpLandmarkMeasurements> m_landmarks;
const unsigned int m_nbLandmarks;
double m_constantDenominator;
double m_constantExpDenominator;
};
{
static const int SOFTWARE_CONTINUE = 42;
bool m_useDisplay;
unsigned int m_nbStepsWarmUp;
unsigned int m_N;
double m_maxDistanceForLikelihood;
double m_ampliMaxX;
double m_ampliMaxY;
double m_ampliMaxTheta;
long m_seedPF;
int m_nbThreads;
: m_useDisplay(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 == "-h") || (arg == "--help")) {
printUsage(std::string(argv[0]));
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;
}
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 << " -h, --help" << std::endl;
std::cout << " Display this help." << std::endl;
std::cout << std::endl;
}
};
int main(const int argc, const char *argv[])
{
return returnCode;
}
const double dt = 0.1;
const double step = 1.;
const double sigmaRange = 0.3;
const double wheelbase = 0.5;
const std::vector<vpLandmarkMeasurements> landmarks = {
vpLandmarkMeasurements(5, 10, sigmaRange, sigmaBearing)
std::vector<vpColVector> cmds = generateCommands();
const unsigned int nbCmds = static_cast<unsigned int>(cmds.size());
unsigned int nbParticles = args.
m_N;
X0[0] = 2.;
X0[1] = 6.;
X0[2] = 0.3;
using std::placeholders::_1;
using std::placeholders::_2;
filter.init(X0, f, likelihoodFunc, checkResamplingFunc, resamplingFunc, weightedMeanFunc, addFunc);
#ifdef VISP_HAVE_DISPLAY
plot->
setTitle(0,
"Position of the robot");
plot->
setUnitX(0,
"Position along x(m)");
plot->
setUnitY(0,
"Position along y (m)");
}
#endif
double averageFilteringTime = 0.;
processFtor.setCommands(noMotionCommand);
filter.filter(z, dt);
}
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) {
processFtor.setCommands(cmds[i]);
filter.filter(z, dt);
double dxFilter = Xest[0] - robot_pos[0];
double dyFilter = Xest[1] - robot_pos[1];
double errorFilter = std::sqrt(dxFilter * dxFilter + dyFilter * dyFilter);
meanErrorFilter += errorFilter;
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
plot->
plot(0, 1, Xest[0], Xest[1]);
plot->
plot(0, 2, xMeas, yMeas);
}
#endif
}
#ifdef VISP_HAVE_DISPLAY
plot->
plot(0, 0, robot_pos[0], robot_pos[1]);
}
#endif
}
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;
std::cout << "Press Enter to quit..." << std::endl;
std::cin.get();
}
#ifdef VISP_HAVE_DISPLAY
delete plot;
}
#endif
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
Class that approximates a 4-wheel robot using a bicycle model.
Implementation of column vector and the associated operations.
static const vpColor blue
static const vpColor black
Class for generating random number with normal probability density.
Class that permits to convert the position + heading of the 4-wheel robot into measurements from a la...
Class that represent a grid of landmarks that measure the distance and relative orientation of the 4-...
double likelihood(const vpColVector &particle, const vpColVector &meas)
Compute the likelihood of a particle (value between 0. and 1.) knowing the measurements....
static double rad(double deg)
static float modulo(const float &value, const float &modulo)
Gives the rest of value divided by modulo when the quotient can only be an integer.
The class permits to use a Particle Filter.
std::function< vpParticlesWithWeights(const std::vector< vpColVector > &, const std::vector< double > &)> vpResamplingFunction
Function that takes as argument the vector of particles and the vector of associated weights....
std::function< vpColVector(const std::vector< vpColVector > &, const std::vector< double > &, const vpStateAddFunction &)> vpFilterFunction
Filter function, which computes the filtered state of the particle filter. The first argument is the ...
std::function< vpColVector(const vpColVector &, const double &)> vpProcessFunction
Process model function, which projects a particle forward in time. The first argument is a particle,...
std::function< bool(const unsigned int &, const std::vector< double > &)> vpResamplingConditionFunction
Function that takes as argument the number of particles and the vector of weights associated to each ...
std::function< double(const vpColVector &, const MeasurementsType &)> vpLikelihoodFunction
Likelihood function, which evaluates the likelihood of a particle with regard to the measurements....
std::function< vpColVector(const vpColVector &, const vpColVector &)> vpStateAddFunction
Function that computes either the equivalent of an addition in the state space. The first argument is...
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
void initGraph(unsigned int graphNum, unsigned int curveNbr)
void setUnitY(unsigned int graphNum, const std::string &unity)
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
void setUnitX(unsigned int graphNum, const std::string &unitx)
void setColor(unsigned int graphNum, unsigned int curveNum, vpColor color)
void setTitle(unsigned int graphNum, const std::string &title)
As the state model {x, y, } does not contain any velocity information, it does not evolve without com...
vpColVector processFunction(const vpColVector &x, const double &dt)
Models the effect of the command on the state model.
VISP_EXPORT double measureTimeMicros()
bool m_useDisplay
If true, activate the plot and the renderer if VISP_HAVE_DISPLAY is defined.
int m_nbThreads
Number of thread to use in the Particle Filter.
double m_ampliMaxTheta
Amplitude max of the noise for the state component corresponding to the heading.
static const int SOFTWARE_CONTINUE
double m_ampliMaxY
Amplitude max of the noise for the state component corresponding to the Y coordinate.
unsigned int m_N
The number of particles.
double m_maxDistanceForLikelihood
The maximum allowed distance between a particle and the measurement, leading to a likelihood equal to...
int parseArgs(const int argc, const char *argv[])
unsigned int m_nbStepsWarmUp
Number of steps for the warmup phase.
double m_ampliMaxX
Amplitude max of the noise for the state component corresponding to the X coordinate.
long m_seedPF
Seed for the random generators of the PF.
![\[ \begin{array}{lcl} \textbf{x}[0] &=& x \\ \textbf{x}[1] &=& y \\ \textbf{x}[2] &=& \theta \end{array} \]](form_1706.png)
is the heading of the robot.
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: ![\[ \begin{array}{lcl} \textbf{z}[2i] &=& \sqrt{(p_x^i - x)^2 + (p_y^i - y)^2} \\ \textbf{z}[2i+1] &=& \tan^{-1}{\frac{p_y^i - y}{p_x^i - x}} - \theta \end{array} \]](form_1709.png)
is the weight associated to the 
1.9.1